diff --git a/cereal/car.capnp b/cereal/car.capnp index 9a5d0198e45089..4c3dac838a6dea 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -327,6 +327,7 @@ struct CarParams { centerToFront @9 :Float32; # [m] GC distance to front axle steerRatio @10 :Float32; # [] ratio between front wheels and steering wheel angles steerRatioRear @11 :Float32; # [] rear steering ratio wrt front steering (usually 0) + eonToFront @54 :Float32; # [m] distance from EON to front wheels # things we can derive rotationalInertia @12 :Float32; # [kg*m2] body rotational inertia @@ -341,7 +342,10 @@ struct CarParams { steerKpDEPRECATED @15 :Float32; steerKiDEPRECATED @16 :Float32; steerKf @25 :Float32; - + steerReactance @51 :Float32; + steerInductance @52 :Float32; + steerResistance @53 :Float32; + # Kp and Ki for the longitudinal control longitudinalKpBP @36 :List(Float32); longitudinalKpV @37 :List(Float32); diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 006b6fa3d67eb3..6b47a1045713d7 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -6,11 +6,11 @@ fi function launch { # apply update - if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then - git reset --hard @{u} && - git clean -xdf && - exec "${BASH_SOURCE[0]}" - fi + #if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then + # git reset --hard @{u} && + # git clean -xdf && + # exec "${BASH_SOURCE[0]}" + #fi # no cpu rationing for now echo 0-3 > /dev/cpuset/background/cpus diff --git a/opendbc/gm_global_a_powertrain.dbc b/opendbc/gm_global_a_powertrain.dbc index d7b95da5ad8e2f..c9a7ecc9c75ce9 100644 --- a/opendbc/gm_global_a_powertrain.dbc +++ b/opendbc/gm_global_a_powertrain.dbc @@ -130,13 +130,15 @@ BO_ 481 ASCMSteeringButton: 7 K124_ASCM SG_ DriveModeButton : 39|1@0+ (1,0) [0|1] "" XXX BO_ 485 PSCMSteeringAngle: 8 K43_PSCM - SG_ SteeringWheelAngle : 15|16@0- (0.0625,0) [-525|525] "deg" NEO + SG_ SteeringWheelAngle : 15|16@0- (0.0625,-0.5) [-525|525] "deg" NEO SG_ SteeringWheelRate : 27|12@0- (0.5,0) [-100|100] "deg/s" NEO BO_ 489 EBCMVehicleDynamic: 8 K17_EBCM - SG_ YawRate : 51|12@0- (0.0625,0) [-2047|2047] "grad/s" NEO - SG_ LateralAcceleration : 3|12@0- (0.0161,0) [-2047|2047] "m/s2" NEO - SG_ BrakePedalPressed : 6|1@0+ (1,0) [0|0] "" NEO + SG_ BrakePedalPressed : 6|1@0+ (1,0) [0|0] "" NEO + SG_ NEW_SIGNAL_3 : 17|10@0- (1,0) [0|1] "" NEO + SG_ LateralAcceleration : 3|12@0- (0.0161,0) [-2047|2047] "m/s2" NEO + SG_ YawRate2 : 49|10@0- (0.0625,0) [-2047|2047] "grad/s" NEO + SG_ YawRate : 33|10@0- (0.0625,0) [-100|100] "grad/s" NEO BO_ 560 EPBStatus: 8 EPB SG_ EPBClosed : 12|1@0+ (1,0) [0|1] "" NEO diff --git a/panda/board/safety/safety_gm.h b/panda/board/safety/safety_gm.h index 313d73be1f5cab..97c8a49cd0619f 100644 --- a/panda/board/safety/safety_gm.h +++ b/panda/board/safety/safety_gm.h @@ -8,8 +8,8 @@ // brake rising edge // brake > 0mph -const int GM_MAX_STEER = 300; -const int GM_MAX_RT_DELTA = 128; // max delta torque allowed for real time checks +const int GM_MAX_STEER = 400; +const int GM_MAX_RT_DELTA = 200; // max delta torque allowed for real time checks const int32_t GM_RT_INTERVAL = 250000; // 250ms between real time checks const int GM_MAX_RATE_UP = 8; const int GM_MAX_RATE_DOWN = 20; diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 633ce4b8ae0458..a7660ec2f57b17 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -77,6 +77,10 @@ def get_params(candidate, fingerprint): ret.steerKf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 f = 1.2 tireStiffnessFront_civic *= f diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 40afa84042f5d9..51a0cc363561ac 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -11,10 +11,10 @@ class CarControllerParams(): def __init__(self, car_fingerprint): if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): - self.STEER_MAX = 300 + self.STEER_MAX = 400 self.STEER_STEP = 2 # how often we update the steer cmd - self.STEER_DELTA_UP = 8 # ~0.75s time to peak torque (255/50hz/0.75s) - self.STEER_DELTA_DOWN = 20 # ~0.3s from peak torque to zero + self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s) + self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero elif car_fingerprint == CAR.CADILLAC_CT6: self.STEER_MAX = 150 self.STEER_STEP = 1 # how often we update the steer cmd diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 97db50588d0b15..8b1fa157fbbd47 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -31,6 +31,7 @@ def get_powertrain_can_parser(CP, canbus): ("LKADriverAppldTrq", "PSCMStatus", 0), ("LKATorqueDeliveredStatus", "PSCMStatus", 0), ("DistanceButton", "ASCMSteeringButton", 0), + ("YawRate", "EBCMVehicleDynamic", 0), ] if CP.carFingerprint == CAR.VOLT: diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 06f663f313afa9..abae74f89af107 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -103,7 +103,7 @@ def get_params(candidate, fingerprint): ret.mass = 4353. * CV.LB_TO_KG + std_cargo ret.safetyModel = car.CarParams.SafetyModels.gm ret.wheelbase = 2.86 - ret.steerRatio = 14.4 #end to end is 13.46 + ret.steerRatio = 12.0 #end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 @@ -183,8 +183,13 @@ def get_params(candidate, fingerprint): elif candidate == CAR.ACADIA: ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] - ret.steerKpV, ret.steerKiV = [[0.63], [0.1]] - ret.steerKf = 0.00006 # full torque for 20 deg at 80mph means 0.00007818594 + ret.steerKpV, ret.steerKiV = [[0.4], [0.1]] + ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + + ret.steerReactance = 0.9 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 1.2 ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] @@ -205,7 +210,7 @@ def get_params(candidate, fingerprint): ret.stoppingControl = True ret.startAccel = 1.0 ret.steerActuatorDelay = 0.15 # Default delay, not measured yet - ret.steerRateCost = 1.0 + ret.steerRateCost = 0.5 ret.steerControlType = car.CarParams.SteerControlType.torque @@ -388,4 +393,4 @@ def apply(self, c, perception_state=log.Live20Data.new_message()): c.hudControl.leadVisible, \ chime, chime_count) - self.frame += 1 + self.frame += 1 diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 2bfaf85ff0a5c4..9c477ad18fa2fb 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -138,7 +138,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ # *** compute control surfaces *** BRAKE_MAX = 1024/4 - if CS.CP.carFingerprint in (CAR.ACURA_ILX): + if CS.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.ACURA_ILX): STEER_MAX = 0xF00 elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value (max value from energee) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index b3c2ee1e7a77ad..1bf0fe33a1ef74 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -36,7 +36,6 @@ def get_can_signals(CP): ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), ("STEER_ANGLE", "STEERING_SENSORS", 0), ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), - ("STEER_ANGLE_OFFSET", "STEERING_SENSORS", 0), ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), ("LEFT_BLINKER", "SCM_FEEDBACK", 0), ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), @@ -240,15 +239,8 @@ def update(self, cp, cp_cam): self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] - - # Apply the reported angle offset for cars that have it defined - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_HATCH): - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET'] - else: - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] - + self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] - self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 400eb2ce84c63a..2fca4481dcae98 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -186,6 +186,10 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerKf = 0.00006 # conservative feed-forward + ret.steerReactance = 1.0 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 if candidate == CAR.CIVIC: stop_and_go = True @@ -226,7 +230,12 @@ def get_params(candidate, fingerprint): ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 15.96 # 11.82 is spec end-to-end tire_stiffness_factor = 0.8467 - ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] + ret.steerReactance = 1.0 + ret.steerInductance = 1.0 + ret.steerResistance = 0.5 + ret.eonToFront = 1.0 + ret.steerKpV, ret.steerKiV = [[0.64], [0.192]] + ret.steerKf = 0.000064 ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] ret.longitudinalKiBP = [0., 35.] diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index c525c0185e939b..2020634dacc451 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -69,6 +69,11 @@ def get_params(candidate, fingerprint): tireStiffnessFront_civic = 192150 tireStiffnessRear_civic = 202500 + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + ret.steerActuatorDelay = 0.1 # Default delay tire_stiffness_factor = 1. diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index b7d60a4257bfee..160d9b469b5f8e 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -74,6 +74,10 @@ def get_params(candidate, fingerprint): ret.longitudinalKiBP = [0.] ret.longitudinalKiV = [0.] ret.steerActuatorDelay = 0. + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 return ret diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 4f3b53236acb18..0947317854ccfd 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -76,6 +76,11 @@ def get_params(candidate, fingerprint): ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + ret.steerReactance = 0.7 + ret.steerInductance = 1.0 + ret.steerResistance = 1.0 + ret.eonToFront = 0.5 + if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 @@ -86,6 +91,7 @@ def get_params(candidate, fingerprint): ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 # TODO: Prius seem to have very laggy actuators. Understand if it is lag or hysteresis ret.steerActuatorDelay = 0.25 + ret.eonToFront = -1.0 elif candidate in [CAR.RAV4, CAR.RAV4H]: ret.safetyParam = 73 # see conversion factor for STEER_TORQUE_EPS in dbc file diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2bef3add47085f..e989e00ca1ab8f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -280,7 +280,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL) # Send a "steering required alert" if saturation count has reached the limit - if LaC.sat_flag and CP.steerLimitAlert and CS.lkMode and not CS.leftBlinker and not CS.rightBlinker: + if LaC.sat_flag and CP.steerLimitAlert and not CS.leftBlinker and not CS.rightBlinker: AM.add("steerSaturated", enabled) # Parse permanent warnings to display constantly @@ -549,6 +549,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): CP.steerRatio = tuning.steerRatio[0] CP.steerActuatorDelay = tuning.steerActuatorDelay[0] CP.steerRateCost = tuning.steerRateCost[0] + CP.steerReactance = tuning.steerReactance[0] + CP.steerInductance = tuning.steerInductance[0] + CP.steerResistance = tuning.steerResistance[0] + CP.eonToFront = tuning.eonToFront[0] last_mod_time = os.path.getmtime(tune_file) else: @@ -562,6 +566,10 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): print "CP.steerRatio: %s" % CP.steerRatio print "CP.steerActuatorDelay: %s" % CP.steerActuatorDelay print "CP.steerRateCost: %s" % CP.steerRateCost + print "CP.steerReactance: %s" % CP.steerReactance + print "CP.steerInductance: %s" % CP.steerInductance + print "CP.steerResistance: %s" % CP.steerResistance + print "CP.eonToFront: %s" % CP.eonToFront VM.update_rt_params(CP) LaC.update_rt_params(CP) diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 92e1fde33a294e..00b6e018026a84 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -12,8 +12,8 @@ _DT_MPC = 0.05 # 20Hz -def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): - states[0].x = v_ego * delay + 1.3 +def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay, long_camera_offset): + states[0].x = max(0.0, v_ego * delay + long_camera_offset) states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay return states @@ -35,22 +35,18 @@ def apply_deadzone(angle, deadzone): class LatControl(object): def __init__(self, CP): - _ADJUST_REACTANCE = 1.0 - _ADJUST_INDUCTANCE = 1.2 - _ADJUST_RESISTANCE = 1.0 - # Eliminate break-points, since they aren't needed (and would cause problems for resonance) - KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV) * _ADJUST_REACTANCE] - KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE] - Kf = CP.steerKf * _ADJUST_INDUCTANCE + KpV = [np.interp(25.0, CP.steerKpBP, CP.steerKpV) * CP.steerReactance] + KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * CP.steerReactance] + Kf = CP.steerKf * CP.steerInductance self.pid = PIController(([0.], KpV), ([0.], KiV), k_f=Kf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(CP.steerRateCost) - self.smooth_factor = _ADJUST_INDUCTANCE * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) - self.projection_factor = _ADJUST_REACTANCE * 5.0 * _DT # Mutiplier for reactive component (PI) - self.accel_limit = 2.0 / _ADJUST_RESISTANCE # Desired acceleration limit to prevent "whip steer" (resistive component) + self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward) + self.projection_factor = CP.steerReactance * 5.0 * _DT # Mutiplier for reactive component (PI) + self.accel_limit = 2.0 / CP.steerResistance # Desired acceleration limit to prevent "whip steer" (resistive component) self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward self.prev_angle_rate = 0 @@ -58,6 +54,7 @@ def __init__(self, CP): self.last_mpc_ts = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_time = 0.0 + self.angle_steers_des_mpc = 0.0 self.projected_angle_steers = 0.0 self.steer_counter = 1.0 self.steer_counter_prev = 0.0 @@ -72,6 +69,9 @@ def update_rt_params(self, CP): #KiV = [np.interp(25.0, CP.steerKiBP, CP.steerKiV) * _ADJUST_REACTANCE] #Kf = CP.steerKf * _ADJUST_INDUCTANCE self.setup_mpc(CP.steerRateCost) + self.smooth_factor = CP.steerInductance * 2.0 * CP.steerActuatorDelay / _DT + self.projection_factor = CP.steerReactance * 5.0 * _DT + self.accel_limit = 2.0 / CP.steerResistance def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc @@ -97,7 +97,7 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly if angle_rate == 0.0 and self.calculate_rate: if angle_steers != self.prev_angle_steers: self.steer_counter_prev = self.steer_counter - self.rough_steers_rate = 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev + self.rough_steers_rate = (self.rough_steers_rate + 100.0 * (angle_steers - self.prev_angle_steers) / self.steer_counter_prev) / 2.0 self.steer_counter = 0.0 elif self.steer_counter >= self.steer_counter_prev: self.rough_steers_rate = (self.steer_counter * self.rough_steers_rate) / (self.steer_counter + 1.0) @@ -120,10 +120,10 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly curvature_factor = VM.curvature_factor(v_ego) # Determine future angle steers using steer rate - projected_angle_steers = float(angle_steers) + CP.steerActuatorDelay * float(angle_rate) + projected_angle_steers = float(angle_steers) + CP.steerActuatorDelay * float(accelerated_angle_rate) # Determine a proper delay time that includes the model's variable processing time - plan_age = cur_time - mpc_time + plan_age = _DT_MPC + cur_time - mpc_time total_delay = CP.steerActuatorDelay + plan_age l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly)) @@ -131,8 +131,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly)) # account for actuation delay and the age of the plan - self.cur_state = calc_states_after_delay(self.cur_state, v_ego, projected_angle_steers, - curvature_factor, CP.steerRatio, total_delay) + self.cur_state = calc_states_after_delay(self.cur_state, v_ego, projected_angle_steers, curvature_factor, + CP.steerRatio, total_delay, CP.eonToFront) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, @@ -151,6 +151,8 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly self.mpc_times = [self.angle_steers_des_time, mpc_time + _DT_MPC, mpc_time + _DT_MPC + _DT_MPC] + + self.angle_steers_des_mpc = self.mpc_angles[1] else: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 78cb21df28aee4..c6a664fdf3c8b3 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,7 +1,7 @@ from common.numpy_fast import interp from selfdrive.controls.lib.latcontrol_helpers import model_polyfit, calc_desired_path, compute_path_pinv -CAMERA_OFFSET = 0.05 # m from center car to camera +CAMERA_OFFSET = 0.06 # m from center car to camera class PathPlanner(object): def __init__(self): @@ -12,9 +12,9 @@ def __init__(self): self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() - self.lane_width_estimate = 3.7 + self.lane_width_estimate = 3.2 self.lane_width_certainty = 1.0 - self.lane_width = 3.7 + self.lane_width = 3.2 def update(self, v_ego, md): if md is not None: diff --git a/selfdrive/dashboard.py b/selfdrive/dashboard.py new file mode 100644 index 00000000000000..c27370d78ed735 --- /dev/null +++ b/selfdrive/dashboard.py @@ -0,0 +1,322 @@ +#!/usr/bin/env python +#import gc +import zmq +import time +import numpy as np +from influxdb import InfluxDBClient, SeriesHelper +#import numpy.matlib +#import importlib +#from collections import defaultdict +#from fastcluster import linkage_vector +import selfdrive.messaging as messaging +from selfdrive.services import service_list +from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset +from selfdrive.controls.lib.pathplanner import PathPlanner +#from selfdrive.controls.lib.radar_helpers import Track, Cluster, fcluster, \ +# RDR_TO_LDR, NO_FUSION_SCORE +from selfdrive.controls.lib.vehicle_model import VehicleModel +#from selfdrive.swaglog import cloudlog +#from cereal import car +#from common.params import Params +from common.realtime import set_realtime_priority, Ratekeeper +#from common.kalman.ekf import EKF, SimpleSensor + + +def dashboard_thread(rate=100): + set_realtime_priority(4) + + USER = '' + PASSWORD = '' + DBNAME = 'carDB' + #influx = InfluxDBClient('192.168.1.61', 8086, USER, PASSWORD, DBNAME) + influx = InfluxDBClient('192.168.43.198', 8086, USER, PASSWORD, DBNAME) + influxLineString = "" + + context = zmq.Context() + + poller = zmq.Poller() + ipaddress = "127.0.0.1" + #ipaddress = "192.168.43.1" + #ipaddress = "192.168.1.33" + model = messaging.sub_sock(context, service_list['model'].port, addr=ipaddress, conflate=True, poller=poller) + live100 = messaging.sub_sock(context, service_list['live100'].port, addr=ipaddress, conflate=True, poller=poller) + live20 = messaging.sub_sock(context, service_list['live20'].port, addr=ipaddress, conflate=True, poller=poller) + carState = messaging.sub_sock(context, service_list['carState'].port, addr=ipaddress, conflate=True, poller=poller) + can = messaging.sub_sock(context, service_list['can'].port, addr=ipaddress, conflate=True, poller=poller) + frame = messaging.sub_sock(context, service_list['frame'].port, addr=ipaddress, conflate=True, poller=poller) + sensorEvents = messaging.sub_sock(context, service_list['sensorEvents'].port, addr=ipaddress, conflate=True, poller=poller) + carControl = messaging.sub_sock(context, service_list['carControl'].port, addr=ipaddress, conflate=True, poller=poller) + health = messaging.sub_sock(context, service_list['health'].port, addr=ipaddress, conflate=True, poller=poller) + sendcan = messaging.sub_sock(context, service_list['sendcan'].port, addr=ipaddress, conflate=True, poller=poller) + androidLog = messaging.sub_sock(context, service_list['androidLog'].port, addr=ipaddress, conflate=True, poller=poller) + + _model = None + _live100 = None + _live20 = None + _carState = None + _can = None + _frame = None + _sensorEvents = None + _carControl = None + _health = None + _sendcan = None + _androidLog = None + + frame_count = 0 + + rk = Ratekeeper(rate, print_delay_threshold=np.inf) + while 1: + sample_str = "" + + for socket, event in poller.poll(0): + if socket is live100: + _live100 = messaging.recv_one(socket) + + if sample_str != "": + sample_str += "," + sample_str += ("angleSteersDes=%f,vEgo=%f,steerOverride=%f,vPid=%f,upSteer=%f,uiSteer=%f,ufSteer=%f,cumLagMs=%f,vCruise=%f" % + (_live100.live100.angleSteersDes, _live100.live100.vEgo, _live100.live100.steerOverride, _live100.live100.vPid, + _live100.live100.upSteer, _live100.live100.uiSteer, _live100.live100.ufSteer, _live100.live100.cumLagMs, _live100.live100.vCruise)) + + '''print(_live100) + live100 = ( + vEgo = 0, + aEgoDEPRECATED = 0, + vPid = 0.3, + vTargetLead = 0, + upAccelCmd = 0, + uiAccelCmd = 0, + yActualDEPRECATED = 0, + yDesDEPRECATED = 0, + upSteer = 0, + uiSteer = 0, + aTargetMinDEPRECATED = 0, + aTargetMaxDEPRECATED = 0, + jerkFactor = 0, + angleSteers = -1, + hudLeadDEPRECATED = 0, + cumLagMs = -0.57132614, + canMonoTimeDEPRECATED = 0, + l20MonoTimeDEPRECATED = 0, + mdMonoTimeDEPRECATED = 0, + enabled = false, + steerOverride = false, + canMonoTimes = [], + vCruise = 255, + rearViewCam = false, + alertText1 = "", + alertText2 = "", + awarenessStatus = 0, + angleOffset = -0.74244022, + planMonoTime = 10166703166901, + angleSteersDes = -1, + longControlState = off, + state = disabled, + vEgoRaw = 0, + ufAccelCmd = 0, + ufSteer = 0, + aTarget = 0, + active = false, + curvature = -0.00038641863, + alertStatus = normal, + alertSize = none, + gpsPlannerActive = false, + engageable = false, + alertBlinkingRate = 0, + driverMonitoringOn = false, + alertType = "", + alertSound = "" ) )''' + elif socket is model: + _model = messaging.recv_one(socket) + '''model = ( + frameId = 19786, + path = ( + points = [-0.002040863, 0.0048789978, 0.0024032593, -0.029251099, -0.050567627, -0.071716309, -0.10424805, -0.14196777, -0.18005371, -0.20825195, -0.2277832, -0.26391602, -0.31420898, -0.38085938, -0.43212891, -0.47900391, -0.51318359, -0.56494141, -0.62646484, -0.68212891, -0.73632812, -0.74951172, -0.82519531, -0.89648438, -0.97265625, -1.0615234, -1.1464844, -1.2412109, -1.3369141, -1.4462891, -1.5488281, -1.6445312, -1.7460938, -1.8544922, -1.9658203, -2.0820312, -2.2089844, -2.3320312, -2.484375, -2.6152344, -2.7265625, -2.8554688, -2.984375, -3.1425781, -3.2636719, -3.4160156, -3.5566406, -3.6835938, -3.8222656, -3.9746094], + prob = 1, + std = -0.66490114 ), + leftLane = ( + points = [1.7112548, 1.7149169, 1.720166, 1.7105224, 1.704602, 1.7070434, 1.7025878, 1.6870239, 1.6792724, 1.6579101, 1.6454589, 1.633374, 1.6216552, 1.6204345, 1.5994384, 1.5890625, 1.5706298, 1.5534179, 1.539746, 1.5231445, 1.5058105, 1.4767578, 1.4601562, 1.4442871, 1.4245117, 1.407666, 1.3986328, 1.3683593, 1.340039, 1.3173339, 1.2873046, 1.2677734, 1.234082, 1.199414, 1.1696289, 1.1339843, 1.1095703, 1.0788085, 1.0495117, 1.0299804, 0.98798829, 0.95527345, 0.925, 0.89277345, 0.86982423, 0.81464845, 0.77656251, 0.75117189, 0.71308595, 0.66328126], + prob = 0.11716748, + std = -3.1076281 ), + rightLane = ( + points = [-2.0839355, -2.0990722, -2.1112792, -2.1142089, -2.1159179, -2.1269042, -2.1374023, -2.1483886, -2.1569335, -2.1713378, -2.1752441, -2.1828125, -2.1942871, -2.2208984, -2.2377441, -2.2616699, -2.3014648, -2.3073242, -2.3302734, -2.3463867, -2.3625, -2.37666, -2.3898437, -2.4123046, -2.4396484, -2.4586914, -2.4953125, -2.5304687, -2.5529296, -2.5724609, -2.6032226, -2.6344726, -2.6637695, -2.6911132, -2.727246, -2.758496, -2.7921875, -2.8205078, -2.8458984, -2.8791015, -2.9230468, -2.9416015, -2.9894531, -3.0148437, -3.0392578, -3.0695312, -3.1144531, -3.1388671, -3.1652343, -3.1916015], + prob = 0.095686942, + std = -2.2504346 ), + lead = (dist = 13.424072, prob = 0.37279058, std = 16.329063), + settings = ( + bigBoxX = 0, + bigBoxY = 0, + bigBoxWidth = 0, + bigBoxHeight = 0, + inputTransform = [1.25, 0, 374, 0, 1.25, 337.75, 0, 0, 1] ) ) )''' + elif socket is live20: + _live20 = messaging.recv_one(socket) + '''live20 = ( + angleOffsetDEPRECATED = 0, + calStatusDEPRECATED = 0, + leadOne = ( + dRel = 0, + yRel = 0, + vRel = 0, + aRel = 0, + vLead = 0, + aLeadDEPRECATED = 0, + dPath = 0, + vLat = 0, + vLeadK = 0, + aLeadK = 0, + fcw = false, + status = false, + aLeadTau = 0 ), + cumLagMs = 7678.7031, + mdMonoTime = 10166684697265, + ftMonoTimeDEPRECATED = 0, + calCycleDEPRECATED = 0, + calPercDEPRECATED = 0, + canMonoTimes = [], + l100MonoTime = 10166705048151, + radarErrors = [] ) )''' + elif socket is carState: + _carState = messaging.recv_one(socket) + + if sample_str != "": + sample_str += "," + sample_str += ("steeringTorque=%f,steeringRate=%f,yawRate=%f" % (_carState.carState.steeringTorque, _carState.carState.steeringRate, _carState.carState.yawRate)) + + '''carState = ( + vEgo = 0, + wheelSpeeds = (fl = 0, fr = 0, rl = 0, rr = 0), + gas = 0, + gasPressed = false, + brake = 0, + brakePressed = false, + steeringAngle = -1, + steeringTorque = 84, + steeringPressed = false, + cruiseState = ( + enabled = false, + speed = 0, + available = true, + speedOffset = -0.3, + standstill = false ), + buttonEvents = [], + canMonoTimes = [], + events = [ + ( name = wrongGear, + enable = false, + noEntry = true, + warning = false, + userDisable = false, + softDisable = true, + immediateDisable = false, + preEnable = false, + permanent = false ), + gearShifter = park, + steeringRate = 0, + aEgo = 0, + vEgoRaw = 0, + standstill = true, + brakeLights = false, + leftBlinker = false, + rightBlinker = false, + yawRate = -0, + genericToggle = false, + doorOpen = false, + seatbeltUnlatched = true ) )''' + elif socket is can: + _can = messaging.recv_one(socket) + '''can = [ + ( address = 513, + busTime = 42188, + dat = "", + src = 1 ),''' + elif socket is frame: + _frame = messaging.recv_one(socket) + '''frame = ( + frameId = 14948, + encodeId = 14947, + timestampEof = 10728391665000, + frameLength = 5419, + integLines = 601, + globalGain = 509, + frameType = unknown, + timestampSof = 0, + transform = [1, 0, 0, 0, 1, 0, 0, 0, 1], + lensPos = 281, + lensSag = -0.59991455, + lensErr = 54, + lensTruePos = 273.13062 ) )''' + elif socket is sensorEvents: + _sensorEvents = messaging.recv_one(socket) + '''sensorEvents = [ + ( version = 104, + sensor = 2, + type = 2, + timestamp = 10551787945172, + magnetic = ( + v = [-40.446472, 92.475891, 17.285156], + status = 2 ), + source = android, + uncalibratedDEPRECATED = false ), + ( version = 104, + sensor = 5, + type = 16, + timestamp = 10551844372174, + source = android, + uncalibratedDEPRECATED = false, + gyroUncalibrated = ( + v = [-0.00062561035, -0.0029144287, -0.039916992, 1.5258789e-05, -0.0028686523, -0.039474487], + status = 0 ) ) ] )''' + elif socket is carControl: + _carControl = messaging.recv_one(socket) + '''carControl = ( + enabled = false, + gasDEPRECATED = 0, + brakeDEPRECATED = 0, + steeringTorqueDEPRECATED = 0, + cruiseControl = ( + cancel = false, + override = true, + speedOverride = 0, + accelOverride = 0.714 ), + hudControl = ( + speedVisible = false, + setSpeed = 70.833336, + lanesVisible = false, + leadVisible = false, + visualAlert = none, + audibleAlert = none ), + actuators = (gas = 0, brake = -0, steer = 0, steerAngle = -2.7103169), + active = false ) )''' + elif socket is _health: + _health = messaging.recv_one(socket) + print(_health) + elif socket is _sendcan: + _sendcan = messaging.recv_one(socket) + print(_sendcan) + elif socket is _androidLog: + _androidLog = messaging.recv_one(socket) + print(_androidLog) + + if sample_str != "": + influxLineString += ("opData,sources=capnp " + sample_str + " %s\n" % int(time.time() * 1000000000)) + frame_count += 1 + + if frame_count >= 20: + headers = { 'Content-type': 'application/octet-stream', 'Accept': 'text/plain' } + try: + return = influx.request("write",'POST', {'db':DBNAME}, influxLineString.encode('utf-8'), 204, headers) + print(return) + print ('%d %d' % (frame_count, len(influxLineString))) + except: + continue + frame_count = 0 + influxLineString = "" + + rk.keep_time() + +def main(rate=100): + dashboard_thread(rate) + +if __name__ == "__main__": + main() diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index 2a03815007ff70..8b1241806aca12 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -112,13 +112,13 @@ def check_car_battery_voltage(should_start, health, charging_disabled, msg): # - 12V battery voltage is too low, and; # - onroad isn't started # - keep battery within 67-70% State of Charge to preserve longevity - if charging_disabled and (health is None or health.health.voltage > 11500) and msg.thermal.batteryPercent < 60: + if charging_disabled and (health is None or health.health.voltage > 11500) and msg.thermal.batteryPercent < 75: charging_disabled = False os.system('echo "1" > /sys/class/power_supply/battery/charging_enabled') - elif not charging_disabled and (msg.thermal.batteryPercent > 70 or (health is not None and health.health.voltage < 11000 and not should_start)): + elif not charging_disabled and (msg.thermal.batteryPercent > 80 or (health is not None and health.health.voltage < 11000 and not should_start)): charging_disabled = True os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled') - elif msg.thermal.batteryCurrent < 0 and msg.thermal.batteryPercent > 70: + elif msg.thermal.batteryCurrent < 0 and msg.thermal.batteryPercent > 80: charging_disabled = True os.system('echo "0" > /sys/class/power_supply/battery/charging_enabled')