sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues 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/1117/head
rbiasini 5 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
Split 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:
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

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

@ -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

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

@ -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

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.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

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.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

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.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

Write Preview
Loading…
Cancel
Save