diff --git a/cereal/log.capnp b/cereal/log.capnp index c4a2c30e0..311421550 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2208,4 +2208,5 @@ struct DragonConf { dpDischargingAt @71 :UInt8; dpIsUpdating @72 :Bool; dpTimebombAssist @73 :Bool; -} \ No newline at end of file + dpDynamicGas @74 :Bool; +} diff --git a/common/dp_conf.py b/common/dp_conf.py index 7d3c15d34..dc3e6e211 100644 --- a/common/dp_conf.py +++ b/common/dp_conf.py @@ -58,6 +58,7 @@ confs = [ {'name': 'dp_dynamic_follow', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 4, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, {'name': 'dp_dynamic_follow_multiplier', 'default': 1., 'type': 'Float32', 'min': 0.85, 'max': 1.2, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param']}, {'name': 'dp_dynamic_follow_min_tr', 'default': 0.9, 'type': 'Float32', 'min': 0.85, 'max': 1.6, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param']}, + {'name': 'dp_dynamic_gas', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, {'name': 'dp_accel_profile', 'default': 0, 'type': 'UInt8', 'min': 0, 'max': 3, 'depends': [{'name': 'dp_atl', 'vals': [False]}], 'conf_type': ['param', 'struct']}, # safety {'name': 'dp_driver_monitor', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0e3e80080..32364ebb3 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -54,7 +54,7 @@ class Controls: self.sm = sm if self.sm is None: - socks = ['thermal', 'health', 'model', 'liveCalibration', + socks = ['thermal', 'health', 'model', 'liveCalibration', 'radarState', 'dMonitoringState', 'plan', 'pathPlan', 'liveLocationKalman', 'dragonConf'] ignore_alive = None if params.get('dp_driver_monitor') == b'1' else ['dMonitoringState'] self.sm = messaging.SubMaster(socks, ignore_alive=ignore_alive) @@ -401,7 +401,7 @@ class Controls: v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP) + actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP, self.sm) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan) diff --git a/selfdrive/controls/lib/dynamic_gas.py b/selfdrive/controls/lib/dynamic_gas.py new file mode 100644 index 000000000..60808d90c --- /dev/null +++ b/selfdrive/controls/lib/dynamic_gas.py @@ -0,0 +1,81 @@ +import numpy as np +from common.numpy_fast import clip, interp + +# dp +DP_OFF = 0 +DP_ECO = 1 +DP_NORMAL = 2 +DP_SPORT = 3 + +class DynamicGas: + def __init__(self, CP): + self.CP = CP + self.candidate = self.CP.carFingerprint + self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False} + self.mpc_TR = 1.8 + self.blinker_status = False + self.gas_pressed = False + self.dp_profile = DP_OFF + + def update(self, CS, sm): + v_ego = CS.vEgo + self.handle_passable(CS, sm) + + current_dp_profile = sm['dragonConf'].dpAccelProfile + if self.dp_profile != current_dp_profile: + self.set_profile() + self.dp_profile = current_dp_profile + + if self.dp_profile == DP_OFF: + return float(interp(v_ego, self.CP.gasMaxBP, self.CP.gasMaxV)) + + + gas = interp(v_ego, self.gasMaxBP, self.gasMaxV) + if self.lead_data['status']: # if lead + x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481] # relative velocity mod + y = [0.9901, 0.905, 0.8045, 0.625, 0.431, 0.2083, .0667, 0] + gas_mod = -(gas * interp(self.lead_data['v_rel'], x, y)) + + x = [0.44704, 1.1176, 1.34112] # lead accel mod + y = [1.0, 0.75, 0.625] # maximum we can reduce gas_mod is 40 percent (never increases mod) + gas_mod *= interp(self.lead_data['a_lead'], x, y) + + # as lead gets further from car, lessen gas mod/reduction + x = [(i+v_ego) for i in self.x_lead_mod_x ] + gas_mod *= interp(self.lead_data['x_lead'],x , self.x_lead_mod_y) + gas = gas + gas_mod + + if (self.blinker_status and self.lead_data['v_rel'] >= 0 ): + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, 1.115, 1.225] + gas *= interp(v_ego, x, y) + + return float(clip(gas, 0.0, 1.0)) + + def set_profile(self): + self.x_lead_mod_y = [1.0, 0.75, 0.5, 0.25, 0.0] # as lead gets further from car, lessen gas mod/reduction + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326, 40] + if self.dp_profile == DP_ECO: + #km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144] + y = [0.45, 0.42, 0.38, 0.33, 0.3, 0.32, 0.31, 0.30, 0.30, 0.28, 0.24, 0.21, 0.20, 0.20, 0.19, 0.19, 0.17, 0.15] + self.x_lead_mod_x = [8.1, 12.15, 25.24, 35 , 50 ] + elif self.dp_profile == DP_SPORT: + #km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144] + y = [0.65, 0.67, 0.63, 0.50, 0.53, 0.53, 0.5229, 0.51784, 0.50765, 0.48, 0.496, 0.509, 0.525, 0.538, 0.45, 0.421, 0.42,0.35] + self.x_lead_mod_x = [4.1, 6.15, 8.24, 10 , 15 ] + else: + #km/h[0, 5, 10, 15, 19, 22, 25, 29, 36, 43, 54, 64, 72, 87, 104, 117, 128 144] + y = [0.55, 0.57, 0.43, 0.35, 0.33, 0.33, 0.3529, 0.36784, 0.37765, 0.38, 0.396, 0.309, 0.325, 0.358, 0.32, 0.301, 0.29,0.25] + self.x_lead_mod_x = [7.1, 10.15, 12.24, 15 , 20 ] + + y = [interp(i, [0.2, (0.2 + 0.45) / 2, 0.45], [1.075 * i, i * 1.05, i]) for i in y] + self.gasMaxBP, self.gasMaxV = x, y + + def handle_passable(self, CS, sm): + self.blinker_status = CS.leftBlinker or CS.rightBlinker + self.gas_pressed = CS.gasPressed + lead_one = sm['radarState'].leadOne + self.lead_data['v_rel'] = lead_one.vRel + self.lead_data['a_lead'] = lead_one.aLeadK + self.lead_data['x_lead'] = lead_one.dRel + self.lead_data['status'] = sm['plan'].hasLead # this fixes radarstate always reporting a lead, thanks to arne diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 98c8c188d..e1edbb8c6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,6 +1,8 @@ from cereal import log from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController +from common.params import Params +from selfdrive.controls.lib.dynamic_gas import DynamicGas LongCtrlState = log.ControlsState.LongControlState @@ -62,18 +64,27 @@ class LongControl(): convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 + #dynamic_gas + params = Params() + self.dp_dynamic_gas = (params.get('dp_dynamic_gas') == b'1') + if self.dp_dynamic_gas: + self.dynamic_gas = DynamicGas(CP) def reset(self, v_pid): """Reset PID controller and change setpoint""" self.pid.reset() self.v_pid = v_pid - def update(self, active, CS, v_target, v_target_future, a_target, CP): + def update(self, active, CS, v_target, v_target_future, a_target, CP, sm): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits gas_max = interp(CS.vEgo, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(CS.vEgo, CP.brakeMaxBP, CP.brakeMaxV) + #dynamic_gas + if self.dp_dynamic_gas: + gas_max = self.dynamic_gas.update(CS, sm) + # Update state machine output_gb = self.last_output_gb self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,