mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-24 01:33:46 +08:00
New Lateral Changes
This commit is contained in:
firestar5683
@@ -532,6 +532,7 @@ struct CarParams {
|
||||
useSteeringAngle @0 :Bool;
|
||||
kp @1 :Float32;
|
||||
ki @2 :Float32;
|
||||
kd @8 :Float32;
|
||||
friction @3 :Float32;
|
||||
kf @4 :Float32;
|
||||
steeringAngleDeadzoneDeg @5 :Float32;
|
||||
|
||||
@@ -5262,17 +5262,17 @@ const ::capnp::_::RawSchema s_9622723fcbd14c2e = {
|
||||
0, 5, i_9622723fcbd14c2e, nullptr, nullptr, { &s_9622723fcbd14c2e, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = {
|
||||
static const ::capnp::_::AlignedData<162> b_80366e0e804ecc1d = {
|
||||
{ 0, 0, 0, 0, 5, 0, 6, 0,
|
||||
29, 204, 78, 128, 14, 110, 54, 128,
|
||||
20, 0, 0, 0, 1, 0, 4, 0,
|
||||
20, 0, 0, 0, 1, 0, 5, 0,
|
||||
218, 169, 170, 144, 36, 55, 105, 140,
|
||||
0, 0, 7, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
21, 0, 0, 0, 66, 1, 0, 0,
|
||||
37, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
33, 0, 0, 0, 199, 1, 0, 0,
|
||||
33, 0, 0, 0, 255, 1, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
99, 97, 114, 46, 99, 97, 112, 110,
|
||||
@@ -5281,63 +5281,70 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = {
|
||||
114, 97, 108, 84, 111, 114, 113, 117,
|
||||
101, 84, 117, 110, 105, 110, 103, 0,
|
||||
0, 0, 0, 0, 1, 0, 1, 0,
|
||||
32, 0, 0, 0, 3, 0, 4, 0,
|
||||
36, 0, 0, 0, 3, 0, 4, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 1, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
209, 0, 0, 0, 138, 0, 0, 0,
|
||||
237, 0, 0, 0, 138, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
212, 0, 0, 0, 3, 0, 1, 0,
|
||||
224, 0, 0, 0, 2, 0, 1, 0,
|
||||
240, 0, 0, 0, 3, 0, 1, 0,
|
||||
252, 0, 0, 0, 2, 0, 1, 0,
|
||||
1, 0, 0, 0, 1, 0, 0, 0,
|
||||
0, 0, 1, 0, 1, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
221, 0, 0, 0, 26, 0, 0, 0,
|
||||
249, 0, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
216, 0, 0, 0, 3, 0, 1, 0,
|
||||
228, 0, 0, 0, 2, 0, 1, 0,
|
||||
244, 0, 0, 0, 3, 0, 1, 0,
|
||||
0, 1, 0, 0, 2, 0, 1, 0,
|
||||
2, 0, 0, 0, 2, 0, 0, 0,
|
||||
0, 0, 1, 0, 2, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
225, 0, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
220, 0, 0, 0, 3, 0, 1, 0,
|
||||
232, 0, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
229, 0, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
228, 0, 0, 0, 3, 0, 1, 0,
|
||||
240, 0, 0, 0, 2, 0, 1, 0,
|
||||
4, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
237, 0, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
232, 0, 0, 0, 3, 0, 1, 0,
|
||||
244, 0, 0, 0, 2, 0, 1, 0,
|
||||
5, 0, 0, 0, 5, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
241, 0, 0, 0, 202, 0, 0, 0,
|
||||
253, 0, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
248, 0, 0, 0, 3, 0, 1, 0,
|
||||
4, 1, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 6, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
4, 0, 0, 0, 3, 0, 0, 0,
|
||||
0, 0, 1, 0, 3, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
1, 1, 0, 0, 122, 0, 0, 0,
|
||||
1, 1, 0, 0, 74, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 1, 0, 0, 3, 0, 1, 0,
|
||||
12, 1, 0, 0, 2, 0, 1, 0,
|
||||
7, 0, 0, 0, 7, 0, 0, 0,
|
||||
5, 0, 0, 0, 4, 0, 0, 0,
|
||||
0, 0, 1, 0, 4, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 1, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
4, 1, 0, 0, 3, 0, 1, 0,
|
||||
16, 1, 0, 0, 2, 0, 1, 0,
|
||||
6, 0, 0, 0, 5, 0, 0, 0,
|
||||
0, 0, 1, 0, 5, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
13, 1, 0, 0, 202, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
20, 1, 0, 0, 3, 0, 1, 0,
|
||||
32, 1, 0, 0, 2, 0, 1, 0,
|
||||
7, 0, 0, 0, 6, 0, 0, 0,
|
||||
0, 0, 1, 0, 6, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
29, 1, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
28, 1, 0, 0, 3, 0, 1, 0,
|
||||
40, 1, 0, 0, 2, 0, 1, 0,
|
||||
8, 0, 0, 0, 7, 0, 0, 0,
|
||||
0, 0, 1, 0, 7, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
9, 1, 0, 0, 122, 0, 0, 0,
|
||||
37, 1, 0, 0, 122, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
8, 1, 0, 0, 3, 0, 1, 0,
|
||||
20, 1, 0, 0, 2, 0, 1, 0,
|
||||
36, 1, 0, 0, 3, 0, 1, 0,
|
||||
48, 1, 0, 0, 2, 0, 1, 0,
|
||||
3, 0, 0, 0, 8, 0, 0, 0,
|
||||
0, 0, 1, 0, 8, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
45, 1, 0, 0, 26, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
40, 1, 0, 0, 3, 0, 1, 0,
|
||||
52, 1, 0, 0, 2, 0, 1, 0,
|
||||
117, 115, 101, 83, 116, 101, 101, 114,
|
||||
105, 110, 103, 65, 110, 103, 108, 101,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -5403,6 +5410,14 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = {
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
108, 97, 116, 65, 99, 99, 101, 108,
|
||||
79, 102, 102, 115, 101, 116, 0, 0,
|
||||
10, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
10, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
107, 100, 0, 0, 0, 0, 0, 0,
|
||||
10, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0, 0, 0,
|
||||
@@ -5413,11 +5428,11 @@ static const ::capnp::_::AlignedData<147> b_80366e0e804ecc1d = {
|
||||
};
|
||||
::capnp::word const* const bp_80366e0e804ecc1d = b_80366e0e804ecc1d.words;
|
||||
#if !CAPNP_LITE
|
||||
static const uint16_t m_80366e0e804ecc1d[] = {3, 4, 2, 1, 6, 7, 5, 0};
|
||||
static const uint16_t i_80366e0e804ecc1d[] = {0, 1, 2, 3, 4, 5, 6, 7};
|
||||
static const uint16_t m_80366e0e804ecc1d[] = {3, 8, 4, 2, 1, 6, 7, 5, 0};
|
||||
static const uint16_t i_80366e0e804ecc1d[] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
|
||||
const ::capnp::_::RawSchema s_80366e0e804ecc1d = {
|
||||
0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 147, nullptr, m_80366e0e804ecc1d,
|
||||
0, 8, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
0x80366e0e804ecc1d, b_80366e0e804ecc1d.words, 162, nullptr, m_80366e0e804ecc1d,
|
||||
0, 9, i_80366e0e804ecc1d, nullptr, nullptr, { &s_80366e0e804ecc1d, nullptr, nullptr, 0, 0, nullptr }, false
|
||||
};
|
||||
#endif // !CAPNP_LITE
|
||||
static const ::capnp::_::AlignedData<151> b_c342cefc303e9b8e = {
|
||||
|
||||
@@ -626,7 +626,7 @@ struct CarParams::LateralTorqueTuning {
|
||||
class Pipeline;
|
||||
|
||||
struct _capnpPrivate {
|
||||
CAPNP_DECLARE_STRUCT_HEADER(80366e0e804ecc1d, 4, 0)
|
||||
CAPNP_DECLARE_STRUCT_HEADER(80366e0e804ecc1d, 5, 0)
|
||||
#if !CAPNP_LITE
|
||||
static constexpr ::capnp::_::RawBrandedSchema const* brand() { return &schema->defaultBrand; }
|
||||
#endif // !CAPNP_LITE
|
||||
@@ -3148,6 +3148,8 @@ public:
|
||||
|
||||
inline float getLatAccelOffset() const;
|
||||
|
||||
inline float getKd() const;
|
||||
|
||||
private:
|
||||
::capnp::_::StructReader _reader;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -3200,6 +3202,9 @@ public:
|
||||
inline float getLatAccelOffset();
|
||||
inline void setLatAccelOffset(float value);
|
||||
|
||||
inline float getKd();
|
||||
inline void setKd(float value);
|
||||
|
||||
private:
|
||||
::capnp::_::StructBuilder _builder;
|
||||
template <typename, ::capnp::Kind>
|
||||
@@ -7847,6 +7852,20 @@ inline void CarParams::LateralTorqueTuning::Builder::setLatAccelOffset(float val
|
||||
::capnp::bounded<7>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline float CarParams::LateralTorqueTuning::Reader::getKd() const {
|
||||
return _reader.getDataField<float>(
|
||||
::capnp::bounded<8>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
|
||||
inline float CarParams::LateralTorqueTuning::Builder::getKd() {
|
||||
return _builder.getDataField<float>(
|
||||
::capnp::bounded<8>() * ::capnp::ELEMENTS);
|
||||
}
|
||||
inline void CarParams::LateralTorqueTuning::Builder::setKd(float value) {
|
||||
_builder.setDataField<float>(
|
||||
::capnp::bounded<8>() * ::capnp::ELEMENTS, value);
|
||||
}
|
||||
|
||||
inline bool CarParams::LongitudinalPIDTuning::Reader::hasKpBP() const {
|
||||
return !_reader.getPointerField(
|
||||
::capnp::bounded<0>() * ::capnp::POINTERS).isNull();
|
||||
|
||||
Binary file not shown.
@@ -2,7 +2,6 @@
|
||||
# Twilsonco's Lateral Neural Network Feedforward
|
||||
from collections import deque
|
||||
from difflib import SequenceMatcher
|
||||
from typing import NamedTuple
|
||||
|
||||
import json
|
||||
import math
|
||||
@@ -30,10 +29,9 @@ from openpilot.frogpilot.common.frogpilot_variables import NNFF_MODELS_PATH, get
|
||||
# dict used to rename activation functions whose names aren't valid python identifiers
|
||||
ACTIVATION_FUNCTION_NAMES = {'σ': 'sigmoid'}
|
||||
|
||||
LOW_SPEED_X = [0, 10, 20, 30]
|
||||
LOW_SPEED_Y = [15, 13, 10, 5]
|
||||
LOW_SPEED_Y_NN = [12, 3, 1, 0]
|
||||
|
||||
|
||||
LAT_PLAN_MIN_IDX = 5
|
||||
|
||||
class FluxModel:
|
||||
@@ -42,7 +40,6 @@ class FluxModel:
|
||||
params = json.load(f)
|
||||
|
||||
self.input_size = params["input_size"]
|
||||
self.output_size = params["output_size"]
|
||||
|
||||
self.input_mean = np.array(params["input_mean"], dtype=np.float32).T
|
||||
self.input_std = np.array(params["input_std"], dtype=np.float32).T
|
||||
@@ -152,11 +149,6 @@ def sign(x):
|
||||
def similarity(s1: str, s2: str) -> float:
|
||||
return SequenceMatcher(None, s1, s2).ratio()
|
||||
|
||||
class LatControlInputs(NamedTuple):
|
||||
lateral_acceleration: float
|
||||
roll_compensation: float
|
||||
vego: float
|
||||
aego: float
|
||||
|
||||
class NeuralNetworkFeedforward:
|
||||
def __init__(self, CP, LatControlTorque):
|
||||
@@ -212,7 +204,7 @@ class NeuralNetworkFeedforward:
|
||||
self.nn_future_times = [time + self.lateral_delay for time in self.future_times]
|
||||
self.past_future_len = len(self.past_times) + len(self.nn_future_times)
|
||||
|
||||
def compute_nnff(self, CS, VM, actual_lateral_accel, desired_lateral_accel, gravity_adjusted_lateral_accel, lateral_accel_deadzone, llk, measurement, model_data, params, pid_log, roll_compensation, setpoint, frogpilot_toggles):
|
||||
def compute_nnff(self, CS, VM, actual_lateral_accel, future_desired_lateral_accel, error, gravity_adjusted_future_lateral_accel, llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles):
|
||||
if self.use_steering_angle:
|
||||
actual_curvature_rate = -VM.calc_curvature(math.radians(CS.steeringRateDeg), CS.vEgo, 0.0)
|
||||
actual_lateral_jerk = actual_curvature_rate * CS.vEgo ** 2
|
||||
@@ -227,7 +219,7 @@ class NeuralNetworkFeedforward:
|
||||
friction_upper_idx = next((idxs for idxs, value in enumerate(ModelConstants.T_IDXS) if value > lookahead), 16)
|
||||
|
||||
predicted_lateral_jerk = get_predicted_lateral_jerk(model_data.acceleration.y, self.t_diffs)
|
||||
desired_lateral_jerk = (interp(self.lateral_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - desired_lateral_accel) / self.lateral_delay
|
||||
desired_lateral_jerk = (interp(self.lateral_delay, ModelConstants.T_IDXS, model_data.acceleration.y) - future_desired_lateral_accel) / self.lateral_delay
|
||||
|
||||
lookahead_lateral_jerk = get_lookahead_value(predicted_lateral_jerk[LAT_PLAN_MIN_IDX:friction_upper_idx], desired_lateral_jerk)
|
||||
|
||||
@@ -252,7 +244,7 @@ class NeuralNetworkFeedforward:
|
||||
roll = roll_pitch_adjust(roll, pitch)
|
||||
|
||||
self.roll_deque.append(roll)
|
||||
self.lateral_accel_desired_deque.append(desired_lateral_accel)
|
||||
self.lateral_accel_desired_deque.append(future_desired_lateral_accel)
|
||||
|
||||
# prepare past and future values
|
||||
# adjust future times to account for longitudinal acceleration
|
||||
@@ -275,16 +267,15 @@ class NeuralNetworkFeedforward:
|
||||
|
||||
pid_log.error = torque_from_setpoint - torque_from_measurement
|
||||
|
||||
error_blend = interp(abs(desired_lateral_accel), [1.0, 2.0], [0.0, 1.0])
|
||||
error_blend = interp(abs(future_desired_lateral_accel), [1.0, 2.0], [0.0, 1.0])
|
||||
if error_blend > 0.0: # blend in stronger error response when in high lat accel
|
||||
torque_from_error = self.lat_torque_nn_model.evaluate([CS.vEgo, setpoint - measurement, lateral_jerk_setpoint - lateral_jerk_measurement, 0.0])
|
||||
if sign(pid_log.error) == sign(torque_from_error) and abs(pid_log.error) < abs(torque_from_error):
|
||||
pid_log.error = pid_log.error * (1.0 - error_blend) + torque_from_error * error_blend
|
||||
|
||||
# compute feedforward (same as nn setpoint output)
|
||||
error = setpoint - measurement
|
||||
friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
|
||||
nn_input = [CS.vEgo, desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common
|
||||
nn_input = [CS.vEgo, future_desired_lateral_accel, friction_input, roll] + past_lateral_accels_desired + future_lateral_accels + nnff_common
|
||||
ff = self.lat_torque_nn_model.evaluate(nn_input)
|
||||
|
||||
# apply friction override for cars with low NN friction response
|
||||
@@ -296,8 +287,7 @@ class NeuralNetworkFeedforward:
|
||||
|
||||
pid_log.error = float(torque_from_setpoint - torque_from_measurement)
|
||||
|
||||
error = desired_lateral_accel - actual_lateral_accel
|
||||
friction_input = self.lat_accel_friction_factor * error + self.lat_jerk_friction_factor * lookahead_lateral_jerk
|
||||
ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.lat_control_torque.torque_params)
|
||||
ff = self.torque_from_lateral_accel(gravity_adjusted_future_lateral_accel, self.lat_control_torque.torque_params)
|
||||
|
||||
return pid_log, ff
|
||||
|
||||
@@ -97,6 +97,19 @@ def get_city_center(latitude, longitude):
|
||||
print(f"Falling back to (0, 0) for {latitude}, {longitude}")
|
||||
return float(0.0), float(0.0), "N/A", "N/A", "N/A"
|
||||
|
||||
def update_branch_commits(now):
|
||||
points = []
|
||||
for branch in ["FrogPilot", "FrogPilot-Staging", "FrogPilot-Testing"]:
|
||||
try:
|
||||
response = requests.get(f"https://api.github.com/repos/FrogAi/FrogPilot/commits/{branch}")
|
||||
response.raise_for_status()
|
||||
sha = response.json()["sha"]
|
||||
points.append(Point("branch_commits").field("commit", sha).tag("branch", branch).time(now))
|
||||
except Exception as e:
|
||||
print(f"Failed to fetch commit for {branch}: {e}")
|
||||
|
||||
return points
|
||||
|
||||
def is_up_to_date(build_metadata):
|
||||
remote_commit = run_cmd(["git", "ls-remote", "origin", build_metadata.channel], f"Fetched remote commit", "Failed to fetch remote commit", report=False)
|
||||
|
||||
@@ -144,7 +157,10 @@ def send_stats():
|
||||
|
||||
selected_theme = random.choice([item for item, count in most_common if count == max_count]).replace("-user_created", "").replace("_", " ")
|
||||
|
||||
point = (Point("user_stats")
|
||||
now = datetime.now(timezone.utc)
|
||||
|
||||
user_point = (
|
||||
Point("user_stats")
|
||||
.field("blocked_user", frogpilot_toggles.block_user)
|
||||
.field("car_make", "GM" if frogpilot_toggles.car_make == "gm" else frogpilot_toggles.car_make.title())
|
||||
.field("car_model", frogpilot_toggles.car_model)
|
||||
@@ -180,10 +196,13 @@ def send_stats():
|
||||
.tag("branch", build_metadata.channel)
|
||||
.tag("dongle_id", params.get("FrogPilotDongleId", encoding="utf-8"))
|
||||
|
||||
.time(datetime.now(timezone.utc))
|
||||
.time(now)
|
||||
)
|
||||
|
||||
InfluxDBClient(org=org_ID, token=token, url=url).write_api(write_options=SYNCHRONOUS).write(bucket=bucket, org=org_ID, record=point)
|
||||
all_points = [user_point] + update_branch_commits(now)
|
||||
|
||||
client = InfluxDBClient(org=org_ID, token=token, url=url)
|
||||
client.write_api(write_options=SYNCHRONOUS).write(bucket=bucket, org=org_ID, record=all_points)
|
||||
print("Successfully sent FrogPilot stats!")
|
||||
except Exception as exception:
|
||||
print(f"Failed to send FrogPilot stats: {exception}")
|
||||
|
||||
@@ -305,6 +305,7 @@ class CarInterfaceBase(ABC):
|
||||
tune.torque.kf = 1.0
|
||||
tune.torque.kp = 1.0
|
||||
tune.torque.ki = 0.3
|
||||
tune.torque.kd = 0.0
|
||||
tune.torque.friction = params['FRICTION']
|
||||
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
|
||||
tune.torque.latAccelOffset = 0.0
|
||||
|
||||
@@ -29,6 +29,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, S
|
||||
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
|
||||
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
from openpilot.frogpilot.tinygrad_modeld.tinygrad_modeld import LAT_SMOOTH_SECONDS
|
||||
|
||||
from openpilot.system.hardware import HARDWARE
|
||||
|
||||
@@ -135,11 +136,11 @@ class Controls:
|
||||
|
||||
self.LaC: LatControl
|
||||
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
|
||||
self.LaC = LatControlAngle(self.CP, self.CI)
|
||||
self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
|
||||
elif self.FPCP.lateralTuning.which() == 'pid':
|
||||
self.LaC = LatControlPID(self.CP, self.CI)
|
||||
self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
|
||||
elif self.FPCP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.FPCP, self.CI)
|
||||
self.LaC = LatControlTorque(self.CP, self.FPCP, self.CI, DT_CTRL)
|
||||
|
||||
self.initialized = False
|
||||
self.state = State.disabled
|
||||
@@ -671,11 +672,12 @@ class Controls:
|
||||
# Reset desired curvature to current to avoid violating the limits on engage
|
||||
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
|
||||
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
|
||||
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
|
||||
|
||||
actuators.curvature = self.desired_curvature
|
||||
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
|
||||
self.steer_limited_by_safety, self.desired_curvature,
|
||||
curvature_limited,
|
||||
curvature_limited, lat_delay,
|
||||
self.sm['liveLocationKalman'],
|
||||
self.sm['modelV2'],
|
||||
self.frogpilot_toggles)
|
||||
|
||||
@@ -1,33 +1,31 @@
|
||||
import numpy as np
|
||||
from abc import abstractmethod, ABC
|
||||
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
|
||||
MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s
|
||||
|
||||
|
||||
class LatControl(ABC):
|
||||
def __init__(self, CP, CI):
|
||||
self.sat_count_rate = 1.0 * DT_CTRL
|
||||
def __init__(self, CP, CI, dt):
|
||||
self.dt = dt
|
||||
self.sat_limit = CP.steerLimitTimer
|
||||
self.sat_count = 0.
|
||||
self.sat_time = 0.
|
||||
self.sat_check_min_speed = 10.
|
||||
|
||||
# we define the steer torque scale as [-1.0...1.0]
|
||||
self.steer_max = 1.0
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles):
|
||||
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float, llk, model_data, frogpilot_toggles):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
self.sat_count = 0.
|
||||
self.sat_time = 0.
|
||||
|
||||
def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
|
||||
# Saturated only if control output is not being limited by car torque/angle rate limits
|
||||
if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
|
||||
self.sat_count += self.sat_count_rate
|
||||
self.sat_time += self.dt
|
||||
else:
|
||||
self.sat_count -= self.sat_count_rate
|
||||
self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
|
||||
return self.sat_count > (self.sat_limit - 1e-3)
|
||||
self.sat_time -= self.dt
|
||||
self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
|
||||
return self.sat_time > (self.sat_limit - 1e-3)
|
||||
|
||||
@@ -8,12 +8,12 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, CI, dt):
|
||||
super().__init__(CP, CI, dt)
|
||||
self.sat_check_min_speed = 5.
|
||||
self.use_steer_limited_by_safety = CP.carName == "tesla"
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if not active:
|
||||
|
||||
@@ -6,14 +6,14 @@ from openpilot.selfdrive.controls.lib.pid import PIDController
|
||||
|
||||
|
||||
class LatControlPID(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, CI, dt):
|
||||
super().__init__(CP, CI, dt)
|
||||
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
|
||||
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
|
||||
k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
|
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function()
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
|
||||
from cereal import log
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import get_friction
|
||||
from openpilot.common.filter_simple import FirstOrderFilter
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED, get_friction
|
||||
from openpilot.selfdrive.car.interfaces import FRICTION_THRESHOLD
|
||||
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
|
||||
from openpilot.selfdrive.controls.lib.pid import PIDController
|
||||
@@ -21,20 +23,26 @@ from openpilot.frogpilot.controls.lib.neural_network_feedforward import LOW_SPEE
|
||||
# friction in the steering wheel that needs to be overcome to
|
||||
# move it at all, this is compensated for too.
|
||||
|
||||
MAX_LAT_JERK_UP = 2.5 # m/s^3
|
||||
|
||||
LOW_SPEED_X = [0, 10, 20, 30]
|
||||
LOW_SPEED_Y = [15, 13, 10, 5]
|
||||
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
def __init__(self, CP, FPCP, CI):
|
||||
super().__init__(CP, CI)
|
||||
def __init__(self, CP, FPCP, CI, dt):
|
||||
super().__init__(CP, CI, dt)
|
||||
self.torque_params = FPCP.lateralTuning.torque
|
||||
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
|
||||
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
|
||||
self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
|
||||
k_f=self.torque_params.kf)
|
||||
k_f=self.torque_params.kf, rate=1/self.dt)
|
||||
self.update_limits()
|
||||
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
|
||||
self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
|
||||
self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)
|
||||
self.previous_measurement = 0.0
|
||||
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
|
||||
|
||||
# FrogPilot variables
|
||||
self.nnff = NeuralNetworkFeedforward(CP, self)
|
||||
@@ -51,29 +59,38 @@ class LatControlTorque(LatControl):
|
||||
self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
|
||||
self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
|
||||
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, llk, model_data, frogpilot_toggles):
|
||||
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay, llk, model_data, frogpilot_toggles):
|
||||
pid_log = log.ControlsState.LateralTorqueState.new_message()
|
||||
if not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
else:
|
||||
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
|
||||
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
|
||||
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
|
||||
|
||||
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y)**2
|
||||
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
|
||||
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
|
||||
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
|
||||
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES))
|
||||
expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
|
||||
# TODO factor out lateral jerk from error to later replace it with delay independent alternative
|
||||
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
|
||||
self.requested_lateral_accel_buffer.append(future_desired_lateral_accel)
|
||||
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
|
||||
desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
|
||||
|
||||
measurement = measured_curvature * CS.vEgo ** 2
|
||||
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
|
||||
self.previous_measurement = measurement
|
||||
|
||||
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y_NN if frogpilot_toggles.nnff else LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
|
||||
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
|
||||
error = setpoint - measurement
|
||||
error_lsf = error + low_speed_factor * error
|
||||
|
||||
if self.nnff_loaded and frogpilot_toggles.nnff or frogpilot_toggles.nnff_lite:
|
||||
pid_log, ff = self.nnff.compute_nnff(
|
||||
CS, VM, actual_lateral_accel, desired_lateral_accel, gravity_adjusted_lateral_accel, lateral_accel_deadzone,
|
||||
llk, measurement, model_data, params, pid_log, roll_compensation, setpoint, frogpilot_toggles
|
||||
CS, VM, measurement, error, future_desired_lateral_accel, gravity_adjusted_future_lateral_accel,
|
||||
llk, measurement, model_data, params, pid_log, setpoint, frogpilot_toggles
|
||||
)
|
||||
|
||||
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
|
||||
@@ -83,17 +100,19 @@ class LatControlTorque(LatControl):
|
||||
freeze_integrator=freeze_integrator)
|
||||
else:
|
||||
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
|
||||
pid_log.error = float(setpoint - measurement)
|
||||
ff = gravity_adjusted_lateral_accel
|
||||
pid_log.error = float(error_lsf)
|
||||
ff = gravity_adjusted_future_lateral_accel
|
||||
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
|
||||
ff -= self.torque_params.latAccelOffset
|
||||
ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
|
||||
# TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
|
||||
ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
|
||||
|
||||
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
|
||||
output_lataccel = self.pid.update(pid_log.error,
|
||||
feedforward=ff,
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=freeze_integrator)
|
||||
-measurement_rate,
|
||||
feedforward=ff,
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=freeze_integrator)
|
||||
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
|
||||
|
||||
pid_log.active = True
|
||||
@@ -102,8 +121,8 @@ class LatControlTorque(LatControl):
|
||||
pid_log.d = float(self.pid.d)
|
||||
pid_log.f = float(self.pid.f)
|
||||
pid_log.output = float(-output_torque) # TODO: log lat accel?
|
||||
pid_log.actualLateralAccel = float(actual_lateral_accel)
|
||||
pid_log.desiredLateralAccel = float(desired_lateral_accel)
|
||||
pid_log.actualLateralAccel = float(measurement)
|
||||
pid_log.desiredLateralAccel = float(setpoint)
|
||||
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
|
||||
|
||||
# TODO left is positive in this convention
|
||||
|
||||
@@ -26,7 +26,7 @@ class PIDController:
|
||||
self.neg_p_limit = neg_p_limit
|
||||
|
||||
self.i_unwind_rate = 0.3 / rate
|
||||
self.i_rate = 1.0 / rate
|
||||
self.i_dt = 1.0 / rate
|
||||
self.speed = 0.0
|
||||
|
||||
self.reset()
|
||||
@@ -61,19 +61,19 @@ class PIDController:
|
||||
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
|
||||
self.p = float(error) * self.k_p
|
||||
self.p = self.k_p * float(error)
|
||||
if self.pos_p_limit is not None and self.p > self.pos_p_limit:
|
||||
self.p = self.pos_p_limit
|
||||
elif self.neg_p_limit is not None and self.p < self.neg_p_limit:
|
||||
self.p = self.neg_p_limit
|
||||
self.f = feedforward * self.k_f
|
||||
self.d = error_rate * self.k_d
|
||||
self.d = self.k_d * error_rate
|
||||
self.f = self.k_f * feedforward
|
||||
|
||||
if override:
|
||||
self.i -= self.i_unwind_rate * float(np.sign(self.i))
|
||||
else:
|
||||
if not freeze_integrator:
|
||||
self.i = self.i + error * self.k_i * self.i_rate
|
||||
self.i = self.i + self.k_i * self.i_dt * error
|
||||
|
||||
# Clip i to prevent exceeding control limits
|
||||
control_no_i = self.p + self.d + self.f
|
||||
|
||||
@@ -16,6 +16,7 @@ from openpilot.common.swaglog import cloudlog
|
||||
from openpilot.common.simple_kalman import KF1D
|
||||
|
||||
from openpilot.frogpilot.common.frogpilot_variables import THRESHOLD, get_frogpilot_toggles
|
||||
from openpilot.selfdrive.controls.controlsd import LaneChangeDirection, LaneChangeState
|
||||
|
||||
# Default lead acceleration decay set to 50% at 1s
|
||||
_LEAD_ACCEL_TAU = 0.6
|
||||
@@ -149,7 +150,15 @@ def laplacian_pdf(x: float, mu: float, b: float):
|
||||
return math.exp(-abs(x-mu)/b)
|
||||
|
||||
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks: dict[int, Track]):
|
||||
def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], frogpilot_toggles: SimpleNamespace):
|
||||
if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and frogpilot_toggles.human_lane_changes:
|
||||
direction = model_data.meta.laneChangeDirection
|
||||
|
||||
if direction == LaneChangeDirection.left:
|
||||
tracks = {k: v for k, v in tracks.items() if v.yRel > 0}
|
||||
elif direction == LaneChangeDirection.right:
|
||||
tracks = {k: v for k, v in tracks.items() if v.yRel < 0}
|
||||
|
||||
offset_vision_dist = lead.x[0] - RADAR_TO_CAMERA
|
||||
|
||||
def prob(c):
|
||||
@@ -194,11 +203,11 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa
|
||||
|
||||
def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader,
|
||||
model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool,
|
||||
frogpilot_toggles: SimpleNamespace, frogpilotPlan: capnp._DynamicStructReader,
|
||||
frogpilotPlan: capnp._DynamicStructReader, frogpilot_toggles: SimpleNamespace,
|
||||
low_speed_override: bool = True) -> dict[str, Any]:
|
||||
# Determine leads, this is where the essential logic happens
|
||||
if len(tracks) > 0 and ready and lead_msg.prob > frogpilot_toggles.lead_detection_probability:
|
||||
track = match_vision_to_track(v_ego, lead_msg, tracks)
|
||||
track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, frogpilot_toggles)
|
||||
else:
|
||||
track = None
|
||||
|
||||
@@ -315,8 +324,8 @@ class RadarD:
|
||||
model_v_ego = self.v_ego
|
||||
leads_v3 = sm['modelV2'].leadsV3
|
||||
if len(leads_v3) > 1:
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotPlan'], low_speed_override=True)
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['carState'].standstill, self.frogpilot_toggles, sm['frogpilotPlan'], low_speed_override=False)
|
||||
self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=True)
|
||||
self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], sm['carState'].standstill, sm['frogpilotPlan'], self.frogpilot_toggles, low_speed_override=False)
|
||||
|
||||
if self.frogpilot_toggles.adjacent_lead_tracking and self.ready:
|
||||
self.frogpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True)
|
||||
|
||||
Reference in New Issue
Block a user