Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 2 additions & 3 deletions opendbc/safety/modes/body.h
Original file line number Diff line number Diff line change
Expand Up @@ -3,9 +3,8 @@
#include "opendbc/safety/declarations.h"

static void body_rx_hook(const CANPacket_t *msg) {
if (msg->addr == 0x201U) {
controls_allowed = true;
}
SAFETY_UNUSED(msg);
controls_allowed = true;
}

static bool body_tx_hook(const CANPacket_t *msg) {
Expand Down
2 changes: 1 addition & 1 deletion opendbc/safety/modes/chrysler.h
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ static void chrysler_rx_hook(const CANPacket_t *msg) {
if ((chrysler_platform != CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == CHRYSLER_ADDR(ESP_8))) {
vehicle_moving = ((msg->data[4] << 8) + msg->data[5]) != 0U;
}
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->bus == 0U) && (msg->addr == 514U)) {
if ((chrysler_platform == CHRYSLER_PACIFICA) && (msg->addr == 514U)) {
int speed_l = (msg->data[0] << 4) + (msg->data[1] >> 4);
int speed_r = (msg->data[2] << 4) + (msg->data[3] >> 4);
vehicle_moving = (speed_l != 0) || (speed_r != 0);
Expand Down
45 changes: 22 additions & 23 deletions opendbc/safety/modes/chrysler_cusw.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,33 +22,32 @@ static safety_config chrysler_cusw_init(uint16_t param) {
}

static void chrysler_cusw_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
if (msg->addr == 0x1ECU) {
// Signal: EPS_STATUS.TORQUE_MOTOR
int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U;
update_sample(&torque_meas, torque_meas_new);
}
// The RX checks already validate the address, bus, and length.
if (msg->addr == 0x1ECU) {
// Signal: EPS_STATUS.TORQUE_MOTOR
int torque_meas_new = ((msg->data[3] & 0xFU) << 8) + msg->data[4] - 2048U;
update_sample(&torque_meas, torque_meas_new);
}

if (msg->addr == 0x2ECU) {
// Signal: ACC_CONTROL.ACC_ACTIVE
bool cruise_engaged = GET_BIT(msg, 7U);
pcm_cruise_check(cruise_engaged);
}
if (msg->addr == 0x2ECU) {
// Signal: ACC_CONTROL.ACC_ACTIVE
bool cruise_engaged = GET_BIT(msg, 7U);
pcm_cruise_check(cruise_engaged);
}

if (msg->addr == 0x1E4U) {
// Signal: BRAKE_1.VEHICLE_SPEED
vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U;
}
if (msg->addr == 0x1E4U) {
// Signal: BRAKE_1.VEHICLE_SPEED
vehicle_moving = (((msg->data[4] & 0x7U) << 8) + msg->data[5]) != 0U;
}

if (msg->addr == 0x1FEU) {
// Signal: ACCEL_GAS.GAS_HUMAN
gas_pressed = msg->data[1] != 0U;
}
if (msg->addr == 0x1FEU) {
// Signal: ACCEL_GAS.GAS_HUMAN
gas_pressed = msg->data[1] != 0U;
}

if (msg->addr == 0x1E8U) {
// Signal: BRAKE_3.DRIVER_BRAKE_SWITCH
brake_pressed = GET_BIT(msg, 18U);
}
if (msg->addr == 0x1E8U) {
// Signal: BRAKE_3.DRIVER_BRAKE_SWITCH
brake_pressed = GET_BIT(msg, 18U);
}
}

Expand Down
97 changes: 46 additions & 51 deletions opendbc/safety/modes/ford.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,9 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) {
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cnt
cnt = (msg->data[2] >> 2) & 0xFU;
} else if (msg->addr == FORD_Yaw_Data_FD1) {
} else { // msg->addr == FORD_Yaw_Data_FD1
// Signal: VehRollYaw_No_Cnt
cnt = msg->data[5];
} else {
}
return cnt;
}
Expand All @@ -39,10 +38,9 @@ static uint32_t ford_get_checksum(const CANPacket_t *msg) {
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: VehVActlBrk_No_Cs
chksum = msg->data[3];
} else if (msg->addr == FORD_Yaw_Data_FD1) {
} else { // msg->addr == FORD_Yaw_Data_FD1
// Signal: VehRollYawW_No_Cs
chksum = msg->data[4];
} else {
}
return chksum;
}
Expand All @@ -54,14 +52,13 @@ static uint32_t ford_compute_checksum(const CANPacket_t *msg) {
chksum += msg->data[2] >> 6; // VehVActlBrk_D_Qf
chksum += (msg->data[2] >> 2) & 0xFU; // VehVActlBrk_No_Cnt
chksum = 0xFFU - chksum;
} else if (msg->addr == FORD_Yaw_Data_FD1) {
} else { // msg->addr == FORD_Yaw_Data_FD1
chksum += msg->data[0] + msg->data[1]; // VehRol_W_Actl
chksum += msg->data[2] + msg->data[3]; // VehYaw_W_Actl
chksum += msg->data[5]; // VehRollYaw_No_Cnt
chksum += msg->data[6] >> 6; // VehRolWActl_D_Qf
chksum += (msg->data[6] >> 4) & 0x3U; // VehYawWActl_D_Qf
chksum = 0xFFU - chksum;
} else {
}
return chksum;
}
Expand All @@ -72,9 +69,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) {
valid = (msg->data[2] >> 6) == 0x3U; // VehVActlBrk_D_Qf
} else if (msg->addr == FORD_EngVehicleSpThrottle2) {
valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf
} else if (msg->addr == FORD_Yaw_Data_FD1) {
} else { // msg->addr == FORD_Yaw_Data_FD1
valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf
} else {
}
return valid;
}
Expand All @@ -96,55 +92,54 @@ static const CurvatureSteeringLimits FORD_STEERING_LIMITS = {
};

