mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 08:16:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
MoreTore
2024-09-25 08:48:20 -05:00
parent 12387967c5
commit 0f9b8bb621
9 changed files with 131 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+3
View File
@@ -570,6 +570,9 @@ std::unordered_map<std::string, uint32_t> keys = {
{"RadarInterceptorEnabled", PERSISTENT},
{"NoMRCC", PERSISTENT},
{"NoFSC", PERSISTENT},
{"ExperimentalLongTune", PERSISTENT},
{"LongOutputGain", PERSISTENT},
{"BlendedACC", PERSISTENT},
};
} // namespace
opendbc/mazda_2019.dbc
+7 -2
View File
@@ -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";
VAL_ 1098 DISTANCE_SETTING 4 "CLOSE" 3 "MEDIUM_CLOSE" 2 "MEDIUM_FAR" 1 "FAR" 0 "ACC_DISABLED";
selfdrive/car/mazda/carcontroller.py
+45 -3
View File
@@ -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,
selfdrive/car/mazda/carstate.py
+7
View File
@@ -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"]
selfdrive/car/mazda/mazdacan.py
+12 -16
View File
@@ -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:
selfdrive/controls/controlsd.py
+5 -1
View File
@@ -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]
selfdrive/controls/lib/longcontrol.py
+35 -1
View File
@@ -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):
selfdrive/ui/qt/offroad/settings.cc
+13 -1
View File
@@ -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."),
system/manager/manager.py
+4 -1
View File
@@ -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')))