Ford: have carstate update returning capnp struct directly (#1113)
* Before abstraction, adding speed init from VW as well * strting to abstract carstate class * fix bug and update lock? * revert pipfile change * another bug * fix linter * bug fix * remove a bunch of diplicated kf code * better to not have class vars. will abstract __init__ anyway later * abstract common instance vars in carstate init and a generic gear parser static method * abstract gear parser for chrysler * abstract gm gear parser too * remove unnecessary random vars * Chrysler: carstate returns capnp struct directly * revert ref commit * test ref * WIP * more WIP * ops, missed this conflict * ford as well * not sure why this got deleted * no need to copy * remove copy * remove copy import * remove unnecessary intermediate variable * remove obsolete comments Co-authored-by: Willem Melching <willem.melching@gmail.com>pull/1117/head
rbiasini
GitHub
parent
807361bdc2
commit
bfa79153b4
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue