car port: comma body (#24019)
* body FPv2 * .. * .. * .. * Temp, REVERT! * more cleanup * typo * .. * del eyes * should work? * fix * new dbc * .. * fixes * static analysis * cln balancing code * no test route * excluded_interfaces * THE DOCS! * comments on steer/speed mixin * switch to bus 0 * less UDS * FAKE bus 0 vin and fingerprint * FAKE locationd * Keep steady and remove handcoded offset * Improve startup sequence, get closer to stock openpilot * Forgot to define angle * lowercase * revert that * little cleanup * change safety model * update refs * body is gold * handle no llk * oops * litte more Co-authored-by: Comma Device <device@comma.ai> Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>pull/80/head
Igor Biletskyy
4 years ago
committed by
GitHub
parent
ab9c017311
commit
031f79ba88
15 changed files with 270 additions and 2 deletions
@ -1 +1 @@ |
|||||||
Subproject commit 93863197dd3228649e286074f79a0174821a8a81 |
Subproject commit 560bcc4063f573e4d5d221235469334a4af14d50 |
||||||
@ -0,0 +1,8 @@ |
|||||||
|
def create_control(packer, torque_l, torque_r): |
||||||
|
can_bus = 0 |
||||||
|
|
||||||
|
values = { |
||||||
|
"TORQUE_L": torque_l, |
||||||
|
"TORQUE_R": torque_r, |
||||||
|
} |
||||||
|
return packer.make_can_msg("TORQUE_CMD", can_bus, values) |
||||||
@ -0,0 +1,101 @@ |
|||||||
|
import numpy as np |
||||||
|
|
||||||
|
from selfdrive.car.body import bodycan |
||||||
|
from opendbc.can.packer import CANPacker |
||||||
|
|
||||||
|
MAX_TORQUE = 500 |
||||||
|
MAX_TORQUE_RATE = 50 |
||||||
|
MAX_ANGLE_ERROR = 7 |
||||||
|
|
||||||
|
class CarController(): |
||||||
|
def __init__(self, dbc_name, CP, VM): |
||||||
|
self.packer = CANPacker(dbc_name) |
||||||
|
|
||||||
|
self.i_speed = 0 |
||||||
|
|
||||||
|
self.i_balance = 0 |
||||||
|
self.d_balance = 0 |
||||||
|
|
||||||
|
self.i_torque = 0 |
||||||
|
|
||||||
|
self.speed_measured = 0. |
||||||
|
self.speed_desired = 0. |
||||||
|
|
||||||
|
self.torque_r_filtered = 0. |
||||||
|
self.torque_l_filtered = 0. |
||||||
|
|
||||||
|
@staticmethod |
||||||
|
def deadband_filter(torque, deadband): |
||||||
|
if torque > 0: |
||||||
|
torque += deadband |
||||||
|
else: |
||||||
|
torque -= deadband |
||||||
|
return torque |
||||||
|
|
||||||
|
def update(self, CC, CS): |
||||||
|
if len(CC.orientationNED) == 0 or len(CC.angularVelocity) == 0: |
||||||
|
return [], CC.actuators.copy() |
||||||
|
|
||||||
|
# /////////////////////////////////////// |
||||||
|
# Steer and accel mixin. Speed should be used as a target? (speed should be in m/s! now it is in RPM) |
||||||
|
# Mix steer into torque_diff |
||||||
|
# self.steerRatio = 0.5 |
||||||
|
# torque_r = int(np.clip((CC.actuators.accel*1000) - (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) |
||||||
|
# torque_l = int(np.clip((CC.actuators.accel*1000) + (CC.actuators.steer*1000) * self.steerRatio, -1000, 1000)) |
||||||
|
# //// |
||||||
|
# Setpoint speed PID |
||||||
|
kp_speed = 0.001 |
||||||
|
ki_speed = 0.00001 |
||||||
|
alpha_speed = 1.0 |
||||||
|
|
||||||
|
self.speed_measured = (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2. |
||||||
|
self.speed_desired = (1. - alpha_speed)*self.speed_desired |
||||||
|
p_speed = (self.speed_desired - self.speed_measured) |
||||||
|
self.i_speed += ki_speed * p_speed |
||||||
|
self.i_speed = np.clip(self.i_speed, -0.1, 0.1) |
||||||
|
set_point = p_speed * kp_speed + self.i_speed |
||||||
|
|
||||||
|
# Balancing PID |
||||||
|
kp_balance = 1300 |
||||||
|
ki_balance = 0 |
||||||
|
kd_balance = 280 |
||||||
|
alpha_d_balance = 1.0 |
||||||
|
|
||||||
|
# Clip angle error, this is enough to get up from stands |
||||||
|
p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR)) |
||||||
|
self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr |
||||||
|
self.d_balance = np.clip(((1. - alpha_d_balance) * self.d_balance + alpha_d_balance * -CC.angularVelocity[1]), -1., 1.) |
||||||
|
torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000)) |
||||||
|
|
||||||
|
# Positional recovery PID |
||||||
|
kp_torque = 0.95 |
||||||
|
ki_torque = 0.1 |
||||||
|
|
||||||
|
p_torque = (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) |
||||||
|
self.i_torque += (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr) |
||||||
|
torque_diff = int(np.clip(p_torque*kp_torque + self.i_torque*ki_torque, -100, 100)) |
||||||
|
|
||||||
|
# Combine 2 PIDs outputs |
||||||
|
torque_r = torque + torque_diff |
||||||
|
torque_l = torque - torque_diff |
||||||
|
|
||||||
|
# Torque rate limits |
||||||
|
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10) , |
||||||
|
self.torque_r_filtered - MAX_TORQUE_RATE, |
||||||
|
self.torque_r_filtered + MAX_TORQUE_RATE) |
||||||
|
self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10), |
||||||
|
self.torque_l_filtered - MAX_TORQUE_RATE, |
||||||
|
self.torque_l_filtered + MAX_TORQUE_RATE) |
||||||
|
torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE)) |
||||||
|
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE)) |
||||||
|
|
||||||
|
# /////////////////////////////////////// |
||||||
|
|
||||||
|
can_sends = [] |
||||||
|
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r)) |
||||||
|
|
||||||
|
new_actuators = CC.actuators.copy() |
||||||
|
new_actuators.accel = torque_l |
||||||
|
new_actuators.steer = torque_r |
||||||
|
|
||||||
|
return new_actuators, can_sends |
||||||
@ -0,0 +1,54 @@ |
|||||||
|
from cereal import car |
||||||
|
from opendbc.can.parser import CANParser |
||||||
|
from selfdrive.car.interfaces import CarStateBase |
||||||
|
from selfdrive.car.body.values import DBC |
||||||
|
|
||||||
|
|
||||||
|
class CarState(CarStateBase): |
||||||
|
def update(self, cp): |
||||||
|
ret = car.CarState.new_message() |
||||||
|
|
||||||
|
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L'] |
||||||
|
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R'] |
||||||
|
|
||||||
|
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor |
||||||
|
|
||||||
|
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||||
|
ret.standstill = abs(ret.vEgo) < 1 |
||||||
|
|
||||||
|
# irrelevant for non-car |
||||||
|
ret.doorOpen = False |
||||||
|
ret.seatbeltUnlatched = False |
||||||
|
ret.gearShifter = car.CarState.GearShifter.drive |
||||||
|
ret.steeringTorque = 0 |
||||||
|
ret.steeringAngleDeg = 0 |
||||||
|
ret.steeringPressed = False |
||||||
|
ret.cruiseState.enabled = True |
||||||
|
ret.cruiseState.available = True |
||||||
|
ret.cruiseState.speed = 0 |
||||||
|
|
||||||
|
return ret |
||||||
|
|
||||||
|
@staticmethod |
||||||
|
def get_can_parser(CP): |
||||||
|
signals = [ |
||||||
|
# sig_name, sig_address |
||||||
|
("SPEED_L", "MOTORS_DATA"), |
||||||
|
("SPEED_R", "MOTORS_DATA"), |
||||||
|
("ELEC_ANGLE_L", "MOTORS_DATA"), |
||||||
|
("ELEC_ANGLE_R", "MOTORS_DATA"), |
||||||
|
("MOTOR_ERR_L", "MOTORS_DATA"), |
||||||
|
("MOTOR_ERR_R", "MOTORS_DATA"), |
||||||
|
("IGNITION", "VAR_VALUES"), |
||||||
|
("ENABLE_MOTORS", "VAR_VALUES"), |
||||||
|
("MCU_TEMP", "BODY_DATA"), |
||||||
|
("BATT_VOLTAGE", "BODY_DATA"), |
||||||
|
] |
||||||
|
|
||||||
|
checks = [ |
||||||
|
("MOTORS_DATA", 100), |
||||||
|
("VAR_VALUES", 10), |
||||||
|
("BODY_DATA", 1), |
||||||
|
] |
||||||
|
|
||||||
|
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) |
||||||
@ -0,0 +1,46 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
from cereal import car |
||||||
|
from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, get_safety_config |
||||||
|
from selfdrive.car.interfaces import CarInterfaceBase |
||||||
|
|
||||||
|
class CarInterface(CarInterfaceBase): |
||||||
|
@staticmethod |
||||||
|
def get_params(candidate, fingerprint=None, car_fw=None, disable_radar=False): |
||||||
|
|
||||||
|
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||||
|
|
||||||
|
ret.carName = "body" |
||||||
|
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] |
||||||
|
|
||||||
|
ret.steerRatio = 0.5 |
||||||
|
ret.steerRateCost = 0.5 |
||||||
|
ret.steerLimitTimer = 1.0 |
||||||
|
ret.steerActuatorDelay = 0. |
||||||
|
|
||||||
|
ret.mass = 9 |
||||||
|
ret.wheelbase = 0.406 |
||||||
|
ret.wheelSpeedFactor = 0.008587 |
||||||
|
ret.centerToFront = ret.wheelbase * 0.44 |
||||||
|
|
||||||
|
ret.radarOffCan = True |
||||||
|
ret.openpilotLongitudinalControl = True |
||||||
|
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||||
|
|
||||||
|
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
||||||
|
|
||||||
|
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) |
||||||
|
|
||||||
|
return ret |
||||||
|
|
||||||
|
def update(self, c, can_strings): |
||||||
|
self.cp.update_strings(can_strings) |
||||||
|
|
||||||
|
ret = self.CS.update(self.cp) |
||||||
|
|
||||||
|
ret.canValid = self.cp.can_valid |
||||||
|
|
||||||
|
self.CS.out = ret.as_reader() |
||||||
|
return self.CS.out |
||||||
|
|
||||||
|
def apply(self, c): |
||||||
|
return self.CC.update(c, self.CS) |
||||||
@ -0,0 +1,5 @@ |
|||||||
|
#!/usr/bin/env python3 |
||||||
|
from selfdrive.car.interfaces import RadarInterfaceBase |
||||||
|
|
||||||
|
class RadarInterface(RadarInterfaceBase): |
||||||
|
pass |
||||||
@ -0,0 +1,33 @@ |
|||||||
|
from typing import Dict |
||||||
|
|
||||||
|
from cereal import car |
||||||
|
from selfdrive.car import dbc_dict |
||||||
|
from selfdrive.car.docs_definitions import CarInfo |
||||||
|
Ecu = car.CarParams.Ecu |
||||||
|
|
||||||
|
|
||||||
|
class CarControllerParams: |
||||||
|
ANGLE_DELTA_BP = [0., 5., 15.] |
||||||
|
ANGLE_DELTA_V = [5., .8, .15] # windup limit |
||||||
|
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit |
||||||
|
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower |
||||||
|
STEER_THRESHOLD = 1.0 |
||||||
|
|
||||||
|
class CAR: |
||||||
|
BODY = "COMMA BODY" |
||||||
|
|
||||||
|
CAR_INFO: Dict[str, CarInfo] = { |
||||||
|
CAR.BODY: CarInfo("comma body", package="All", good_torque=True), |
||||||
|
} |
||||||
|
|
||||||
|
FW_VERSIONS = { |
||||||
|
CAR.BODY: { |
||||||
|
(Ecu.engine, 0x720, None): [ |
||||||
|
b'02/27/2022' |
||||||
|
], |
||||||
|
}, |
||||||
|
} |
||||||
|
|
||||||
|
DBC = { |
||||||
|
CAR.BODY: dbc_dict('comma_body', None), |
||||||
|
} |
||||||
@ -1 +1 @@ |
|||||||
98d82b2da600755b12e9ecb2744bf4a8b0942ca1 |
6c72ebf4ef60b895bde5f0700babf150fd97d523 |
||||||
Loading…
Reference in new issue