static void ford_rx_hook(const CANPacket_t *msg) {
if (msg->bus == FORD_MAIN_BUS) {
// Update in motion state from standstill signal
if (msg->addr == FORD_DesiredTorqBrk) {
// Signal: VehStop_D_Stat
vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U;
}
// The RX checks already validate the address, bus, and length.
// Update in motion state from standstill signal
if (msg->addr == FORD_DesiredTorqBrk) {
// Signal: VehStop_D_Stat
vehicle_moving = ((msg->data[3] >> 3) & 0x3U) != 1U;
}

// Update vehicle speed
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: Veh_V_ActlBrk
UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS);
}
// Update vehicle speed
if (msg->addr == FORD_BrakeSysFeatures) {
// Signal: Veh_V_ActlBrk
UPDATE_VEHICLE_SPEED(((msg->data[0] << 8) | msg->data[1]) * 0.01 * KPH_TO_MS);
}

// Check vehicle speed against a second source
if (msg->addr == FORD_EngVehicleSpThrottle2) {
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
// Signal: Veh_V_ActlEng
float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS;
UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed);
}
// Check vehicle speed against a second source
if (msg->addr == FORD_EngVehicleSpThrottle2) {
// Disable controls if speeds from ABS and PCM ECUs are too far apart.
// Signal: Veh_V_ActlEng
float filtered_pcm_speed = ((msg->data[6] << 8) | msg->data[7]) * 0.01 * KPH_TO_MS;
UPDATE_VEHICLE_SPEED_2(filtered_pcm_speed);
}

// Update vehicle yaw rate
if (msg->addr == FORD_Yaw_Data_FD1) {
// FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received
// Signal: VehYaw_W_Actl
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can));
}
// Update vehicle yaw rate
if (msg->addr == FORD_Yaw_Data_FD1) {
// FIXME: safety can receive yaw before new vehicle speed, it should recompute meas on either received
// Signal: VehYaw_W_Actl
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
update_sample(&curvature_state.meas, ROUND(current_curvature * FORD_STEERING_LIMITS.curvature_to_can));
}

// Update gas pedal
if (msg->addr == FORD_EngVehicleSpThrottle) {
// Pedal position: (0.1 * val) in percent
// Signal: ApedPos_Pc_ActlArb
gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U;
}
// Update gas pedal
if (msg->addr == FORD_EngVehicleSpThrottle) {
// Pedal position: (0.1 * val) in percent
// Signal: ApedPos_Pc_ActlArb
gas_pressed = (((msg->data[0] & 0x03U) << 8) | msg->data[1]) > 0U;
}

// Update brake pedal and cruise state
if (msg->addr == FORD_EngBrakeData) {
// Signal: BpedDrvAppl_D_Actl
brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U;
// Update brake pedal and cruise state
if (msg->addr == FORD_EngBrakeData) {
// Signal: BpedDrvAppl_D_Actl
brake_pressed = ((msg->data[0] >> 4) & 0x3U) == 2U;

// Signal: CcStat_D_Actl
unsigned int cruise_state = msg->data[1] & 0x07U;
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
pcm_cruise_check(cruise_engaged);
}
// Signal: CcStat_D_Actl
unsigned int cruise_state = msg->data[1] & 0x07U;
bool cruise_engaged = (cruise_state == 4U) || (cruise_state == 5U);
pcm_cruise_check(cruise_engaged);
}
}

