diff --git a/common/params.cc b/common/params.cc index 295d1267d..440ce49cb 100644 --- a/common/params.cc +++ b/common/params.cc @@ -570,6 +570,9 @@ std::unordered_map keys = { {"RadarInterceptorEnabled", PERSISTENT}, {"NoMRCC", PERSISTENT}, {"NoFSC", PERSISTENT}, + {"ExperimentalLongTune", PERSISTENT}, + {"LongOutputGain", PERSISTENT}, + {"BlendedACC", PERSISTENT}, }; } // namespace diff --git a/opendbc/mazda_2019.dbc b/opendbc/mazda_2019.dbc index a7ab005f2..447fc7b07 100644 --- a/opendbc/mazda_2019.dbc +++ b/opendbc/mazda_2019.dbc @@ -433,7 +433,7 @@ BO_ 512 NEW_MSG_32: 8 XXX SG_ NEW_SIGNAL_3 : 39|16@0+ (1,0) [0|65535] "" XXX BO_ 514 ENGINE_DATA: 8 XXX - SG_ RPM : 7|13@0+ (1,0) [0|8191] "" XXX + SG_ RPM : 7|14@0+ (1,0) [0|8191] "" XXX SG_ NEW_SIGNAL_1 : 19|12@0+ (1,0) [0|255] "" XXX SG_ PEDAL_GAS : 39|10@0+ (1,0) [0|65535] "" XXX SG_ ENGINE_ON : 52|1@0+ (1,0) [0|1] "" XXX @@ -497,8 +497,12 @@ BO_ 546 ACC_2: 8 XXX BO_ 552 GEAR: 8 XXX SG_ NEW_SIGNAL_1 : 0|1@0+ (1,0) [0|1] "" XXX + SG_ SHIFT : 8|1@0+ (1,0) [0|1] "" XXX + SG_ TORQUE_CONVERTER_LOCK : 11|1@0+ (1,0) [0|1] "" XXX SG_ NEW_SIGNAL_2 : 16|1@0+ (1,0) [0|1] "" XXX SG_ GEAR : 33|4@1+ (1,0) [0|255] "" XXX + SG_ GEAR_SHIFT : 43|4@0+ (1,0) [0|15] "" XXX + SG_ NEW_SIGNAL_3 : 56|6@1+ (1,0) [0|63] "" XXX BO_ 576 STEER_TORQUE: 8 XXX SG_ STEER_TORQUE_SENSOR : 7|16@0+ (1,0) [0|65535] "" XXX @@ -626,5 +630,6 @@ BO_ 1868 EPS_FEEDBACK3: 8 XXX CM_ SG_ 31 GEAR "13-P, 12-R, 11-N, 1-6-D"; VAL_ 31 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D"; VAL_ 552 GEAR 13 "P" 12 "R" 11 "N" 1 "D" 2 "D" 3 "D" 4 "D" 5 "D" 6 "D"; +VAL_ 552 GEAR_SHIFT 6 "6th" 5 "5th" 4 "4th" 3 "3rd" 2 "2nd" 1 "1st" 14 "Shift" 13 "Park" 11 "Neutral" 12 "Reverse"; VAL_ 1098 CRZ_STATE 0 "CRUISE_DISABLED" 1 "CRUISE_READY" 2 "CRUISE_ENABLED" 4 "GAS_OVERRIDE"; -VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED"; \ No newline at end of file +VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED"; diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index e5eca30ea..dafba9f93 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -4,7 +4,9 @@ from openpilot.selfdrive.car import apply_driver_steer_torque_limits, apply_ti_s from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.mazda import mazdacan from openpilot.selfdrive.car.mazda.values import CarControllerParams, Buttons, MazdaFlags -from openpilot.common.realtime import ControlsTimer as Timer +from openpilot.common.realtime import ControlsTimer as Timer, DT_CTRL +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.params import Params VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -23,6 +25,9 @@ class CarController(CarControllerBase): self.hold_delay = Timer(.5) # delay before we start holding as to not hit the brakes too hard self.resume_timer = Timer(0.5) self.cancel_delay = Timer(0.07) # 70ms delay to try to avoid a race condition with stock system + self.acc_filter = FirstOrderFilter(0.0, .1, DT_CTRL, initialized=False) + self.filtered_acc_last = 0 + self.params = Params() def update(self, CC, CS, now_nanos, frogpilot_toggles): can_sends = [] @@ -75,10 +80,47 @@ class CarController(CarControllerBase): hold = self.hold_timer.active() else: self.hold_timer.reset() + + if CC.longActive: + raw_acc_output = CC.actuators.accel * 1150 + raw_acc_output = max(-1000, min(raw_acc_output, 1000)) + + if self.params.get_bool("BlendedACC"): + if self.params.get_bool("ExperimentalMode"): + self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/100) + filtered_acc_output = int(self.acc_filter.update(raw_acc_output)) + else: + # we want to use the stock value in this case but we need a smooth transition. + self.acc_filter.update_alpha(abs(CS.crz_info["ACCEL_CMD"]-self.filtered_acc_last)/100) + filtered_acc_output = int(self.acc_filter.update(CS.crz_info["ACCEL_CMD"])) + + CS.crz_info["ACCEL_CMD"] = int(filtered_acc_output) + self.filtered_acc_last = filtered_acc_output + else: + acc_output = raw_acc_output + if self.frame % 2 == 0: - can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC, CS, hold)) + can_sends.extend(mazdacan.create_radar_command(self.packer, self.frame, CC.longActive, CS, hold)) else: + raw_acc_output = (CC.actuators.accel * 240) + 2000 + if self.params.get_bool("BlendedACC"): + if self.params.get_bool("ExperimentalMode"): + self.acc_filter.update_alpha(abs(raw_acc_output-self.filtered_acc_last)/100) + filtered_acc_output = int(self.acc_filter.update(raw_acc_output)) + else: + # we want to use the stock value in this case but we need a smooth transition. + self.acc_filter.update_alpha(abs(CS.acc["ACCEL_CMD"]-self.filtered_acc_last)/100) + filtered_acc_output = int(self.acc_filter.update(CS.acc["ACCEL_CMD"])) + + acc_output = filtered_acc_output + self.filtered_acc_last = filtered_acc_output + else: + acc_output = raw_acc_output + + if self.params.get_bool("ExperimentalLongitudinalEnabled") and CC.longActive: + CS.acc["ACCEL_CMD"] = acc_output + resume = False hold = False if Timer.interval(2): # send ACC command at 50hz @@ -102,7 +144,7 @@ class CarController(CarControllerBase): self.hold_delay.reset() # reset the hold delay resume = self.resume_timer.active() # stay on for 0.5s to release the brake. This allows the car to move. - can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS, CC, hold, resume)) + can_sends.append(mazdacan.create_acc_cmd(self, self.packer, CS.acc, hold, resume)) # send steering command can_sends.extend(mazdacan.create_steering_control(self.packer, self.CP, diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index de5fbe45f..d18806876 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -31,6 +31,9 @@ class CarState(CarStateBase): self.ti_error = 0 self.ti_lkas_allowed = False + self.shifting = False + self.torque_converter_lock = True + self.update = self.update_gen1 if CP.flags & MazdaFlags.GEN1: self.update = self.update_gen1 @@ -172,6 +175,10 @@ class CarState(CarStateBase): ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_lamp(100, cp.vl["BLINK_INFO"]["LEFT_BLINK"] == 1, cp.vl["BLINK_INFO"]["RIGHT_BLINK"] == 1) + ret.rpm = cp_cam.vl["ENGINE_DATA"]["RPM"] + self.shifting = cp_cam.vl["GEAR"]["SHIFT"] + self.torque_converter_lock = cp_cam.vl["GEAR"]["TORQUE_CONVERTER_LOCK"] + ret.steeringAngleDeg = cp_cam.vl["STEER"]["STEER_ANGLE"] ret.steeringTorque = cp_body.vl["EPS_FEEDBACK"]["STEER_TORQUE_SENSOR"] diff --git a/selfdrive/car/mazda/mazdacan.py b/selfdrive/car/mazda/mazdacan.py index 950c267a3..a9b1fe7e2 100644 --- a/selfdrive/car/mazda/mazdacan.py +++ b/selfdrive/car/mazda/mazdacan.py @@ -1,5 +1,4 @@ from openpilot.selfdrive.car.mazda.values import Buttons, MazdaFlags -from openpilot.common.params import Params from openpilot.common.numpy_fast import clip def create_steering_control(packer, CP, frame, apply_steer, lkas): @@ -178,27 +177,27 @@ STATIC_DATA_366 = [0xFFF7FE7F, 0xFBFF3FC] static_data_list = [STATIC_DATA_361, STATIC_DATA_362, STATIC_DATA_363, STATIC_DATA_364, STATIC_DATA_365, STATIC_DATA_366] # GEN1 radar interceptor -def create_radar_command(packer, frame, CC, CS, hold): - accel = 0 +def create_radar_command(packer, frame, active, CS, hold): + #accel = 0 ret = [] crz_ctrl = CS.crz_cntr crz_info = CS.crz_info - if CC.longActive: # this is set true in longcontrol.py - accel = CC.actuators.accel * 1170 - accel = accel if accel < 1000 else 1000 - else: - accel = int(crz_info["ACCEL_CMD"]) + # if CC.longActive: # this is set true in longcontrol.py + # accel = CC.actuators.accel * 1150 + # accel = accel if accel < 1000 else 1000 + # else: + # accel = int(crz_info["ACCEL_CMD"]) - crz_info["ACC_ACTIVE"] = int(CC.longActive) + crz_info["ACC_ACTIVE"] = active crz_info["ACC_SET_ALLOWED"] = int(bool(int(CS.cp.vl["GEAR"]["GEAR"]) & 4)) # we can set ACC_SET_ALLOWED bit when in drive. Allows crz to be set from 1kmh. crz_info["CRZ_ENDED"] = 0 # this should keep acc on down to 5km/h on my 2018 M3 - crz_info["ACCEL_CMD"] = accel + #crz_info["ACCEL_CMD"] = accel crz_info["STOPPING_MAYBE"] = hold crz_info["STOPPING_MAYBE2"] = hold - crz_ctrl["CRZ_ACTIVE"] = int(CC.longActive) - crz_ctrl["ACC_ACTIVE_2"] = int(CC.longActive) + crz_ctrl["CRZ_ACTIVE"] = active + crz_ctrl["ACC_ACTIVE_2"] = active crz_ctrl["DISABLE_TIMER_1"] = 0 crz_ctrl["DISABLE_TIMER_2"] = 0 @@ -230,14 +229,11 @@ def create_radar_command(packer, frame, CC, CS, hold): return ret # GEN2 new mazdas -def create_acc_cmd(self, packer, CS, CC, hold, resume): - values = CS.acc +def create_acc_cmd(self, packer, values, hold, resume): msg_name = "ACC" bus = 2 if (values["ACC_ENABLED"]): - if Params().get_bool("ExperimentalLongitudinalEnabled") and CC.longActive: - values["ACCEL_CMD"] = (CC.actuators.accel * 240) + 2000 values["HOLD"] = hold values["RESUME"] = resume else: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1a708e00b..1a2d20eaf 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -616,7 +616,11 @@ class Controls: t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update_old_long(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan) else: - actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits) + try: + pitch = self.sm['liveLocationKalman'].calibratedOrientationNED.value[1] + except: + pitch = 0.0 + actuators.accel = self.LoC.update(CC.longActive, CS, long_plan.aTarget, pitch, long_plan.shouldStop, pid_accel_limits) if len(long_plan.speeds): actuators.speed = long_plan.speeds[-1] diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 160dbc393..a819bbe67 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, apply_deadzone from openpilot.selfdrive.controls.lib.pid import PIDController from openpilot.selfdrive.modeld.constants import ModelConstants +from openpilot.common.params import Params CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] @@ -94,11 +95,17 @@ class LongControl: k_f=CP.longitudinalTuning.kf, rate=1 / DT_CTRL) self.v_pid = 0.0 self.last_output_accel = 0.0 + self.gain_step = 0.0001 # Step size for increasing/decreasing gain + self.params = Params() + gain = self.params.get_float("LongOutputGain") + self.auto_tune = self.params.get_bool("ExperimentalLongTune") + self.output_gain = gain if gain != 0.0 and self.auto_tune else 1.0 # Initial output gain + self.experimental_mode_last = False def reset(self): self.pid.reset() - def update(self, active, CS, a_target, should_stop, accel_limits): + def update(self, active, CS, a_target, pitch, should_stop, accel_limits): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] @@ -106,6 +113,12 @@ class LongControl: self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, should_stop, CS.brakePressed, CS.cruiseState.standstill) + if self.params.get_bool("BlendedACC"): + experimental_mode = self.params.get_bool("ExperimentalMode") + if experimental_mode and not self.experimental_mode_last: + self.reset() + self.experimental_mode_last = experimental_mode + if self.long_control_state == LongCtrlState.off: self.reset() output_accel = 0. @@ -129,7 +142,28 @@ class LongControl: output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target) + output_accel = output_accel * self.output_gain self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) + is_flat = abs(pitch) <= 0.05 + + self.auto_tune = self.params.get_bool("ExperimentalLongTune") + if not self.auto_tune: + self.output_gain = 1.0 + if is_flat and self.auto_tune and active and self.last_output_accel == output_accel: # don't adjust when limited or inactive + # if the signs of accel and integrator match, increase the output gain + i = self.pid.i + if (i > 0.02 and a_target > 0.2) or (i < -0.02 and a_target < -0.2): + self.output_gain += self.gain_step + self.output_gain = max(0.5, min(self.output_gain, 2.0)) + self.params.put_float_nonblocking("LongOutputGain",self.output_gain) + self.pid.i = 0.0 + # if the signs of accel and integrator are opposite, decrease the output gain + elif (i < -0.02 and a_target > 0.2) or (i > 0.02 and a_target < -0.2): + self.output_gain -= self.gain_step + self.output_gain = max(0.5, min(self.output_gain, 2.0)) + self.params.put_float_nonblocking("LongOutputGain", self.output_gain) + self.pid.i = 0.0 + return self.last_output_accel def reset_old_long(self, v_pid): diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index ffece2b54..aa0ce0f50 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -48,7 +48,19 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "", "../assets/img_experimental_white.svg", }, - { + { + "ExperimentalLongTune", + tr("Longitudinal Auto-Tune (Beta)"), + tr("Enable the longitudinal auto-tuning feature. Slowly adjusts the acceleration gain to minimize error"), + "../assets/offroad/icon_openpilot.png", + }, + { + "BlendedACC", + tr("Blended Acc (Experimental)"), + tr("Blend stock MRCC and Experimental Mode longitudinal control."), + "../assets/offroad/icon_openpilot.png", + }, + { "TorqueInterceptorEnabled", tr("Torque Interceptor Installed"), tr("Enable the torque interceptor to control the steering wheel."), diff --git a/system/manager/manager.py b/system/manager/manager.py index da682a203..b60b5304b 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -362,7 +362,10 @@ def manager_init() -> None: ("WD40LiveTorqueParameters", ""), ("WD40Score", "0"), ("WheelIcon", "frog"), - ("WheelSpeed", "0") + ("WheelSpeed", "0"), + ("ExperimentalLongTune", "0"), + ("LongOutputGain", "1.0"), + ("BlendedACC", "0"), ] if not PC: default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))