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