125 lines
4.7 KiB
C
125 lines
4.7 KiB
C
#pragma once
|
|
|
|
#include "opendbc/safety/declarations.h"
|
|
#include "opendbc/safety/modes/subaru_common.h"
|
|
|
|
// Preglobal platform
|
|
// 0x161 is ES_CruiseThrottle
|
|
// 0x164 is ES_LKAS
|
|
|
|
#define MSG_SUBARU_PG_CruiseControl 0x144U
|
|
#define MSG_SUBARU_PG_Throttle 0x140U
|
|
#define MSG_SUBARU_PG_Wheel_Speeds 0xD4U
|
|
#define MSG_SUBARU_PG_Brake_Pedal 0xD1U
|
|
#define MSG_SUBARU_PG_ES_LKAS 0x164U
|
|
#define MSG_SUBARU_PG_ES_Distance 0x161U
|
|
#define MSG_SUBARU_PG_Steering_Torque 0x371U
|
|
|
|
#define SUBARU_PG_MAIN_BUS 0U
|
|
#define SUBARU_PG_CAM_BUS 2U
|
|
|
|
#define SUBARU_PG_COMMON_TX_MSGS \
|
|
{MSG_SUBARU_PG_ES_Distance, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
|
|
{MSG_SUBARU_PG_ES_LKAS, SUBARU_PG_MAIN_BUS, 8, .check_relay = true}, \
|
|
|
|
#define SUBARU_PG_STOP_AND_GO_TX_MSGS \
|
|
{MSG_SUBARU_PG_Throttle, SUBARU_PG_CAM_BUS, 8, .check_relay = false}, \
|
|
{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_CAM_BUS, 4, .check_relay = false}, \
|
|
|
|
static bool subaru_pg_reversed_driver_torque = false;
|
|
|
|
static void subaru_preglobal_rx_hook(const CANPacket_t *msg) {
|
|
if (msg->bus == SUBARU_PG_MAIN_BUS) {
|
|
if (msg->addr == MSG_SUBARU_PG_Steering_Torque) {
|
|
int torque_driver_new;
|
|
torque_driver_new = (msg->data[3] >> 5) + (msg->data[4] << 3);
|
|
torque_driver_new = to_signed(torque_driver_new, 11);
|
|
torque_driver_new = subaru_pg_reversed_driver_torque ? -torque_driver_new : torque_driver_new;
|
|
update_sample(&torque_driver, torque_driver_new);
|
|
}
|
|
|
|
// enter controls on rising edge of ACC, exit controls on ACC off
|
|
if (msg->addr == MSG_SUBARU_PG_CruiseControl) {
|
|
bool cruise_engaged = (msg->data[6] >> 1) & 1U;
|
|
pcm_cruise_check(cruise_engaged);
|
|
acc_main_on = GET_BIT(msg, 48U);
|
|
}
|
|
|
|
// update vehicle moving with any non-zero wheel speed
|
|
if (msg->addr == MSG_SUBARU_PG_Wheel_Speeds) {
|
|
vehicle_moving = ((GET_BYTES(msg, 0, 4) >> 12) != 0U) || (GET_BYTES(msg, 4, 4) != 0U);
|
|
}
|
|
|
|
if (msg->addr == MSG_SUBARU_PG_Brake_Pedal) {
|
|
brake_pressed = ((GET_BYTES(msg, 0, 4) >> 16) & 0xFFU) > 0U;
|
|
}
|
|
|
|
if (msg->addr == MSG_SUBARU_PG_Throttle) {
|
|
gas_pressed = msg->data[0] != 0U;
|
|
}
|
|
}
|
|
}
|
|
|
|
static bool subaru_preglobal_tx_hook(const CANPacket_t *msg) {
|
|
const TorqueSteeringLimits SUBARU_PG_STEERING_LIMITS = {
|
|
.max_torque = 2047,
|
|
.max_rt_delta = 940,
|
|
.max_rate_up = 50,
|
|
.max_rate_down = 70,
|
|
.driver_torque_multiplier = 10,
|
|
.driver_torque_allowance = 75,
|
|
.type = TorqueDriverLimited,
|
|
};
|
|
|
|
bool tx = true;
|
|
|
|
// steer cmd checks
|
|
if (msg->addr == MSG_SUBARU_PG_ES_LKAS) {
|
|
int desired_torque = ((GET_BYTES(msg, 0, 4) >> 8) & 0x1FFFU);
|
|
desired_torque = -1 * to_signed(desired_torque, 13);
|
|
|
|
bool steer_req = (msg->data[3] >> 0) & 1U;
|
|
|
|
if (steer_torque_cmd_checks(desired_torque, steer_req, SUBARU_PG_STEERING_LIMITS)) {
|
|
tx = false;
|
|
}
|
|
}
|
|
return tx;
|
|
}
|
|
|
|
static safety_config subaru_preglobal_init(uint16_t param) {
|
|
static const CanMsg SUBARU_PG_TX_MSGS[] = {
|
|
SUBARU_PG_COMMON_TX_MSGS
|
|
};
|
|
|
|
static const CanMsg subaru_pg_stop_and_go_tx_msgs[] = {
|
|
SUBARU_PG_COMMON_TX_MSGS
|
|
SUBARU_PG_STOP_AND_GO_TX_MSGS
|
|
};
|
|
|
|
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
|
|
static RxCheck subaru_preglobal_rx_checks[] = {
|
|
{.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_SUBARU_PG_Wheel_Speeds, SUBARU_PG_MAIN_BUS, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
{.msg = {{MSG_SUBARU_PG_Brake_Pedal, SUBARU_PG_MAIN_BUS, 4, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
|
};
|
|
|
|
subaru_common_init();
|
|
|
|
const uint16_t SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 4;
|
|
|
|
subaru_pg_reversed_driver_torque = GET_FLAG(param, SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE);
|
|
|
|
safety_config ret = subaru_stop_and_go ? BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, subaru_pg_stop_and_go_tx_msgs) : \
|
|
BUILD_SAFETY_CFG(subaru_preglobal_rx_checks, SUBARU_PG_TX_MSGS);
|
|
return ret;
|
|
}
|
|
|
|
const safety_hooks subaru_preglobal_hooks = {
|
|
.init = subaru_preglobal_init,
|
|
.rx = subaru_preglobal_rx_hook,
|
|
.tx = subaru_preglobal_tx_hook,
|
|
};
|