diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index db574f38a9..d708c13581 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -33,27 +33,27 @@ class CarController(): if (frame % 3) == 0: - curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.v_ego) + curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.out.vEgo) # The use of the toggle below is handy for trying out the various LKAS modes if TOGGLE_DEBUG: - self.lkas_action += int(CS.generic_toggle and not self.generic_toggle_last) + self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) self.lkas_action &= 0xf else: self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy can_sends.append(create_steer_command(self.packer, apply_steer, enabled, - CS.lkas_state, CS.angle_steers, curvature, self.lkas_action)) - self.generic_toggle_last = CS.generic_toggle + CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action)) + self.generic_toggle_last = CS.out.genericToggle if (frame % 100) == 0: can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) - if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \ + if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ (self.steer_alert_last != steer_alert): - can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert)) + can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) if (frame % 200) == 0: can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) @@ -81,7 +81,7 @@ class CarController(): can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) self.enabled_last = enabled - self.main_on_last = CS.main_on + self.main_on_last = CS.out.cruiseState.available self.steer_alert_last = steer_alert return can_sends diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 978363120a..ec461ab169 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,3 +1,4 @@ +from cereal import car from opendbc.can.parser import CANParser from common.numpy_fast import mean from selfdrive.config import Conversions as CV @@ -34,28 +35,26 @@ def get_can_parser(CP): class CarState(CarStateBase): def update(self, cp): - # update prevs, update must run once per loop - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on - - # calc best v_ego estimate, by averaging two opposite corners - self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS - self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS - self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS - self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS - self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - self.standstill = not self.v_ego_raw > 0.001 - - self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] - self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS - self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State'] - self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0 - self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] + ret = car.CarState.new_message() + ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS + ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS + ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS + ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS + ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = not ret.vEgoRaw > 0.001 + ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] + ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] + ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS + ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State'] in [0, 3]) + ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0 + ret.gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] / 100. + ret.gasPressed = ret.gas > 1e-6 + ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"]) + ret.brakeLights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"]) + ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) # TODO: we also need raw driver torque, needed for Assisted Lane Change - self.steer_override = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] + self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] - self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] - self.brake_pressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"]) - self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"]) - self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) + + return ret diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index d9fad4bb5c..6b1f9340f4 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -106,38 +106,10 @@ class CarInterface(CarInterfaceBase): # ******************* do can recv ******************* self.cp.update_strings(can_strings) - self.CS.update(self.cp) - - # create message - ret = car.CarState.new_message() + ret = self.CS.update(self.cp) ret.canValid = self.cp.can_valid - # speeds - ret.vEgo = self.CS.v_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringPressed = self.CS.steer_override - - # gas pedal - ret.gas = self.CS.user_gas / 100. - ret.gasPressed = self.CS.user_gas > 0.0001 - ret.brakePressed = self.CS.brake_pressed - ret.brakeLights = self.CS.brake_lights - - ret.cruiseState.enabled = not (self.CS.pcm_acc_status in [0, 3]) - ret.cruiseState.speed = self.CS.v_cruise_pcm - ret.cruiseState.available = self.CS.pcm_acc_status != 0 - - ret.genericToggle = self.CS.generic_toggle - # events events = [] @@ -167,7 +139,9 @@ class CarInterface(CarInterfaceBase): self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + self.CS.out = ret.as_reader() + + return self.CS.out # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 8e19565d94..f7b8d57790 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -233,7 +233,6 @@ class CarState(CarStateBase): self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2'] self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] - # calc best v_ego estimate, by averaging two opposite corners speed_factor = SPEED_FACTOR[self.CP.carFingerprint] self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e0515b64ec..5305396f30 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -142,7 +142,6 @@ class CarState(CarStateBase): self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 self.pcm_acc_status = int(self.acc_active) - # calc best v_ego estimate, by averaging two opposite corners self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 35644e5653..cb172b591c 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -106,7 +106,6 @@ class CarState(CarStateBase): self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] - # calc best v_ego estimate, by averaging two opposite corners self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS