Compare commits
30 Commits
master
...
comma_body
Author | SHA1 | Date |
---|---|---|
![]() |
1771bc4813 | |
![]() |
cc43073da4 | |
![]() |
15109d98ab | |
![]() |
3ad445822d | |
![]() |
09d06e4b89 | |
![]() |
7a6847289c | |
![]() |
4bc46bc156 | |
![]() |
c1736a10b5 | |
![]() |
139f967f21 | |
![]() |
400c3d9d63 | |
![]() |
79b2cf5c46 | |
![]() |
30e25abcfd | |
![]() |
094ae151a4 | |
![]() |
8615a21ef0 | |
![]() |
2c91ee2de8 | |
![]() |
60511e1370 | |
![]() |
c6e3d19a39 | |
![]() |
30e71de05d | |
![]() |
3a94a1771f | |
![]() |
c9bceee358 | |
![]() |
a2d8b0c0ff | |
![]() |
87789e5234 | |
![]() |
ff7b87ed17 | |
![]() |
9e7dc3a3f6 | |
![]() |
0d614f5d4e | |
![]() |
71f38396b8 | |
![]() |
d9bad43558 | |
![]() |
f79ec09cd7 | |
![]() |
0d6f389ffc | |
![]() |
61794f4cae |
|
@ -77,6 +77,7 @@ How We Rate The Cars
|
||||||
|Audi|Q2 2018|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Audi|Q2 2018|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|Audi|Q3 2020-21|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Audi|Q3 2020-21|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|Audi|S3 2015-17|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Audi|S3 2015-17|ACC + Lane Assist|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|
|Comma|Body|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|
|
||||||
|Genesis|G70 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Genesis|G70 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|Genesis|G80 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Genesis|G80 2018|All|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|Hyundai|Elantra 2021-22|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
|Hyundai|Elantra 2021-22|SCC + LKAS|<a href="#"><img valign="top" src="assets/icon-star-empty.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||||
|
|
2
opendbc
2
opendbc
|
@ -1 +1 @@
|
||||||
Subproject commit eab58f6f8f41f255920365ab1fd9c75e312a6869
|
Subproject commit 0802017cb3e49a612a9832701f716cc0385af656
|
|
@ -115,7 +115,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
|
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ALL_OUTPUT);
|
||||||
|
|
||||||
Params p = Params();
|
Params p = Params();
|
||||||
|
|
||||||
|
@ -135,7 +135,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
|
||||||
util::sleep_for(20);
|
util::sleep_for(20);
|
||||||
}
|
}
|
||||||
|
|
||||||
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
|
pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ALL_OUTPUT);
|
||||||
|
|
||||||
std::string params;
|
std::string params;
|
||||||
LOGW("waiting for params to set safety model");
|
LOGW("waiting for params to set safety model");
|
||||||
|
|
|
@ -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,110 @@
|
||||||
|
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.CP = CP
|
||||||
|
self.car_fingerprint = CP.carFingerprint
|
||||||
|
|
||||||
|
self.lkas_max_torque = 0
|
||||||
|
self.last_angle = 0
|
||||||
|
|
||||||
|
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, frame, actuators, cruise_cancel, hud_alert,
|
||||||
|
left_line, right_line, left_lane_depart, right_lane_depart):
|
||||||
|
|
||||||
|
actuators = CC.actuators
|
||||||
|
|
||||||
|
# ///////////////////////////////////////
|
||||||
|
# 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((actuators.accel*1000) - (actuators.steer*1000) * self.steerRatio, -1000, 1000))
|
||||||
|
# torque_l = int(np.clip((actuators.accel*1000) + (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 = []
|
||||||
|
|
||||||
|
apply_angle = actuators.steeringAngleDeg
|
||||||
|
|
||||||
|
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
|
||||||
|
|
||||||
|
new_actuators = actuators.copy()
|
||||||
|
new_actuators.steeringAngleDeg = apply_angle
|
||||||
|
new_actuators.accel = torque_l
|
||||||
|
new_actuators.steer = torque_r
|
||||||
|
|
||||||
|
return new_actuators, can_sends
|
|
@ -0,0 +1,58 @@
|
||||||
|
from cereal import car
|
||||||
|
from selfdrive.car.interfaces import CarStateBase
|
||||||
|
from opendbc.can.parser import CANParser
|
||||||
|
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
|
||||||
|
|
||||||
|
ret.cruiseState.enabled = True
|
||||||
|
ret.cruiseState.available = True
|
||||||
|
ret.seatbeltUnlatched = False
|
||||||
|
|
||||||
|
ret.cruiseState.speed = 0
|
||||||
|
ret.steeringTorque = 0
|
||||||
|
|
||||||
|
ret.steeringPressed = False
|
||||||
|
|
||||||
|
ret.steeringAngleDeg = 0
|
||||||
|
|
||||||
|
ret.doorOpen = False
|
||||||
|
|
||||||
|
ret.gearShifter = self.parse_gear_shifter("D")
|
||||||
|
|
||||||
|
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,56 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
from cereal import car
|
||||||
|
from selfdrive.car import scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
|
||||||
|
from selfdrive.car.interfaces import CarInterfaceBase
|
||||||
|
|
||||||
|
class CarInterface(CarInterfaceBase):
|
||||||
|
@staticmethod
|
||||||
|
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False):
|
||||||
|
|
||||||
|
ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
|
||||||
|
ret.carName = "body"
|
||||||
|
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.allOutput)]
|
||||||
|
|
||||||
|
ret.steerLimitTimer = 1.0
|
||||||
|
ret.steerRateCost = 0.5
|
||||||
|
|
||||||
|
ret.steerActuatorDelay = 0.
|
||||||
|
|
||||||
|
ret.mass = 9
|
||||||
|
ret.wheelbase = 0.406
|
||||||
|
ret.wheelSpeedFactor = 0.008587
|
||||||
|
ret.centerToFront = ret.wheelbase * 0.44
|
||||||
|
ret.steerRatio = 0.5
|
||||||
|
|
||||||
|
ret.steerControlType = car.CarParams.SteerControlType.angle
|
||||||
|
ret.radarOffCan = False
|
||||||
|
ret.openpilotLongitudinalControl = True
|
||||||
|
|
||||||
|
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
|
||||||
|
|
||||||
|
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront)
|
||||||
|
|
||||||
|
ret.minEnableSpeed = -1.
|
||||||
|
|
||||||
|
return ret
|
||||||
|
|
||||||
|
# returns a car.CarState
|
||||||
|
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):
|
||||||
|
hud_control = c.hudControl
|
||||||
|
ret = self.CC.update(c, self.CS, self.frame, c.actuators,
|
||||||
|
c.cruiseControl.cancel, hud_control.visualAlert,
|
||||||
|
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||||
|
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||||
|
self.frame += 1
|
||||||
|
return ret
|
|
@ -0,0 +1,5 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||||
|
|
||||||
|
class RadarInterface(RadarInterfaceBase):
|
||||||
|
pass
|
|
@ -0,0 +1,39 @@
|
||||||
|
from typing import Dict, List, Union
|
||||||
|
from dataclasses import dataclass
|
||||||
|
|
||||||
|
from selfdrive.car import dbc_dict
|
||||||
|
from selfdrive.car.docs_definitions import CarInfo
|
||||||
|
from cereal import car
|
||||||
|
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"
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class CommaCarInfo(CarInfo):
|
||||||
|
package: str = "All"
|
||||||
|
good_torque: bool = True
|
||||||
|
|
||||||
|
CAR_INFO: Dict[str, Union[CommaCarInfo, List[CommaCarInfo]]] = {
|
||||||
|
CAR.BODY: CommaCarInfo("Comma Body"),
|
||||||
|
}
|
||||||
|
|
||||||
|
FW_VERSIONS = {
|
||||||
|
CAR.BODY: {
|
||||||
|
(Ecu.engine, 0x720, None): [
|
||||||
|
b'02/27/2022'
|
||||||
|
],
|
||||||
|
},
|
||||||
|
}
|
||||||
|
|
||||||
|
DBC = {
|
||||||
|
CAR.BODY: dbc_dict('comma_body', None),
|
||||||
|
}
|
|
@ -94,7 +94,7 @@ def fingerprint(logcan, sendcan):
|
||||||
|
|
||||||
if not fixed_fingerprint and not skip_fw_query:
|
if not fixed_fingerprint and not skip_fw_query:
|
||||||
# Vin query only reliably works thorugh OBDII
|
# Vin query only reliably works thorugh OBDII
|
||||||
bus = 1
|
bus = 0
|
||||||
|
|
||||||
cached_params = Params().get("CarParamsCache")
|
cached_params = Params().get("CarParamsCache")
|
||||||
if cached_params is not None:
|
if cached_params is not None:
|
||||||
|
|
|
@ -92,6 +92,13 @@ SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40
|
||||||
|
|
||||||
# brand, request, response, response offset
|
# brand, request, response, response offset
|
||||||
REQUESTS = [
|
REQUESTS = [
|
||||||
|
# Body
|
||||||
|
(
|
||||||
|
"body",
|
||||||
|
[TESTER_PRESENT_REQUEST, UDS_VERSION_REQUEST],
|
||||||
|
[TESTER_PRESENT_RESPONSE, UDS_VERSION_RESPONSE],
|
||||||
|
DEFAULT_RX_OFFSET,
|
||||||
|
),
|
||||||
# Subaru
|
# Subaru
|
||||||
(
|
(
|
||||||
"subaru",
|
"subaru",
|
||||||
|
|
|
@ -11,6 +11,7 @@ from selfdrive.car.subaru.values import CAR as SUBARU
|
||||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||||
from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
|
from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN
|
||||||
from selfdrive.car.tesla.values import CAR as TESLA
|
from selfdrive.car.tesla.values import CAR as TESLA
|
||||||
|
from selfdrive.car.body.values import CAR as COMMA
|
||||||
|
|
||||||
# TODO: add routes for these cars
|
# TODO: add routes for these cars
|
||||||
non_tested_cars = [
|
non_tested_cars = [
|
||||||
|
@ -20,6 +21,7 @@ non_tested_cars = [
|
||||||
HYUNDAI.ELANTRA_GT_I30,
|
HYUNDAI.ELANTRA_GT_I30,
|
||||||
HYUNDAI.GENESIS_G90,
|
HYUNDAI.GENESIS_G90,
|
||||||
HYUNDAI.KIA_OPTIMA_H,
|
HYUNDAI.KIA_OPTIMA_H,
|
||||||
|
COMMA.BODY,
|
||||||
]
|
]
|
||||||
|
|
||||||
TestRoute = namedtuple('TestRoute', ['route', 'car_fingerprint'])
|
TestRoute = namedtuple('TestRoute', ['route', 'car_fingerprint'])
|
||||||
|
|
|
@ -196,6 +196,9 @@ VectorXd Localizer::get_stdev() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
|
void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
|
||||||
|
MatrixXdr ecef_vel_R = Vector3d::Constant(1. * 1.).asDiagonal();
|
||||||
|
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { Vector3d(0., 0., 0.) }, { ecef_vel_R });
|
||||||
|
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
|
||||||
// TODO does not yet account for double sensor readings in the log
|
// TODO does not yet account for double sensor readings in the log
|
||||||
for (int i = 0; i < log.size(); i++) {
|
for (int i = 0; i < log.size(); i++) {
|
||||||
const cereal::SensorEventData::Reader& sensor_reading = log[i];
|
const cereal::SensorEventData::Reader& sensor_reading = log[i];
|
||||||
|
@ -443,14 +446,14 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
||||||
this->time_check(t);
|
this->time_check(t);
|
||||||
if (log.isSensorEvents()) {
|
if (log.isSensorEvents()) {
|
||||||
this->handle_sensors(t, log.getSensorEvents());
|
this->handle_sensors(t, log.getSensorEvents());
|
||||||
} else if (log.isGpsLocationExternal()) {
|
// } else if (log.isGpsLocationExternal()) {
|
||||||
this->handle_gps(t, log.getGpsLocationExternal());
|
// this->handle_gps(t, log.getGpsLocationExternal());
|
||||||
} else if (log.isCarState()) {
|
// } else if (log.isCarState()) {
|
||||||
this->handle_car_state(t, log.getCarState());
|
// this->handle_car_state(t, log.getCarState());
|
||||||
} else if (log.isCameraOdometry()) {
|
// } else if (log.isCameraOdometry()) {
|
||||||
this->handle_cam_odo(t, log.getCameraOdometry());
|
// this->handle_cam_odo(t, log.getCameraOdometry());
|
||||||
} else if (log.isLiveCalibration()) {
|
// } else if (log.isLiveCalibration()) {
|
||||||
this->handle_live_calib(t, log.getLiveCalibration());
|
// this->handle_live_calib(t, log.getLiveCalibration());
|
||||||
}
|
}
|
||||||
this->finite_check();
|
this->finite_check();
|
||||||
this->update_reset_tracker();
|
this->update_reset_tracker();
|
||||||
|
@ -495,44 +498,24 @@ int Localizer::locationd_thread() {
|
||||||
const std::initializer_list<const char *> service_list =
|
const std::initializer_list<const char *> service_list =
|
||||||
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
|
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
|
||||||
PubMaster pm({ "liveLocationKalman" });
|
PubMaster pm({ "liveLocationKalman" });
|
||||||
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
|
SubMaster sm(service_list, nullptr, { "sensorEvents" });
|
||||||
|
|
||||||
uint64_t cnt = 0;
|
bool filterInitialized = true;
|
||||||
bool filterInitialized = false;
|
|
||||||
|
|
||||||
while (!do_exit) {
|
while (!do_exit) {
|
||||||
sm.update();
|
sm.update();
|
||||||
if (filterInitialized){
|
for (const char* service : service_list) {
|
||||||
for (const char* service : service_list) {
|
if (sm.updated(service) && sm.valid(service)) {
|
||||||
if (sm.updated(service) && sm.valid(service)){
|
const cereal::Event::Reader log = sm[service];
|
||||||
const cereal::Event::Reader log = sm[service];
|
this->handle_msg(log);
|
||||||
this->handle_msg(log);
|
MessageBuilder msg_builder;
|
||||||
}
|
uint64_t logMonoTime = sm["sensorEvents"].getLogMonoTime();
|
||||||
|
bool inputsOK = 1;
|
||||||
|
bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
|
||||||
|
bool gpsOK = 0;
|
||||||
|
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||||
|
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||||
}
|
}
|
||||||
} else {
|
|
||||||
filterInitialized = sm.allAliveAndValid();
|
|
||||||
}
|
|
||||||
|
|
||||||
if (sm.updated("cameraOdometry")) {
|
|
||||||
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
|
|
||||||
bool inputsOK = sm.allAliveAndValid();
|
|
||||||
bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
|
|
||||||
bool gpsOK = this->isGpsOK();
|
|
||||||
|
|
||||||
MessageBuilder msg_builder;
|
|
||||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
|
||||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
|
||||||
|
|
||||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
|
||||||
VectorXd posGeo = this->get_position_geodetic();
|
|
||||||
std::string lastGPSPosJSON = util::string_format(
|
|
||||||
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
|
|
||||||
|
|
||||||
std::thread([] (const std::string gpsjson) {
|
|
||||||
Params().put("LastGPSPosition", gpsjson);
|
|
||||||
}, lastGPSPosJSON).detach();
|
|
||||||
}
|
|
||||||
cnt++;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return 0;
|
return 0;
|
||||||
|
|
|
@ -78,7 +78,7 @@ class LiveKalman():
|
||||||
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
|
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
|
||||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
|
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
|
||||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
|
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||||
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
|
ObservationKind.NO_ROT: np.array([10**2, 10**2, .1**2]),
|
||||||
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
|
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||||
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
|
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
|
||||||
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
|
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
|
||||||
|
|
|
@ -45,7 +45,7 @@ segments = [
|
||||||
]
|
]
|
||||||
|
|
||||||
# dashcamOnly makes don't need to be tested until a full port is done
|
# dashcamOnly makes don't need to be tested until a full port is done
|
||||||
excluded_interfaces = ["mock", "ford", "mazda", "tesla"]
|
excluded_interfaces = ["mock", "ford", "mazda", "tesla", "body"]
|
||||||
|
|
||||||
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
|
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
|
||||||
|
|
||||||
|
|
|
@ -2,6 +2,7 @@ moc_*
|
||||||
*.moc
|
*.moc
|
||||||
|
|
||||||
_mui
|
_mui
|
||||||
|
eyes
|
||||||
watch3
|
watch3
|
||||||
installer/installers/*
|
installer/installers/*
|
||||||
replay/replay
|
replay/replay
|
||||||
|
|
Loading…
Reference in New Issue