sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 Packages Projects Releases Wiki Activity

Ford: have carstate update returning capnp struct directly (#1113)

Browse Source 
* 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/214/head
rbiasini 6 years ago committed by GitHub
parent 807361bdc2
commit bfa79153b4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 33 additions and 63 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 14
      selfdrive/car/ford/carcontroller.py
  2. 45
      selfdrive/car/ford/carstate.py
  3. 34
      selfdrive/car/ford/interface.py
  4. 1
      selfdrive/car/honda/carstate.py
  5. 1
      selfdrive/car/hyundai/carstate.py
  6. 1
      selfdrive/car/toyota/carstate.py

14
selfdrive/car/ford/carcontroller.py
Unescape View File

@ -33,27 +33,27 @@ class CarController():
if (frame % 3) == 0: 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 # The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG: 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 self.lkas_action &= 0xf
else: else:
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy 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, can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.angle_steers, curvature, self.lkas_action)) CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action))
self.generic_toggle_last = CS.generic_toggle self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0: 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(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)) #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): (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: if (frame % 200) == 0:
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) 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)) 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.enabled_last = enabled
self.main_on_last = CS.main_on self.main_on_last = CS.out.cruiseState.available
self.steer_alert_last = steer_alert self.steer_alert_last = steer_alert
return can_sends return can_sends

45
selfdrive/car/ford/carstate.py
Unescape View File

@ -1,3 +1,4 @@
from cereal import car
from opendbc.can.parser import CANParser from opendbc.can.parser import CANParser
from common.numpy_fast import mean from common.numpy_fast import mean
from selfdrive.config import Conversions as CV from selfdrive.config import Conversions as CV
@ -34,28 +35,26 @@ def get_can_parser(CP):
class CarState(CarStateBase): class CarState(CarStateBase):
def update(self, cp): def update(self, cp):
# update prevs, update must run once per loop ret = car.CarState.new_message()
self.prev_left_blinker_on = self.left_blinker_on ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS
self.prev_right_blinker_on = self.right_blinker_on ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
# calc best v_ego estimate, by averaging two opposite corners ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
self.v_wheel_fl = cp.vl["WheelSpeed_CG1"]['WhlRr_W_Meas'] * WHEEL_RADIUS ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS ret.standstill = not ret.vEgoRaw > 0.001
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
self.standstill = not self.v_ego_raw > 0.001 ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]['Cruise_State'] in [0, 3])
ret.cruiseState.available = cp.vl["Cruise_Status"]['Cruise_State'] != 0
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns'] ret.gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] / 100.
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS ret.gasPressed = ret.gas > 1e-6
self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State'] ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"])
self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0 ret.brakeLights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])
# TODO: we also need raw driver torque, needed for Assisted Lane Change # 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.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"]) return ret
self.brake_lights = bool(cp.vl["BCM_to_HS_Body"]["Brake_Lights"])
self.generic_toggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"])

34
selfdrive/car/ford/interface.py
Unescape View File

@ -106,38 +106,10 @@ class CarInterface(CarInterfaceBase):
# ******************* do can recv ******************* # ******************* do can recv *******************
self.cp.update_strings(can_strings) self.cp.update_strings(can_strings)
self.CS.update(self.cp) ret = self.CS.update(self.cp)
# create message
ret = car.CarState.new_message()
ret.canValid = self.cp.can_valid 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
events = [] events = []
@ -167,7 +139,9 @@ class CarInterface(CarInterfaceBase):
self.brake_pressed_prev = ret.brakePressed self.brake_pressed_prev = ret.brakePressed
self.cruise_enabled_prev = ret.cruiseState.enabled 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 # pass in a car.CarControl
# to be called @ 100hz # to be called @ 100hz

1
selfdrive/car/honda/carstate.py
Unescape View File

@ -233,7 +233,6 @@ class CarState(CarStateBase):
self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2'] 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'] 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] 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_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 self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor

1
selfdrive/car/hyundai/carstate.py
Unescape View File

@ -142,7 +142,6 @@ class CarState(CarStateBase):
self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0
self.pcm_acc_status = int(self.acc_active) 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_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_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 self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS

1
selfdrive/car/toyota/carstate.py
Unescape View File

@ -106,7 +106,6 @@ class CarState(CarStateBase):
self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL']
self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] 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_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_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 self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS

Write Preview
Loading…
Cancel
Save