Skip to content

Commit

Permalink
Squashed commit of the following:
Browse files Browse the repository at this point in the history 
commit 19e340e
Author: dzid26 <dzidmail@gmail.com>
Date:   Fri Nov 8 17:37:42 2024 +0000

    relax angle limit to E9x

commit 313ee80
Author: dzid26 <dzidmail@gmail.com>
Date:   Fri Nov 8 17:36:32 2024 +0000

    EU guidlines angle limits for E8x

commit c313e72
Author: dzid26 <dzidmail@gmail.com>
Date:   Fri Nov 8 01:11:22 2024 +0000

    check lateral acc signal from the car

commit 84b6e0d
Author: dzid26 <dzidmail@gmail.com>
Date:   Thu Nov 7 16:54:04 2024 +0000

    speed is signed

commit 2786bd0
Author: dzid26 <dzidmail@gmail.com>
Date:   Thu Nov 7 00:00:03 2024 +0000

    Added .max_counter rx check

commit e57c3b5
Author: dzid26 <dzidmail@gmail.com>
Date:   Wed Nov 6 22:14:03 2024 +0000

    consolidate BMW_Speed rx
  • Loading branch information
@dzid26
dzid26 committed Nov 9, 2024
1 parent d233518 commit 19e123d
Showing 1 changed file with 39 additions and 17 deletions.
56 changes: 39 additions & 17 deletions board/safety/safety_bmw.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
#define BMW_EngineAndBrake 0xA8
#define BMW_AccPedal 0xAA
#define BMW_Speed 0x1A0
#define BMW_SteeringWheelAngl_slow 0xC8
#define BMW_SteeringWheelAngle_slow 0xC8
#define BMW_CruiseControlStatus 0x200
#define BMW_DynamicCruiseControlStatus 0x193
#define BMW_CruiseControlStalk 0x194
Expand All @@ -13,12 +13,12 @@
#define BMW_AUX_CAN 2


