Forgot to define angle

docs/CARS.md

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

opendbc

@@ -1 +1 @@
-Subproject commit eab58f6f8f41f255920365ab1fd9c75e312a6869
+Subproject commit 0802017cb3e49a612a9832701f716cc0385af656

selfdrive/boardd/boardd.cc

@@ -115,7 +115,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
     return false;
   }
 
-  pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
+  pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ALL_OUTPUT);
 
   Params p = Params();
 
@@ -135,7 +135,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
     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;
   LOGW("waiting for params to set safety model");

selfdrive/car/body/bodycan.py

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

selfdrive/car/body/carcontroller.py

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

selfdrive/car/body/carstate.py

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

selfdrive/car/body/interface.py

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

selfdrive/car/body/radar_interface.py

@@ -0,0 +1,5 @@
+#!/usr/bin/env python3
+from selfdrive.car.interfaces import RadarInterfaceBase
+
+class RadarInterface(RadarInterfaceBase):
+  pass

selfdrive/car/body/values.py

@@ -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),
+}

selfdrive/car/car_helpers.py

@@ -94,7 +94,7 @@ def fingerprint(logcan, sendcan):
 
   if not fixed_fingerprint and not skip_fw_query:
     # Vin query only reliably works thorugh OBDII
-    bus = 1
+    bus = 0
 
     cached_params = Params().get("CarParamsCache")
     if cached_params is not None:

selfdrive/car/fw_versions.py

@@ -92,6 +92,13 @@ SUBARU_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40
 
 # brand, request, response, response offset
 REQUESTS = [
+  # Body
+  (
+    "body",
+    [TESTER_PRESENT_REQUEST, UDS_VERSION_REQUEST],
+    [TESTER_PRESENT_RESPONSE, UDS_VERSION_RESPONSE],
+    DEFAULT_RX_OFFSET,
+  ),
   # Subaru
   (
     "subaru",

selfdrive/car/tests/routes.py

@@ -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.volkswagen.values import CAR as VOLKSWAGEN
 from selfdrive.car.tesla.values import CAR as TESLA
+from selfdrive.car.body.values import CAR as COMMA
 
 # TODO: add routes for these cars
 non_tested_cars = [
@@ -20,6 +21,7 @@ non_tested_cars = [
   HYUNDAI.ELANTRA_GT_I30,
   HYUNDAI.GENESIS_G90,
   HYUNDAI.KIA_OPTIMA_H,
+  COMMA.BODY,
 ]
 
 TestRoute = namedtuple('TestRoute', ['route', 'car_fingerprint'])

selfdrive/locationd/locationd.cc

@@ -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) {
+  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
   for (int i = 0; i < log.size(); 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);
   if (log.isSensorEvents()) {
     this->handle_sensors(t, log.getSensorEvents());
-  } else if (log.isGpsLocationExternal()) {
-    this->handle_gps(t, log.getGpsLocationExternal());
-  } else if (log.isCarState()) {
-    this->handle_car_state(t, log.getCarState());
-  } else if (log.isCameraOdometry()) {
-    this->handle_cam_odo(t, log.getCameraOdometry());
-  } else if (log.isLiveCalibration()) {
-    this->handle_live_calib(t, log.getLiveCalibration());
+  // } else if (log.isGpsLocationExternal()) {
+  //   this->handle_gps(t, log.getGpsLocationExternal());
+  // } else if (log.isCarState()) {
+  //   this->handle_car_state(t, log.getCarState());
+  // } else if (log.isCameraOdometry()) {
+  //   this->handle_cam_odo(t, log.getCameraOdometry());
+  // } else if (log.isLiveCalibration()) {
+  //   this->handle_live_calib(t, log.getLiveCalibration());
   }
   this->finite_check();
   this->update_reset_tracker();
@@ -495,44 +498,24 @@ int Localizer::locationd_thread() {
   const std::initializer_list<const char *> service_list =
       { "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
   PubMaster pm({ "liveLocationKalman" });
-  SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
+  SubMaster sm(service_list, nullptr, { "sensorEvents" });
 
-  uint64_t cnt = 0;
-  bool filterInitialized = false;
+  bool filterInitialized = true;
 
   while (!do_exit) {
     sm.update();
-    if (filterInitialized){
-      for (const char* service : service_list) {
-        if (sm.updated(service) && sm.valid(service)){
-          const cereal::Event::Reader log = sm[service];
-          this->handle_msg(log);
-        }
+    for (const char* service : service_list) {
+      if (sm.updated(service) && sm.valid(service)) {
+        const cereal::Event::Reader log = sm[service];
+        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;

selfdrive/locationd/models/live_kf.py

@@ -78,7 +78,7 @@ class LiveKalman():
   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.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.ECEF_POS: np.array([5**2, 5**2, 5**2]),
                     ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),

selfdrive/test/process_replay/test_processes.py

@@ -45,7 +45,7 @@ segments = [
 ]
 
 # 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/"
 

selfdrive/ui/.gitignore

@@ -2,6 +2,7 @@ moc_*
 *.moc
 
 _mui
+eyes
 watch3
 installer/installers/*
 replay/replay