diff --git a/cereal/car.capnp b/cereal/car.capnp index f0522253e..056cbf8b0 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -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; diff --git a/cereal/gen/cpp/car.capnp.c++ b/cereal/gen/cpp/car.capnp.c++ index 5ad5d4652..0e1555736 100644 --- a/cereal/gen/cpp/car.capnp.c++ +++ b/cereal/gen/cpp/car.capnp.c++ @@ -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 = { diff --git a/cereal/gen/cpp/car.capnp.h b/cereal/gen/cpp/car.capnp.h index 8fa546740..dbd41180a 100644 --- a/cereal/gen/cpp/car.capnp.h +++ b/cereal/gen/cpp/car.capnp.h @@ -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 @@ -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 @@ -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( + ::capnp::bounded<8>() * ::capnp::ELEMENTS); +} + +inline float CarParams::LateralTorqueTuning::Builder::getKd() { + return _builder.getDataField( + ::capnp::bounded<8>() * ::capnp::ELEMENTS); +} +inline void CarParams::LateralTorqueTuning::Builder::setKd(float value) { + _builder.setDataField( + ::capnp::bounded<8>() * ::capnp::ELEMENTS, value); +} + inline bool CarParams::LongitudinalPIDTuning::Reader::hasKpBP() const { return !_reader.getPointerField( ::capnp::bounded<0>() * ::capnp::POINTERS).isNull(); diff --git a/cereal/libcereal_shared.so b/cereal/libcereal_shared.so index bf57e0cd3..52e9984af 100755 Binary files a/cereal/libcereal_shared.so and b/cereal/libcereal_shared.so differ diff --git a/frogpilot/controls/lib/neural_network_feedforward.py b/frogpilot/controls/lib/neural_network_feedforward.py index fac09d9fd..d2dcf99ed 100644 --- a/frogpilot/controls/lib/neural_network_feedforward.py +++ b/frogpilot/controls/lib/neural_network_feedforward.py @@ -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 diff --git a/frogpilot/system/frogpilot_stats.py b/frogpilot/system/frogpilot_stats.py index a3f4dc963..977dfb961 100644 --- a/frogpilot/system/frogpilot_stats.py +++ b/frogpilot/system/frogpilot_stats.py @@ -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}") diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 7f925f8e6..ccb7b810c 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c19733b87..0911de46c 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 93f59f276..b4ef85fe7 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -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) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 01efe0868..d2cd07a44 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -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: diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index ed3a62f03..73ed3b17b 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index dbc8d638f..35bf53480 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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 diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 44accf4e2..a076ea2e0 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -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 diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 30b398e7a..3addaba86 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -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)