RxCheck bmw_rx_checks[] = { // todo add .check_checksum and .max_counter
{.msg = {{BMW_EngineAndBrake, BMW_PT_CAN, 8, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{BMW_AccPedal, BMW_PT_CAN, 8, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{BMW_Speed, BMW_PT_CAN, 8, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{BMW_SteeringWheelAngl_slow, BMW_PT_CAN, 6, .frequency = 5U}, { 0 }, { 0 }}},
{.msg = {{BMW_TransmissionDataDisplay, BMW_PT_CAN, 6, .frequency = 5U}, { 0 }, { 0 }}},
RxCheck bmw_rx_checks[] = { // todo add .check_checksum
{.msg = {{BMW_EngineAndBrake, BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{BMW_AccPedal, BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 100U}, { 0 }, { 0 }}},
{.msg = {{BMW_Speed, BMW_PT_CAN, 8, .max_counter = 14U, .frequency = 50U}, { 0 }, { 0 }}},
{.msg = {{BMW_SteeringWheelAngle_slow, BMW_PT_CAN, 6, .max_counter = 0U, .frequency = 5U}, { 0 }, { 0 }}},
{.msg = {{BMW_TransmissionDataDisplay, BMW_PT_CAN, 6, .max_counter = 14U, .frequency = 5U}, { 0 }, { 0 }}},
// todo cruise control type dependant, use param:
// {.msg = {{BMW_CruiseControlStatus, BMW_PT_CAN, 8, .frequency = 5U}, { 0 }, { 0 }}},
// {.msg = {{BMW_DynamicCruiseControlStatus, BMW_F_CAN, 7, .frequency = 5U}, { 0 }, { 0 }}},
Expand All @@ -27,6 +27,19 @@ RxCheck bmw_rx_checks[] = { // todo add .check_checksum and .max_counter
};


static uint8_t bmw_get_counter(const CANPacket_t *to_push) {
uint8_t cnt = 0;
int addr = GET_ADDR(to_push);
if (addr == BMW_TransmissionDataDisplay) {
cnt = (GET_BYTE(to_push, 3) >> 4) & 0xFU;
} else if (addr == BMW_Speed) {
cnt = (GET_BYTE(to_push, 6) >> 4) & 0xFU;
} else {
cnt = GET_BYTE(to_push, 1) & 0xFU;
}
return cnt;
}

const CanMsg BMW_TX_MSGS[] = {
{BMW_CruiseControlStalk, BMW_PT_CAN, 4}, // Normal cruise control send status on PT-CAN
{BMW_CruiseControlStalk, BMW_F_CAN, 4}, // Dynamic cruise control send status on F-CAN
Expand All @@ -38,6 +51,7 @@ const CanMsg BMW_TX_MSGS[] = {

#define CAN_BMW_SPEED_FAC 0.1
#define CAN_BMW_ANGLE_FAC 0.04395
#define CAN_BMW_ACC_FAC 0.025
#define CAN_ACTUATOR_POS_FAC 0.125
#define CAN_ACTUATOR_TQ_FAC 0.125
#define CAN_ACTUATOR_CONTROL_STATUS_SOFTOFF_BIT 2
Expand All @@ -49,9 +63,12 @@ bool bmw_fmax_limit_check(float val, const float MAX_VAL, const float MIN_VAL) {
// rounding error margin
float BMW_MARGIN = 0.1;

#define BMW_LAT_ACC_MAX 3.0 // EU guideline

// steering angle based on EU 3m/s2 lat acc limit for 2.76m wheelbase and 16.0 steer ratio
const struct lookup_t BMW_LOOKUP_MAX_ANGLE = {
{5., 15., 30.}, // m/s
{200., 20., 10.}}; // deg
{5., 15., 25.}, // m/s
{303.6, 33.7, 12.1}}; // deg


const struct lookup_t BMW_ANGLE_RATE_WINDUP = { // deg/s windup rate limit
Expand Down Expand Up @@ -115,11 +132,21 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {

//get vehicle speed
if (addr == BMW_Speed) {
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
bmw_speed = to_signed(((GET_BYTE(to_push, 1) & 0xF) << 8) + GET_BYTE(to_push, 0), 12) * 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;

// check moving forward and reverse
vehicle_moving = (GET_BYTE(to_push, 1) & 0x30U) != 0U;

// check lateral acceleration limits
float bmw_lat_acc = to_signed((GET_BYTE(to_push, 4) << 4) | (GET_BYTE(to_push, 3) >> 4), 12) * CAN_BMW_ACC_FAC;
if (ABS(bmw_lat_acc) > BMW_LAT_ACC_MAX) {
print("Too big lateral acc \n");
controls_allowed = false;
}
}

// STEPPER_SERVO_CAN: get STEERING_STATUS
Expand All @@ -133,7 +160,7 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
}

//get latest steering wheel angle rate
if ((addr == BMW_SteeringWheelAngl_slow) && (bus == BMW_PT_CAN)) {
if ((addr == BMW_SteeringWheelAngle_slow) && (bus == BMW_PT_CAN)) {
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
// todo use common steer_angle_cmd_checks()
Expand All @@ -156,12 +183,6 @@ static void bmw_rx_hook(const CANPacket_t *to_push) {
bmw_rt_angle_last = meas_angle;
}

// Update in motion state from standstill signal
if (addr == BMW_Speed) {
// check moving forward and reverse
vehicle_moving = (GET_BYTE(to_push, 1) & 0x30U) != 0U;
}

// exit controls on brake press
if (addr == BMW_EngineAndBrake) {
// any of two bits at position 61 & 62
Expand Down Expand Up @@ -243,4 +264,5 @@ const safety_hooks bmw_hooks = {
.rx = bmw_rx_hook,
.tx = bmw_tx_hook,
.fwd = default_fwd_hook,
.get_counter = bmw_get_counter,
};

0 comments on commit 19e123d

Please sign in to comment.