Expand Down
93 changes: 46 additions & 47 deletions opendbc/safety/modes/gm.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,64 +32,63 @@ static bool gm_pcm_cruise = false;
static void gm_rx_hook(const CANPacket_t *msg) {
const int GM_STANDSTILL_THRSLD = 10; // 0.311kph

if (msg->bus == 0U) {
if (msg->addr == 0x184U) {
int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[7];
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}

// sample rear wheel speeds
if (msg->addr == 0x34AU) {
int left_rear_speed = (msg->data[0] << 8) | msg->data[1];
int right_rear_speed = (msg->data[2] << 8) | msg->data[3];
vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
}

// ACC steering wheel buttons (GM_CAM is tied to the PCM)
if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) {
int button = (msg->data[5] & 0x70U) >> 4;
// The RX checks already validate the address, bus, and length.
if (msg->addr == 0x184U) {
int torque_driver_new = ((msg->data[6] & 0x7U) << 8) | msg->data[7];
torque_driver_new = to_signed(torque_driver_new, 11);
// update array of samples
update_sample(&torque_driver, torque_driver_new);
}

// enter controls on falling edge of set or rising edge of resume (avoids fault)
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}
// sample rear wheel speeds
if (msg->addr == 0x34AU) {
int left_rear_speed = (msg->data[0] << 8) | msg->data[1];
int right_rear_speed = (msg->data[2] << 8) | msg->data[3];
vehicle_moving = (left_rear_speed > GM_STANDSTILL_THRSLD) || (right_rear_speed > GM_STANDSTILL_THRSLD);
}

// exit controls on cancel press
if (button == GM_BTN_CANCEL) {
controls_allowed = false;
}
// ACC steering wheel buttons (GM_CAM is tied to the PCM)
if ((msg->addr == 0x1E1U) && !gm_pcm_cruise) {
int button = (msg->data[5] & 0x70U) >> 4;

cruise_button_prev = button;
// enter controls on falling edge of set or rising edge of resume (avoids fault)
bool set = (button != GM_BTN_SET) && (cruise_button_prev == GM_BTN_SET);
bool res = (button == GM_BTN_RESUME) && (cruise_button_prev != GM_BTN_RESUME);
if (set || res) {
controls_allowed = true;
}

// Reference for brake pressed signals:
// https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) {
brake_pressed = msg->data[1] >= 8U;
// exit controls on cancel press
if (button == GM_BTN_CANCEL) {
controls_allowed = false;
}

if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) {
brake_pressed = GET_BIT(msg, 40U);
}
cruise_button_prev = button;
}

if (msg->addr == 0x1C4U) {
gas_pressed = msg->data[5] != 0U;
// Reference for brake pressed signals:
// https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
if ((msg->addr == 0xBEU) && (gm_hw == GM_ASCM)) {
brake_pressed = msg->data[1] >= 8U;
}

// enter controls on rising edge of ACC, exit controls when ACC off
if (gm_pcm_cruise) {
bool cruise_engaged = (msg->data[1] >> 5) != 0U;
pcm_cruise_check(cruise_engaged);
}
}
if ((msg->addr == 0xC9U) && (gm_hw == GM_CAM)) {
brake_pressed = GET_BIT(msg, 40U);
}

if (msg->addr == 0xBDU) {
regen_braking = (msg->data[0] >> 4) != 0U;
if (msg->addr == 0x1C4U) {
gas_pressed = msg->data[5] != 0U;

// enter controls on rising edge of ACC, exit controls when ACC off
if (gm_pcm_cruise) {
bool cruise_engaged = (msg->data[1] >> 5) != 0U;
pcm_cruise_check(cruise_engaged);
}
}

if (msg->addr == 0xBDU) {
regen_braking = (msg->data[0] >> 4) != 0U;
}
}

static bool gm_tx_hook(const CANPacket_t *msg) {
Expand Down Expand Up @@ -143,7 +142,7 @@ static bool gm_tx_hook(const CANPacket_t *msg) {
}

// BUTTONS: used for resume spamming and cruise cancellation with stock longitudinal
if ((msg->addr == 0x1E1U) && gm_pcm_cruise) {
if (msg->addr == 0x1E1U) {
int button = (msg->data[5] >> 4) & 0x7U;

bool allowed_cancel = (button == 6) && cruise_engaged_prev;
Expand Down
Loading
Loading