bmw merged, removed gmlan switch, todo fix test

from: bbdb0ad

board/drivers/can_common.h

@@ -197,9 +197,10 @@ void can_set_forwarding(uint8_t from, uint8_t to) {
 
 void ignition_can_hook(CANPacket_t *to_push) {
   int bus = GET_BUS(to_push);
+  int addr = GET_ADDR(to_push);
+  int len = GET_LEN(to_push);
+
   if (bus == 0) {
-    int addr = GET_ADDR(to_push);
-    int len = GET_LEN(to_push);
 
     // GM exception
     if ((addr == 0x1F1) && (len == 8)) {
@@ -220,7 +221,14 @@ void ignition_can_hook(CANPacket_t *to_push) {
       ignition_can = (GET_BYTE(to_push, 0) >> 5) == 0x6U;
       ignition_can_cnt = 0U;
     }
+  }
 
+  if (bus == 1) {
+    // BMW case
+    if ((addr == 0x130) && (len == 5)) {
+      // bits 22 and 23 are zero when ignition is on 
+      ignition_can = (GET_BYTE(to_push, 0) & 0x04) !=0;
+    } 
   }
 }
 

board/safety.h

@@ -5,6 +5,7 @@
 #include "safety/safety_defaults.h"
 #include "safety/safety_honda.h"
 #include "safety/safety_toyota.h"
+#include "safety/safety_bmw.h"
 #include "safety/safety_tesla.h"
 #include "safety/safety_gm.h"
 #include "safety/safety_ford.h"
@@ -36,6 +37,7 @@
 #define SAFETY_CHRYSLER 9U
 #define SAFETY_TESLA 10U
 #define SAFETY_SUBARU 11U
+#define SAFETY_BMW 12U
 #define SAFETY_MAZDA 13U
 #define SAFETY_NISSAN 14U
 #define SAFETY_VOLKSWAGEN_MQB 15U
@@ -307,6 +309,7 @@ const safety_hook_config safety_hook_registry[] = {
   {SAFETY_SUBARU, &subaru_hooks},
   {SAFETY_VOLKSWAGEN_MQB, &volkswagen_mqb_hooks},
   {SAFETY_NISSAN, &nissan_hooks},
+  {SAFETY_BMW, &bmw_hooks },
   {SAFETY_NOOUTPUT, &nooutput_hooks},
   {SAFETY_HYUNDAI_LEGACY, &hyundai_legacy_hooks},
   {SAFETY_MAZDA, &mazda_hooks},

board/safety/safety_bmw.h

@@ -0,0 +1,230 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+const CanMsg BMW_TX_MSGS[] = {
+  {404, 0, 4},
+  {404, 1, 4},
+  {558, 2, 8},
+};
+
+int SAMPLING_FREQ = 100; //Hz
+
+#define KPH_TO_MS 0.277778
+
+#define CAN_BMW_SPEED_FAC 0.1
+#define CAN_BMW_ANGLE_FAC 0.04395
+#define CAN_ACTUATOR_POS_FAC 0.125
+#define CAN_ACTUATOR_TQ_FAC 0.125
+#define CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT 2
+
+bool bmw_fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) {
+  return (val > MAX_VAL) || (val < MIN_VAL);
+}
+
+// rounding error margin
+float BMW_MARGIN = 0.1;
+
+const struct lookup_t BMW_LOOKUP_MAX_ANGLE = {
+    {5., 15., 30.},     // m/s
+    {200., 20., 10.}};  // deg
+
+
+const struct lookup_t BMW_ANGLE_RATE_WINDUP = { // deg/s windup rate limit
+    {0., 5., 15.},      // m/s
+    {500., 80., 15.}};  // deg/s
+
+const struct lookup_t BMW_ANGLE_RATE_UNWIND = { // deg/s unwind rate limit
+    {0., 5., 15.},      // m/s
+    {500., 350., 40.}}; // deg/s
+
+const struct lookup_t BMW_MAX_TQ_RATE = {
+    {0., 5., 15.},      // m/s
+    {16., 8., 1.}};   // Nm/10ms
+
+const uint32_t BMW_RT_INTERVAL = 250000; // 250ms between real time checks
+
+// state of angle limits
+float bmw_desired_angle_last = 0; // last desired steer angle
+float bmw_rt_angle_last = 0.; // last actual angle
+
+float angle_rate_up = 0;
+float angle_rate_down = 0;
+float bmw_max_angle = 0;
+float max_tq_rate = 0;
+
+int bmw_controls_allowed_last = 0;
+
+int lever_position = -1; //0 is when no ignition, so -1 unset
+float bmw_speed = 0;
+float actuator_torque;
+
+bool cruise_engaged_last = 0;
+
+static void bmw_rx_hook(const CANPacket_t *to_push) {
+  int addr = GET_ADDR(to_push);
+  int bus = GET_BUS(to_push);
+
+  bool cruise_engaged = false;
+  if ((addr == 0x193) || (addr == 0x200)) { //handles both vehicle options VO544 and Vo540
+
+    if (addr == 0x193) { //dynamic cruise control
+      cruise_engaged = (((GET_BYTE(to_push, 5) >> 3) & 0x1U) == 1U);
+    } else if (addr == 0x200) { //normal cruise control option
+      cruise_engaged = (((GET_BYTE(to_push, 1) >> 5) & 0x1U) == 1U);
+    } else {
+      cruise_engaged = false;
+    }
+    if (!cruise_engaged) {
+      controls_allowed = false;
+    } 
+    if (cruise_engaged && !cruise_engaged_last) {
+      controls_allowed = true;
+    }
+    cruise_engaged_last = cruise_engaged;
+  }
+
+  if (addr == 404){ //disable on cruise stalk cancel
+    if ((GET_BYTE(to_push, 2) & 0x90) != 0x0){
+      controls_allowed = false;
+    }
+  }
+  if (addr == 466) { //TransmissionDataDisplay  
+    if ((GET_BYTE(to_push, 0) & 0xF) == ((GET_BYTE(to_push, 0) >> 4) ^ 0xF)) { //check agains shift lever compliment signal 
+      lever_position = GET_BYTE(to_push, 0) & 0xF; //compliment match
+    } else {
+      lever_position = -1; //invalid
+    }
+    // if not in Drive
+    if (lever_position != 8 ){
+      controls_allowed = false;
+    }
+  }
+
+  // exit controls on brake press
+  if (addr == 168) {
+    // any of two bits at position 61 & 62
+    if ((GET_BYTE(to_push, 7)  & 0x60) != 0) {
+      controls_allowed = false;
+    }
+  }
+  //get vehicle speed
+  if (addr == 416) {
+    bmw_speed = ((((GET_BYTE(to_push, 1) & 0xF) << 8) | GET_BYTE(to_push, 0)) * CAN_BMW_SPEED_FAC * KPH_TO_MS); //raw to km/h to m/s
+    angle_rate_up = interpolate(BMW_ANGLE_RATE_WINDUP, bmw_speed) + BMW_MARGIN;   // deg/1s
+    angle_rate_down = interpolate(BMW_ANGLE_RATE_UNWIND, bmw_speed) + BMW_MARGIN; // deg/1s
+    bmw_max_angle = interpolate(BMW_LOOKUP_MAX_ANGLE, bmw_speed) + BMW_MARGIN;
+    max_tq_rate = interpolate(BMW_MAX_TQ_RATE, bmw_speed) + BMW_MARGIN;
+  }
+
+  //get STEERING_STATUS
+  if ((addr == 559)  && (bus == 2)) {
+    actuator_torque = ((float)(int8_t)(GET_BYTE(to_push, 2))) * CAN_ACTUATOR_TQ_FAC; //Nm
+
+    if((((GET_BYTE(to_push, 1)>>4)>>CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT) & 0x1) != 0x0){ //Soft off status means motor is shutting down due to error
+      controls_allowed = false;
+      print("BMW soft off\n");
+    }
+  }
+
+  //get latest steering wheel angle rate
+  if ((addr == 196) && ((bus == 0) || (bus == 1))) {
+    float meas_angle = to_signed((GET_BYTE(to_push, 1) << 8) | GET_BYTE(to_push, 0), 16) * CAN_BMW_ANGLE_FAC; //deg
+    float angle_rate = to_signed((GET_BYTE(to_push, 4) << 8) | GET_BYTE(to_push, 3), 16) * CAN_BMW_ANGLE_FAC; //deg/s
+
+    if(bmw_fmax_limit_check(meas_angle, bmw_max_angle, -bmw_max_angle)){
+      // We should not be able to STEER under these conditions
+      controls_allowed = false;
+      if (cruise_engaged){
+        print("Too big angle \n");
+      }
+    }
+
+    if (bmw_fmax_limit_check((bmw_rt_angle_last >= 0.) ? angle_rate : -angle_rate, angle_rate_up, -angle_rate_down)) { //should be sensitive for jerks to the outside
+      controls_allowed = false;         // todo  ^ handle zero crossing a bit smarter
+      if (cruise_engaged){
+        print("To fast angle rate \n");
+      }
+    }
+
+    if ((controls_allowed && !bmw_controls_allowed_last)) {
+      bmw_rt_angle_last = meas_angle;
+    }
+  }
+
+  bmw_controls_allowed_last = controls_allowed;
+}
+
+// all commands: gas/regen, friction brake and steering
+// if controls_allowed and no pedals pressed
+//     allow all commands up to limit
+// else
+//     block all commands that produce actuation
+
+static bool bmw_tx_hook(const CANPacket_t *to_send) {
+
+  bool tx = true;
+  int addr = GET_ADDR(to_send);
+
+  // do not transmit CAN message if steering angle too high
+  if (addr == 558) {
+    if (((GET_BYTE(to_send, 1) >> 4) & 0b11u) != 0x0){ //control enabled
+      float steer_torque = ((float)(int8_t)(GET_BYTE(to_send, 4))) * CAN_ACTUATOR_TQ_FAC; //Nm
+      if (bmw_fmax_limit_check(steer_torque - actuator_torque, max_tq_rate, -max_tq_rate)){
+        print("Violation torque rate");
+        // printf("Tq: %f, ActTq: %f, Max: %f\n", steer_torque, actuator_torque, max_tq_rate);
+        tx = false;
+      }
+    }
+    float desired_angle = 0;
+    if (((GET_BYTE(to_send, 1) >> 4) & 0b11u) == 0x2){ //position control enabled
+      float angle_delta_req = ((float)(int16_t)((GET_BYTE(to_send, 2)) | (GET_BYTE(to_send, 3) << 8))) * CAN_ACTUATOR_POS_FAC; //deg/10ms
+      desired_angle = bmw_rt_angle_last + angle_delta_req; //measured + requested delta
+
+      if (controls_allowed == true) {
+        bool violation = false;
+        //check for max angles
+        violation |= bmw_fmax_limit_check(desired_angle, bmw_max_angle, -bmw_max_angle);
+        print("Violation desired angle");
+        //angle is rate limited in carcontrols so it shouldn't exceed max delta
+        float angle_delta_req_side = (bmw_desired_angle_last >= 0.) ? angle_delta_req : -angle_delta_req;
+        violation |= bmw_fmax_limit_check(angle_delta_req_side, angle_rate_up, -angle_rate_down);
+        print("Violation  delta");
+
+        if (violation) {
+          tx = false;
+          desired_angle = bmw_desired_angle_last; //nothing was sent - hold to previous
+        }
+      }
+    }
+    bmw_desired_angle_last = desired_angle;
+  }
+  if(controls_allowed == false){
+    tx = false;
+  }
+
+  return tx;
+}
+
+static safety_config bmw_init(uint16_t param) {
+  UNUSED(param);
+  controls_allowed = false;
+  bmw_speed = 0;
+  lever_position = -1;
+
+  safety_config ret;
+  SET_TX_MSGS(BMW_TX_MSGS, ret); //todo different depending DSC vehicle option
+  // todo add RX checks
+
+  #ifdef ALLOW_DEBUG
+    print("BMW safety init\n");
+  #endif
+
+  return ret;
+}
+
+const safety_hooks bmw_hooks = {
+  .init = bmw_init,
+  .rx = bmw_rx_hook,
+  .tx = bmw_tx_hook,
+  .fwd = default_fwd_hook,
+};

python/__init__.py

@@ -126,6 +126,7 @@ class Panda:
   SAFETY_CHRYSLER = 9
   SAFETY_TESLA = 10
   SAFETY_SUBARU = 11
+  SAFETY_BMW = 12
   SAFETY_MAZDA = 13
   SAFETY_NISSAN = 14
   SAFETY_VOLKSWAGEN_MQB = 15

python/set_safety.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import time
+
+sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), ".."))
+from panda import Panda
+import _thread
+
+def set_safety(id):
+  p = Panda()
+  p.set_safety_mode(id)
+  start_heartbeat_thread(p)
+  input('Press key to stop')
+
+
+def start_heartbeat_thread(p):
+  def heartbeat_thread(p):
+    while True:
+      try:
+        p.send_heartbeat()
+        p.set_power_save(False)
+        time.sleep(1)
+      except:
+        break
+  _thread.start_new_thread(heartbeat_thread, (p,))
+
+if __name__ == "__main__":
+  safety_id = int(os.getenv("SAFETY", 0))  #run in console ./set_safety.py 1 for Honda
+  set_safety(12)