diff --git a/bluepilot/params/params.json b/bluepilot/params/params.json index 79e1ff8a21..abc929e78e 100644 --- a/bluepilot/params/params.json +++ b/bluepilot/params/params.json @@ -26,6 +26,14 @@ "create_default": true, "enabled": true }, + { + "name": "FordPrefSteerAngleCurvature", + "default": false, + "type": "bool", + "flags": ["PERSISTENT", "BACKUP"], + "create_default": true, + "enabled": true + }, { "name": "FordPrefDisableDownhillCompUI", "default": false, diff --git a/common/params_keys.h b/common/params_keys.h index 2bd7406008..e183f1180d 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -317,6 +317,7 @@ inline static std::unordered_map keys = { {"disable_ford_radar_UI", {PERSISTENT | BACKUP, BOOL, "0"}}, {"vbatt_pause_charging", {PERSISTENT | BACKUP, FLOAT, "11.8"}}, {"show_lead_speed", {PERSISTENT | BACKUP, BOOL, "1"}}, + {"FordPrefSteerAngleCurvature", {PERSISTENT | BACKUP, BOOL, "0"}}, // pinion-sourced curvature measurement (bad-yaw-sensor workaround); read at car init {"FordPrefShowRadarLeadOverlay", {PERSISTENT | BACKUP, BOOL, "1"}}, {"FordPrefRadarOverlaySize", {PERSISTENT | BACKUP, INT, "1"}}, {"FordPrefHybridBatteryStatus", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 9b3fb0f90d..9900c67e2b 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -9,6 +9,7 @@ #define FORD_BrakeSysFeatures 0x415U // RX from ABS, for vehicle speed #define FORD_EngVehicleSpThrottle2 0x202U // RX from PCM, for second vehicle speed #define FORD_Yaw_Data_FD1 0x91U // RX from RCM, for yaw rate +#define FORD_SteeringPinion_Data 0x7EU // RX from PSCM, optional angle_meas source (STEER_ANGLE_CURVATURE) #define FORD_Steering_Data_FD1 0x083U // TX by OP, various driver switches and LKAS/CC buttons #define FORD_ACCDATA 0x186U // TX by OP, ACC controls #define FORD_ACCDATA_3 0x18AU // TX by OP, ACC/TJA user interface @@ -29,6 +30,9 @@ static uint8_t ford_get_counter(const CANPacket_t *msg) { } else if (msg->addr == FORD_Yaw_Data_FD1) { // Signal: VehRollYaw_No_Cnt cnt = msg->data[5]; + } else if (msg->addr == FORD_SteeringPinion_Data) { + // Signal: StePinAn_No_Cnt (47|4@0+) + cnt = (msg->data[5] >> 4) & 0xFU; } else { } return cnt; @@ -74,6 +78,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { valid = ((msg->data[4] >> 5) & 0x3U) == 0x3U; // VehVActlEng_D_Qf } else if (msg->addr == FORD_Yaw_Data_FD1) { valid = ((msg->data[6] >> 4) & 0x3U) == 0x3U; // VehYawWActl_D_Qf + } else if (msg->addr == FORD_SteeringPinion_Data) { + valid = ((msg->data[5] >> 2) & 0x3U) == 0x3U; // StePinCompAnEst_D_Qf (3=OK) } else { } return valid; @@ -106,10 +112,14 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { // Curvature rate limits -#define FORD_LIMITS(limit_lateral_acceleration) { \ +// max_angle_err: 100 (0.002) on the stock yaw-sourced angle_meas path; 150 (0.003) on the +// BluePilot pinion-sourced path (STEER_ANGLE_CURVATURE), because the raw pinion angle has +// no roll/alignment-offset compensation in firmware (the Python layer compensates via +// liveParameters; firmware uses the raw pinion angle). +#define FORD_LIMITS(limit_lateral_acceleration, max_angle_err) { \ .max_angle = 1000, /* 0.02 curvature */ \ .angle_deg_to_can = 50000, /* 1 / (2e-5) rad to can */ \ - .max_angle_error = 100, /* 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can */ \ + .max_angle_error = (max_angle_err), \ /* Looser symmetric ROCs (former down table); Python control uses stricter up row in values_ext */ \ .angle_rate_up_lookup = { \ {5., 16., 25.}, \ @@ -221,7 +231,39 @@ static const AngleSteeringLimits FORD_CURVATURE_RATE_LIMITS_CANFD = { .inactive_angle_is_zero = true, }; -static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false); +static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false, 100); +static const AngleSteeringLimits FORD_STEERING_LIMITS_PINION = FORD_LIMITS(false, 150); + +// BluePilot: per-platform geometry for pinion-angle -> curvature conversion (the optional +// angle_meas source), selected by the 4-bit geometry index in current_safety_param_sp +// bits 1-4. Row order and values must match FORD_PINION_GEOMETRY_INDEX in +// opendbc/sunnypilot/car/ford/values_ext.py (enforced by test_ford.py's +// geometry-consistency test against CarSpecs + calc_slip_factor(VehicleModel(CP))). +// Index 0 is reserved as invalid; ford_init disables the feature outright on a zero or +// out-of-range index so a half-configured param can never select the wrong geometry. +// FORD_EDGE_MK2 (ALT_STEER_ANGLE: relative pinion angle + learned offset) is unsupported +// and deliberately absent. +#define FORD_PINION_GEOMETRY_COUNT 12U +static const AngleSteeringParams ford_pinion_geometry[FORD_PINION_GEOMETRY_COUNT + 1U] = { + {.slip_factor = 0.0f, .steer_ratio = 1.0f, .wheelbase = 1.0f}, // 0: invalid + {.slip_factor = -0.00062819555f, .steer_ratio = 17.7f, .wheelbase = 2.670f}, // 1: FORD_BRONCO_SPORT_MK1 + {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 2: FORD_ESCAPE_MK4 + {.slip_factor = -0.00061892325f, .steer_ratio = 16.7f, .wheelbase = 2.710f}, // 3: FORD_ESCAPE_MK4_5 + {.slip_factor = -0.00045454798f, .steer_ratio = 17.0f, .wheelbase = 3.690f}, // 4: FORD_EXPEDITION_MK4 + {.slip_factor = -0.00055447339f, .steer_ratio = 16.8f, .wheelbase = 3.025f}, // 5: FORD_EXPLORER_MK6 + {.slip_factor = -0.00062121569f, .steer_ratio = 15.0f, .wheelbase = 2.700f}, // 6: FORD_FOCUS_MK4 + {.slip_factor = -0.00045331952f, .steer_ratio = 16.9f, .wheelbase = 3.700f}, // 7: FORD_F_150_LIGHTNING_MK1 + {.slip_factor = -0.00042037149f, .steer_ratio = 17.0f, .wheelbase = 3.990f}, // 8: FORD_F_150_MK14 + {.slip_factor = -0.00054528036f, .steer_ratio = 17.0f, .wheelbase = 3.076f}, // 9: FORD_MAVERICK_MK1 + {.slip_factor = -0.00058852001f, .steer_ratio = 14.8f, .wheelbase = 2.850f}, // 10: FORD_MONDEO_MK5 + {.slip_factor = -0.00056209187f, .steer_ratio = 17.0f, .wheelbase = 2.984f}, // 11: FORD_MUSTANG_MACH_E_MK1 + {.slip_factor = -0.00051293030f, .steer_ratio = 17.0f, .wheelbase = 3.270f}, // 12: FORD_RANGER_MK2 +}; + +// BluePilot: steering-angle curvature measurement state (STEER_ANGLE_CURVATURE), set once +// in ford_init from current_safety_param_sp. Default off = stock yaw-sourced angle_meas. +static bool ford_bp_pinion_curvature = false; +static const AngleSteeringParams *ford_bp_pinion_params = &ford_pinion_geometry[0]; @@ -396,8 +438,8 @@ static void ford_rx_hook(const CANPacket_t *msg) { speed_mismatch_check(filtered_pcm_speed); } - // Update vehicle yaw rate - if (msg->addr == FORD_Yaw_Data_FD1) { + // Update vehicle yaw rate (stock angle_meas source; skipped when the pinion source is enabled) + if ((msg->addr == FORD_Yaw_Data_FD1) && !ford_bp_pinion_curvature) { // 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; @@ -406,6 +448,27 @@ static void ford_rx_hook(const CANPacket_t *msg) { update_sample(&angle_meas, ROUND(current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can)); } + // BluePilot: optional angle_meas source -- measured curvature from the steering pinion + // angle (PSCM) via the vehicle model, for vehicles whose RCM broadcasts implausible yaw + // (sign-inverted vs IMU/steering geometry) while its quality flag still reads OK. The + // pinion angle was validated against the comma IMU (corr +0.99 on real routes); the + // Python control layer measures from the same source when this is enabled + // (lateral_curv_ext.get_current_curvature), so the layers always agree. + if ((msg->addr == FORD_SteeringPinion_Data) && ford_bp_pinion_curvature) { + // Signal: StePinComp_An_Est, 22|15@0+ (0.1,-1600) deg + int angle_raw = ((msg->data[2] & 0x7FU) << 8) | msg->data[3]; + float pinion_angle_deg = ((float)angle_raw * 0.1f) - 1600.0f; + float pinion_angle_rad = pinion_angle_deg * 0.017453292519943295f; // DEG_TO_RAD + // angle -> curvature via vehicle model (matches VehicleModel.curvature_factor); + // sign: firmware angle_meas is Ford wire convention (the yaw block uses +yaw/v), and + // pinion angle correlates +0.97 with wire desired curvature on real frames -> positive. + float speed = SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1); + float curvature_factor = get_curvature_factor(speed, *ford_bp_pinion_params); + float current_curvature = pinion_angle_rad * curvature_factor / ford_bp_pinion_params->steer_ratio; + // convert current curvature into units on CAN for comparison with desired curvature + update_sample(&angle_meas, ROUND(current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can)); + } + // Update gas pedal if (msg->addr == FORD_EngVehicleSpThrottle) { // Pedal position: (0.1 * val) in percent @@ -590,7 +653,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // keeps its own checks regardless. But steer_angle_cmd_checks also carries the // controls_allowed gate bp-6.0 relied on for every frame; restore that piece explicitly so a // steer_control_enabled frame at curvature == 0 can't bypass it (see LMC2 block below). - bool curvature_violation = steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS); + // BluePilot: the pinion-sourced angle_meas variant carries a wider error band (150 vs 100) -- + // see the FORD_LIMITS macro comment. Everything else in the two limit sets is identical. + const AngleSteeringLimits *ford_lmc_limits = ford_bp_pinion_curvature ? &FORD_STEERING_LIMITS_PINION : &FORD_STEERING_LIMITS; + bool curvature_violation = steer_angle_cmd_checks(desired_curvature, steer_control_enabled, *ford_lmc_limits); if (desired_curvature != 0) { violation |= curvature_violation; } else { @@ -609,7 +675,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // it's still bounded by the tight path_angle range above and steer_control_enabled's own checks. if ((desired_curvature == 0) && ford_bp_angle_mode_engaged) { int shadow_curvature_can = FORD_BP_SHADOW_CURVATURE_TO_CAN(ford_bp_shadow_curvature_raw); - violation |= ford_shadow_curvature_error_check(shadow_curvature_can, steer_control_enabled, FORD_STEERING_LIMITS); + violation |= ford_shadow_curvature_error_check(shadow_curvature_can, steer_control_enabled, *ford_lmc_limits); } // Check path angle rate of change limits @@ -649,7 +715,10 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // Safety check for LateralMotionControl2 action if (msg->addr == FORD_LateralMotionControl2) { - static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true); + static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true, 100); + static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS_PINION = FORD_LIMITS(true, 150); + // BluePilot: see the CAN handler's ford_lmc_limits comment. + const AngleSteeringLimits *ford_lmc2_limits = ford_bp_pinion_curvature ? &FORD_CANFD_STEERING_LIMITS_PINION : &FORD_CANFD_STEERING_LIMITS; // Signal: LatCtl_D2_Rq bool steer_control_enabled = ((msg->data[0] >> 4) & 0x7U) != 0U; @@ -727,7 +796,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // keeps its own checks regardless. But steer_angle_cmd_checks also carries the // controls_allowed gate bp-6.0 relied on for every frame; restore that piece explicitly so a // steer_control_enabled frame at curvature == 0 can't bypass it. - bool curvature_violation = steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_CANFD_STEERING_LIMITS); + bool curvature_violation = steer_angle_cmd_checks(desired_curvature, steer_control_enabled, *ford_lmc2_limits); if (desired_curvature != 0) { violation |= curvature_violation; } else { @@ -746,7 +815,7 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // it's still bounded by the tight path_angle range above and steer_control_enabled's own checks. if ((desired_curvature == 0) && ford_bp_angle_mode_engaged) { int shadow_curvature_can = FORD_BP_SHADOW_CURVATURE_TO_CAN(ford_bp_shadow_curvature_raw); - violation |= ford_shadow_curvature_error_check(shadow_curvature_can, steer_control_enabled, FORD_CANFD_STEERING_LIMITS); + violation |= ford_shadow_curvature_error_check(shadow_curvature_can, steer_control_enabled, *ford_lmc2_limits); } // Check path angle rate of change limits @@ -793,18 +862,32 @@ static bool ford_tx_hook(const CANPacket_t *msg) { static safety_config ford_init(uint16_t param) { // warning: quality flags are not yet checked in openpilot's CAN parser, // this may be the cause of blocked messages + #define FORD_COMMON_RX_CHECKS \ + {.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, \ + /* FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug */ \ + /* Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC */ \ + /* It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that */ \ + {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, \ + {.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}}, \ + /* These messages have no counter or checksum */ \ + {.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \ + static RxCheck ford_rx_checks[] = { - {.msg = {{FORD_BrakeSysFeatures, 0, 8, 50U, .max_counter = 15U}, { 0 }, { 0 }}}, - // FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug - // Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC - // It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that - {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, - {.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}}, - // These messages have no counter or checksum - {.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, - {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, - {.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, - {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + FORD_COMMON_RX_CHECKS + }; + + // BluePilot: only enforced when the pinion angle_meas source is enabled -- keeping this + // entry in the stock config would make a pinion hiccup disable controls for users who + // never consume the message. + static RxCheck ford_rx_checks_pinion[] = { + FORD_COMMON_RX_CHECKS + // Pinion angle (angle_meas source). Counter verified 0-15 on real frames. + // StePinAn_No_Cs checksum algorithm is unknown (Ford sum-invert patterns don't match + // real frames) -> ignore_checksum; integrity via counter + quality flag + 100Hz check. + {.msg = {{FORD_SteeringPinion_Data, 0, 8, 100U, .max_counter = 15U, .ignore_checksum = true}, { 0 }, { 0 }}}, }; // BluePilot: an earlier design tried a dedicated CAN message (0x5F0) for python->ford.h state, @@ -858,6 +941,21 @@ static safety_config ford_init(uint16_t param) { // Longitudinal is the default for CAN, and optional for CAN FD w/ ALLOW_DEBUG // ford_longitudinal = !ford_canfd || ford_longitudinal; + // BluePilot: steering-angle curvature measurement (bad-yaw-sensor workaround), read from + // the sunnypilot SP safety param (current_safety_param_sp, delivered via USB 0xdf before + // the safety model is set -- a separate uint16 from this function's param; pattern: + // subaru_common.h). Bit 0 enables; bits 1-4 carry the platform geometry-table index. + // A zero or out-of-range index disables the feature outright, so a half-configured param + // can never select the wrong geometry: the stock yaw path is kept in that case. + const uint16_t FORD_PARAM_SP_STEER_ANGLE_CURVATURE = 1; + bool pinion_enabled = GET_FLAG(current_safety_param_sp, FORD_PARAM_SP_STEER_ANGLE_CURVATURE); + const uint16_t pinion_geometry_index = (current_safety_param_sp >> 1) & 0xFU; + if ((pinion_geometry_index == 0U) || (pinion_geometry_index > FORD_PINION_GEOMETRY_COUNT)) { + pinion_enabled = false; + } + ford_bp_pinion_curvature = pinion_enabled; + ford_bp_pinion_params = pinion_enabled ? &ford_pinion_geometry[pinion_geometry_index] : &ford_pinion_geometry[0]; + safety_config ret; if (ford_canfd) { ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS) : \ @@ -866,6 +964,10 @@ static safety_config ford_init(uint16_t param) { ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS) : \ BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS); } + if (ford_bp_pinion_curvature) { + // Enforce 100Hz/counter/QF on the pinion message only when it is actually consumed. + SET_RX_CHECKS(ford_rx_checks_pinion, ret); + } return ret; } diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py index 3cf23d68a7..06cfbc2ce8 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py @@ -119,6 +119,10 @@ class CANPacket: bool get_controls_requested_lateral(void); void set_current_safety_param_sp(uint16_t param); uint16_t get_current_safety_param_sp(void); +int get_ford_pinion_geometry_count(void); +float get_ford_pinion_geometry_slip_factor(int idx); +float get_ford_pinion_geometry_steer_ratio(int idx); +float get_ford_pinion_geometry_wheelbase(int idx); bool get_enable_mads(void); bool get_disengage_lateral_on_brake(void); bool get_pause_lateral_on_brake(void); diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index ef3e8f1e97..bbf30c89ea 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c @@ -258,6 +258,28 @@ uint16_t get_current_safety_param_sp(void){ return current_safety_param_sp; } +// BluePilot: debug getters for the Ford pinion geometry table (ALLOW_DEBUG builds only). +// Consumed by test_ford.py's geometry-consistency test, which compares every firmware row +// against CarSpecs + calc_slip_factor(VehicleModel(CP)) so the table cannot rot as +// platforms change -- without fragile header parsing. +#ifdef ALLOW_DEBUG +int get_ford_pinion_geometry_count(void){ + return (int)FORD_PINION_GEOMETRY_COUNT; +} + +float get_ford_pinion_geometry_slip_factor(int idx){ + return ((idx >= 0) && (idx <= (int)FORD_PINION_GEOMETRY_COUNT)) ? ford_pinion_geometry[idx].slip_factor : 0.0f; +} + +float get_ford_pinion_geometry_steer_ratio(int idx){ + return ((idx >= 0) && (idx <= (int)FORD_PINION_GEOMETRY_COUNT)) ? ford_pinion_geometry[idx].steer_ratio : 0.0f; +} + +float get_ford_pinion_geometry_wheelbase(int idx){ + return ((idx >= 0) && (idx <= (int)FORD_PINION_GEOMETRY_COUNT)) ? ford_pinion_geometry[idx].wheelbase : 0.0f; +} +#endif + void set_mads_button_press(int c){ mads_button_press = c; } diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index 9c88751857..ffed7e39f0 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -5,7 +5,10 @@ import opendbc.safety.tests.common as common from opendbc.car.ford.carcontroller import MAX_LATERAL_ACCEL -from opendbc.car.ford.values import FordSafetyFlags +from opendbc.car.ford.values import CAR, FordFlags, FordSafetyFlags +from opendbc.car.interfaces import scale_tire_stiffness +from opendbc.car.vehicle_model import VehicleModel, calc_slip_factor +from opendbc.sunnypilot.car.ford.values_ext import FORD_PINION_GEOMETRY_INDEX, FORD_PINION_GEOMETRY_SHIFT, FordSafetyFlagsSP from opendbc.car.structs import CarParams from opendbc.safety.tests.libsafety import libsafety_py from opendbc.safety.tests.common import CANPackerSafety @@ -66,6 +69,11 @@ class Buttons: # * CAN FD with openpilot longitudinal class TestFordSafetyBase(common.CarSafetyTest): + # BluePilot: sunnypilot SP safety param (current_safety_param_sp), set before + # set_safety_hooks in every concrete setUp -- ford_init reads it. 0 = stock behavior; + # the pinion-curvature classes below override it (Toyota SAFETY_PARAM_SP convention). + SAFETY_PARAM_SP: int = 0 + STANDSTILL_THRESHOLD = 1 RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl, MSG_LateralMotionControl2, MSG_IPMA_Data)} @@ -489,6 +497,7 @@ class TestFordCANFDStockSafety(TestFordSafetyBase): def setUp(self): self.packer = CANPackerSafety("ford_lincoln_base_pt") self.safety = libsafety_py.libsafety + self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP) self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.CANFD) self.safety.init_tests() @@ -560,6 +569,7 @@ class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase): def setUp(self): self.packer = CANPackerSafety("ford_lincoln_base_pt") self.safety = libsafety_py.libsafety + self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP) # Make sure we enforce long safety even without long flag for CAN self.safety.set_safety_hooks(CarParams.SafetyModel.ford, 0) self.safety.init_tests() @@ -585,9 +595,264 @@ class TestFordCANFDLongitudinalSafety(TestFordLongitudinalSafetyBase): def setUp(self): self.packer = CANPackerSafety("ford_lincoln_base_pt") self.safety = libsafety_py.libsafety + self.safety.set_current_safety_param_sp(self.SAFETY_PARAM_SP) self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.LONG_CONTROL | FordSafetyFlags.CANFD) self.safety.init_tests() +# ============================================================================= +# BluePilot: steering-angle curvature measurement (FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) +# +# Opt-in alternative angle_meas source for vehicles whose RCM broadcasts implausible yaw +# while its quality flag reads OK. The classes below run the ENTIRE stock test matrix with +# angle_meas sourced from SteeringPinion_Data and the widened 0.003 error band, plus +# pinion-specific tests. The stock (flag-off) classes above never set SAFETY_PARAM_SP, so +# their outcomes (including any pre-existing failures) must stay bit-identical to the base +# branch -- that comparison is the default-off zero-delta check. +# ============================================================================= + +class TestFordPinionCurvatureSafetyBase(TestFordSafetyBase): + MAX_CURVATURE_ERROR = 0.003 # widened: raw pinion angle has no roll/offset compensation in firmware + + # Per-platform geometry (see the *PinionGeometry mixins). Values must match the + # ford_pinion_geometry row for GEOMETRY_INDEX -- the table itself is checked against + # CarSpecs + calc_slip_factor by TestFordPinionGeometryTable, so these literals only + # need to agree with that already-verified table. + GEOMETRY_INDEX = 0 + PINION_SLIP_FACTOR = 0.0 + PINION_STEER_RATIO = 1.0 + PINION_WHEELBASE = 1.0 + + cnt_pinion = 0 + + def _curvature_to_pinion_angle_deg(self, curvature: float, speed: float) -> float: + # Inverse of the firmware conversion in ford_rx_hook (modes/ford.h): + # curvature = angle_rad * curvature_factor(speed) / steer_ratio + speed = max(speed, 0.1) + curvature_factor = 1. / (1. - (self.PINION_SLIP_FACTOR * (speed ** 2))) / self.PINION_WHEELBASE + angle_rad = curvature * self.PINION_STEER_RATIO / curvature_factor + return float(np.degrees(angle_rad)) + + def _pinion_quant_tol(self, speed: float) -> int: + # 0.1 deg DBC quantization -> curvature CAN units at this speed (+2 for float rounding) + speed = max(speed, 0.1) + curvature_factor = 1. / (1. - (self.PINION_SLIP_FACTOR * (speed ** 2))) / self.PINION_WHEELBASE + return int(np.radians(0.1) * curvature_factor / self.PINION_STEER_RATIO * self.DEG_TO_CAN) + 2 + + # Current curvature measurement (pinion-angle sourced, not yaw) + def _pinion_msg(self, curvature: float, speed: float, quality_flag=True): + values = {"StePinComp_An_Est": self._curvature_to_pinion_angle_deg(curvature, speed), + "StePinCompAnEst_D_Qf": 3 if quality_flag else 0, + "StePinAn_No_Cnt": self.cnt_pinion % 16} + self.__class__.cnt_pinion += 1 + return self.packer.make_can_msg_safety("SteeringPinion_Data", 0, values) + + def _reset_curvature_measurement(self, curvature, speed): + # 14 frames, not 6: frames after a counter discontinuity (e.g. rejected bad-QF frames + # advanced the python-side counter) are dropped by the rx counter check until it + # re-syncs, which would otherwise leave stale samples in the 6-deep angle_meas buffer + for _ in range(14): + self._rx(self._speed_msg(speed)) + self._rx(self._pinion_msg(curvature, speed)) + + def _drain_reset_bypass_latch(self, curvature): + # ford.h arms a 60-frame bypass latch whenever a curvature==0 && path_angle==0 frame is + # sent (human-turn ramp-up support). Prior tests commonly end on zeroed commands, so the + # latch may be live. Drain it with >60 nonzero-curvature frames (each decrements it), + # keeping the command at the measured curvature so nothing else violates meanwhile. + self.safety.set_controls_allowed(True) + for _ in range(70): + self._set_prev_desired_angle(curvature) + self._tx(self._lat_ctl_msg(True, 0, 0, curvature, 0)) + self.safety.set_controls_allowed(True) + + def test_rx_hook(self): + # checksum, counter, and quality flag checks (stock matrix + the pinion message) + for quality_flag in [True, False]: + for msg_type in ["speed", "speed_2", "yaw", "pinion"]: + self.safety.set_controls_allowed(True) + # send multiple times to verify counter checks + for _ in range(10): + if msg_type == "speed": + msg = self._speed_msg(0, quality_flag=quality_flag) + elif msg_type == "speed_2": + msg = self._speed_msg_2(0, quality_flag=quality_flag) + elif msg_type == "yaw": + msg = self._yaw_rate_msg(0, 0, quality_flag=quality_flag) + elif msg_type == "pinion": + msg = self._pinion_msg(0, 0, quality_flag=quality_flag) + + self.assertEqual(quality_flag, self._rx(msg)) + self.assertEqual(quality_flag, self.safety.get_controls_allowed()) + + # Mess with checksum to make it fail; checksum is not checked for 2nd speed or pinion + # (pinion has an unknown OEM checksum algorithm; integrity is via counter + quality flag) + msg[0].data[3] = 0 # Speed checksum & half of yaw/pinion angle signal + should_rx = msg_type in ("speed_2", "pinion") and quality_flag + self.assertEqual(should_rx, self._rx(msg)) + self.assertEqual(should_rx, self.safety.get_controls_allowed()) + + def test_angle_measurements(self): + """Tests rx hook correctly parses the curvature measurement from the steering pinion angle. + + The DBC signal quantizes to 0.1 deg, so allow the quantization-equivalent CAN-unit + tolerance from the round trip through the packer. + """ + for speed in np.arange(0.5, 40, 0.5): + for curvature in np.arange(0, self.MAX_CURVATURE * 2, 2e-3): + self._rx(self._speed_msg(speed)) + for c in (curvature, -curvature, 0, 0, 0, 0): + self._rx(self._pinion_msg(c, speed)) + + quant_tol = self._pinion_quant_tol(speed) + self.assertAlmostEqual(self.safety.get_angle_meas_min(), round(-curvature * self.DEG_TO_CAN), delta=quant_tol) + self.assertAlmostEqual(self.safety.get_angle_meas_max(), round(curvature * self.DEG_TO_CAN), delta=quant_tol) + + self._rx(self._pinion_msg(0, speed)) + self.assertAlmostEqual(self.safety.get_angle_meas_min(), round(-curvature * self.DEG_TO_CAN), delta=quant_tol) + self.assertAlmostEqual(self.safety.get_angle_meas_max(), 0, delta=quant_tol) + + self._rx(self._pinion_msg(0, speed)) + self.assertAlmostEqual(self.safety.get_angle_meas_min(), 0, delta=quant_tol) + self.assertAlmostEqual(self.safety.get_angle_meas_max(), 0, delta=quant_tol) + + def test_pinion_quality_flag_gates_measurement(self): + """A bad pinion quality flag must reject the message (measurement not updated).""" + speed = self.CURVATURE_ERROR_MIN_SPEED + 5 + self._reset_curvature_measurement(0.005, speed) + meas_max_before = self.safety.get_angle_meas_max() + self.assertGreater(meas_max_before, 0) + + # bad-QF frames must be rejected at rx and leave angle_meas untouched + for _ in range(6): + self.assertFalse(self._rx(self._pinion_msg(0, speed, quality_flag=False))) + self.assertEqual(self.safety.get_angle_meas_max(), meas_max_before) + + def test_pinion_sign_convention(self): + """Command matching the measured curvature sign passes the error check; a sign-inverted + command (the broken-yaw failure mode) violates above the gate speed.""" + speed = self.CURVATURE_ERROR_MIN_SPEED + 5 + curvature = 0.005 # well above MAX_CURVATURE_ERROR so the inverted case must violate + + for sign in (1, -1): + with self.subTest(sign=sign): + self._reset_curvature_measurement(sign * curvature, speed) + self._drain_reset_bypass_latch(sign * curvature) + self._set_prev_desired_angle(sign * curvature) + # matching-sign command: allowed + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, sign * curvature, 0))) + # inverted command (what a sign-flipped sensor would demand): blocked + self._set_prev_desired_angle(-sign * curvature) + self.assertFalse(self._tx(self._lat_ctl_msg(True, 0, 0, -sign * curvature, 0))) + + def test_pinion_check_inert_below_gate_speed(self): + """Below CURVATURE_ERROR_MIN_SPEED the deviation check must not constrain commands.""" + self.safety.set_controls_allowed(True) + speed = self.CURVATURE_ERROR_MIN_SPEED - 2 + self._reset_curvature_measurement(0.005, speed) + # command far from measured, but below gate: allowed (rate limits still apply, so seed prev) + inverted = -0.005 + self._set_prev_desired_angle(inverted) + self.assertTrue(self._tx(self._lat_ctl_msg(True, 0, 0, inverted, 0))) + + +class FordExplorerPinionGeometry: + """FORD_EXPLORER_MK6 -- the on-road-validated primary platform.""" + GEOMETRY_INDEX = 5 + PINION_SLIP_FACTOR = -0.00055447339 + PINION_STEER_RATIO = 16.8 + PINION_WHEELBASE = 3.025 + SAFETY_PARAM_SP = int(FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) | (GEOMETRY_INDEX << FORD_PINION_GEOMETRY_SHIFT) + + +class FordBroncoSportPinionGeometry: + """FORD_BRONCO_SPORT_MK1 -- smallest wheelbase in the table.""" + GEOMETRY_INDEX = 1 + PINION_SLIP_FACTOR = -0.00062819555 + PINION_STEER_RATIO = 17.7 + PINION_WHEELBASE = 2.670 + SAFETY_PARAM_SP = int(FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) | (GEOMETRY_INDEX << FORD_PINION_GEOMETRY_SHIFT) + + +class FordF150PinionGeometry: + """FORD_F_150_MK14 -- largest wheelbase in the table.""" + GEOMETRY_INDEX = 8 + PINION_SLIP_FACTOR = -0.00042037149 + PINION_STEER_RATIO = 17.0 + PINION_WHEELBASE = 3.990 + SAFETY_PARAM_SP = int(FordSafetyFlagsSP.STEER_ANGLE_CURVATURE) | (GEOMETRY_INDEX << FORD_PINION_GEOMETRY_SHIFT) + + +class TestFordPinionLongitudinalSafety(FordExplorerPinionGeometry, TestFordPinionCurvatureSafetyBase, TestFordLongitudinalSafety): + pass + + +class TestFordPinionCANFDStockSafety(FordExplorerPinionGeometry, TestFordPinionCurvatureSafetyBase, TestFordCANFDStockSafety): + pass + + +class TestFordPinionCANFDLongitudinalSafety(FordExplorerPinionGeometry, TestFordPinionCurvatureSafetyBase, TestFordCANFDLongitudinalSafety): + pass + + +class TestFordPinionBroncoSportSafety(FordBroncoSportPinionGeometry, TestFordPinionCurvatureSafetyBase, TestFordLongitudinalSafety): + pass + + +class TestFordPinionF150Safety(FordF150PinionGeometry, TestFordPinionCurvatureSafetyBase, TestFordCANFDLongitudinalSafety): + pass + + +class TestFordPinionGeometryTable(unittest.TestCase): + """The firmware geometry table must match CarSpecs + calc_slip_factor(VehicleModel(CP)) + for every supported platform, so the table cannot rot as platforms change. Reads the + table through the ALLOW_DEBUG libsafety getters -- no header parsing.""" + + TX_MSGS: list = [] # not a CarSafetyTest; keeps common.py's cross-mode TX sweep happy + + def test_geometry_matches_carspecs(self): + safety = libsafety_py.libsafety + count = safety.get_ford_pinion_geometry_count() + self.assertEqual(count, len(FORD_PINION_GEOMETRY_INDEX)) + # the index rides bits 1-4 of current_safety_param_sp; growing past 15 would silently + # disable the firmware side while the control side still enables -- never allow it + self.assertLessEqual(count, 15) + + seen = set() + for car in CAR: + if car.config.flags & FordFlags.ALT_STEER_ANGLE: + # relative pinion angle with a learned offset -- unsupported by design + self.assertNotIn(car, FORD_PINION_GEOMETRY_INDEX) + continue + self.assertIn(car, FORD_PINION_GEOMETRY_INDEX, f"{car} has no geometry-table row") + idx = FORD_PINION_GEOMETRY_INDEX[car] + self.assertTrue(1 <= idx <= count, f"{car}: index {idx} out of range") + self.assertNotIn(idx, seen, f"{car}: duplicate index {idx}") + seen.add(idx) + + specs = car.config.specs + CP = CarParams() + CP.mass = specs.mass + CP.wheelbase = specs.wheelbase + CP.steerRatio = specs.steerRatio + CP.centerToFront = specs.wheelbase * specs.centerToFrontRatio + CP.tireStiffnessFactor = specs.tireStiffnessFactor + CP.tireStiffnessFront, CP.tireStiffnessRear = scale_tire_stiffness( + CP.mass, CP.wheelbase, CP.centerToFront, CP.tireStiffnessFactor) + slip_factor = calc_slip_factor(VehicleModel(CP)) + + self.assertAlmostEqual(safety.get_ford_pinion_geometry_steer_ratio(idx), specs.steerRatio, places=3, msg=str(car)) + self.assertAlmostEqual(safety.get_ford_pinion_geometry_wheelbase(idx), specs.wheelbase, places=3, msg=str(car)) + self.assertAlmostEqual(safety.get_ford_pinion_geometry_slip_factor(idx), slip_factor, + delta=abs(slip_factor) * 1e-4, msg=str(car)) + + def test_invalid_index_row_is_inert(self): + # index 0 is the reserved invalid row: zero slip, unit ratios + safety = libsafety_py.libsafety + self.assertEqual(safety.get_ford_pinion_geometry_slip_factor(0), 0.0) + self.assertEqual(safety.get_ford_pinion_geometry_steer_ratio(0), 1.0) + self.assertEqual(safety.get_ford_pinion_geometry_wheelbase(0), 1.0) + + if __name__ == "__main__": unittest.main() diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py index 14975b5cd3..42aa95ed59 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_angle_ext.py @@ -228,7 +228,15 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - self.bp_kappa_cmd = 0.0 + # Publish the shadow curvature from the measured curvature while inactive. LKA keeps + # carrying angle_mode_engaged whenever angle mode is configured (independent of + # latActive), and ford.h latches the shadow from every LKA frame -- so the latched + # value must track reality here, not sit at a stale zero. Otherwise the first enabled + # LMC frame after (re-)engage races LKA's 33Hz latch against LMC's 20Hz enable bit and + # ford.h's deviation check compares a zero shadow against real measured curvature. + # (ford.h skips the check while steer_control_enabled is 0, so the value is free to + # follow the measurement during the inactive period itself.) + self.bp_kappa_cmd = self.get_current_curvature(CS) self.human_turn_detector.reset() self.angle_human_turn_active = False self.stall_blip_hold_s = 0.0 @@ -265,9 +273,10 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - # Zero the shadow curvature on the wire during the override (mirrors the inactive path); - # ford.h skips the deviation check while steer_control_enabled is 0 either way. - self.bp_kappa_cmd = 0.0 + # Truthful shadow during the override (mirrors the inactive path -- see the comment + # there): the driver is steering, so the honest command is the car's actual curvature, + # and the panda-latched shadow stays current for the re-engage frame. + self.bp_kappa_cmd = self.get_current_curvature(CS) # Keep exit detection current so resume doesn't compare against a stale pre-turn value. self._desired_curvature_last = float(actuators.curvature) # A human turn ends any stall episode -- its own mode 0 does the PSCM reset job. That also @@ -317,7 +326,8 @@ def update_angle_strategy(self, CC, CS, actuators, CP): self.bp_curvature_rate_limited = False self.bp_curvature_deviation_limited = False self.sim_curvature_last = 0.0 - self.bp_kappa_cmd = 0.0 + # Truthful shadow during the blip (see the inactive-path comment). + self.bp_kappa_cmd = self.get_current_curvature(CS) self._desired_curvature_last = float(actuators.curvature) self.precision_type = 1 if self.stall_blip_frames_left <= 0: @@ -427,7 +437,7 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # routinely, not just on genuine pothole/override divergence. Curvature mode has always clipped # here; this brings angle mode's actual steering intent in line with that proven behavior rather # than only clipping the value reported to panda (which would make the check a no-op). - current_curvature = -CS.out.yawRate / max(v_ego, 0.1) + current_curvature = self.get_current_curvature(CS) self.bp_curvature_deviation_limited = False if v_ego > 9: _kappa_cmd_pre_error_clip = kappa_cmd @@ -505,7 +515,13 @@ def update_angle_strategy(self, CC, CS, actuators, CP): # BluePilot: the error-clipped kappa path_angle was derived from -- carcontroller.py reads this # as shadow_curvature for ford.h's angle-mode deviation check (see fordcan_ext.create_lka_msg). # Not just telemetry: an actively-consumed value, unlike the removed *_kappa_cmd_raw stubs. - self.bp_kappa_cmd = kappa_cmd + # While the driver is pressing (before the human-turn override latches), the clipped planner + # kappa can't follow the wheel: the driver moves the measured curvature faster than the + # deviation clip tracks it, so the shadow can exit ford.h's error band mid-curve -- the one + # in-drive lateral safety block observed across ~3h of replayed road-test routes was exactly + # this (driver fighting a sustained curve with the mode still enabled). The honest command + # during a press is the driver's actual curvature. + self.bp_kappa_cmd = self.get_current_curvature(CS) if CS.out.steeringPressed else kappa_cmd # BluePilot: would the equivalent curvature (kappa_cmd) have been rate-limited by curvature-mode's # ROC (apply_std_steer_angle_limits)? kappa_cmd is already error-clipped above (same clip diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py index c6c69683b8..ebef4393e6 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/lateral_curv_ext.py @@ -15,6 +15,7 @@ https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706 """ +import math from collections import namedtuple, deque from enum import IntEnum @@ -27,7 +28,7 @@ from opendbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits from opendbc.car.vehicle_model import VehicleModel from opendbc.car.ford.values import CarControllerParams, FordFlags -from opendbc.sunnypilot.car.ford.values_ext import BP_ANGLE_LIMITS, CURVATURE_MAX +from opendbc.sunnypilot.car.ford.values_ext import BP_ANGLE_LIMITS, CURVATURE_MAX, FordSafetyFlagsSP from opendbc.sunnypilot.car.ford.human_turn import HumanTurnDetector from selfdrive.modeld.constants import ModelConstants @@ -121,6 +122,14 @@ def __init__(self, CP, CP_SP): # Primary lateral control variable: consumed by CarController's lateral dispatch. self.primary_lateral_control = PrimaryLateralControl.curvature + # BluePilot: steering-angle curvature measurement (bad-yaw-sensor workaround). + # Mirrors the STEER_ANGLE_CURVATURE flag the safety firmware reads from + # current_safety_param_sp -- both layers must always agree, so this is init-time + # state from CP_SP (set by _initialize_ford at car init), never a live Params read: + # a live flip against stale firmware would fight the panda. + self.bp_pinion_curvature_enabled = bool( + CP_SP is not None and (CP_SP.safetyParam & FordSafetyFlagsSP.STEER_ANGLE_CURVATURE)) + # Toggles (updated from Params each frame) self.enable_human_turn_detection_curv = True self.enable_lane_positioning_curv = False @@ -230,6 +239,33 @@ def _ensure_lateral_curv_initialized(self, CP): # branch LateralCurvExt state is initialized eagerly in __init__, so nothing to do here. pass + def get_current_curvature(self, CS): + """Measured curvature of the car right now (OP sign convention). + + The single measurement source for every BluePilot lateral consumer: the deviation + clip, the stall detector, and the shadow curvature published to ford.h's angle-mode + deviation check. The default source is the RCM yaw rate -- the same family stock + ford.h derives its angle_meas from. The shadow value judged against that check must + always come from the same measurement as the check's own reference, so route all + reads through here. + + With the steering-angle curvature measurement enabled (FordPrefSteerAngleCurvature + toggle -> FordSafetyFlagsSP.STEER_ANGLE_CURVATURE), the pinion angle via the vehicle + model is used instead: some vehicles (e.g. a 2021 Explorer with a faulty RCM) + broadcast implausible VehYaw_W_Actl (sign-inverted vs IMU and steering geometry) + while its CAN quality flag still reads OK. The pinion angle (SteeringPinion_Data, + PSCM) is an equivalent measurement, independently validated against the comma IMU + (corr +0.99), and the panda safety angle_meas switches to the same source (see + safety/modes/ford.h) -- the layers always agree. angleOffsetDeg/roll come from + liveParameters (paramsd, IMU-derived, not the car yaw sensor). + """ + if self.bp_pinion_curvature_enabled: + angle_offset_deg = self.lp.angleOffsetDeg if self.lp is not None else 0.0 + roll = self.lp.roll if self.lp is not None else 0.0 + return -self.VM.calc_curvature(math.radians(CS.out.steeringAngleDeg - angle_offset_deg), + CS.out.vEgoRaw, roll) + return -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + def update_sm(self): """Update SubMaster and vehicle model. Called each frame before lateral/long update.""" self.sm.update(0) @@ -285,7 +321,7 @@ def update(self, CC, CS, actuators, apply_curvature_last, CP): self.pc_blend_ratio_v = [self.pc_blend_ratio_low_C, self.pc_blend_ratio_high_C] # Current and desired curvature - current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1) + current_curvature = self.get_current_curvature(CS) desired_curvature = actuators.curvature # Extract predicted curvature from modelV2 diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/__init__.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py new file mode 100644 index 0000000000..a6650d9072 --- /dev/null +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py @@ -0,0 +1,206 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" + +# Unit tests for angle-mode shadow-curvature publishing (bp_kappa_cmd). +# +# The shadow value is consumed by carcontroller as the input to ford.h's angle-mode +# deviation check (Lane_Assist_Data1 bytes 5-6, judged against angle_meas). These tests +# pin the truthfulness contract: whenever the planner kappa cannot honestly describe the +# car's steering -- inactive, human-turn override, stall blip, driver pressing -- the +# published shadow must equal the measured curvature, so the panda-latched value always +# stays inside the check's band and re-engage frames never compare a stale zero against +# real measured curvature. + +import math +import unittest +from dataclasses import dataclass +from unittest import mock + +from opendbc.car import structs +from opendbc.car.ford.values import CarControllerParams +from opendbc.car.interfaces import scale_tire_stiffness +from opendbc.sunnypilot.car.ford import lateral_curv_ext +from opendbc.sunnypilot.car.ford.values_ext import FordSafetyFlagsSP +from opendbc.sunnypilot.car.ford.lateral_curv_ext import LateralCurvExt +from opendbc.sunnypilot.car.ford.lateral_angle_ext import LateralAngleExt + + +def _explorer_cp(): + CP = structs.CarParams() + CP.mass = 2050. + CP.wheelbase = 3.025 + CP.steerRatio = 16.8 + CP.centerToFront = CP.wheelbase * 0.44 + CP.tireStiffnessFactor = 0.82 + CP.tireStiffnessFront, CP.tireStiffnessRear = scale_tire_stiffness( + CP.mass, CP.wheelbase, CP.centerToFront, CP.tireStiffnessFactor) + return CP + + +class _FakeLiveDelay: + lateralDelay = 0.2 + + +class _FakeSubMaster: + def __init__(self, *args, **kwargs): + self.updated = {s: False for s in ('modelV2', 'liveParameters', 'selfdriveState', 'radarState', 'liveDelay')} + + def update(self, timeout=0): + pass + + def __getitem__(self, key): + if key == 'liveDelay': + return _FakeLiveDelay() + raise KeyError(key) + + +class _ForcedDetector: + def __init__(self, active): + self.active = active + + def update(self, *_args): + return self.active + + def reset(self): + pass + + +@dataclass +class _CSOut: + vEgoRaw: float = 15.0 + vEgo: float = 15.0 + steeringPressed: bool = False + steeringAngleDeg: float = 0.0 + yawRate: float = 0.0 + + +class _CS: + def __init__(self, **kwargs): + self.out = _CSOut(**kwargs) + self.lat_ctl_lim_stat = 0 + + +@dataclass +class _CC: + latActive: bool = True + + +@dataclass +class _Actuators: + curvature: float = 0.0 + + +class _Harness(LateralCurvExt, LateralAngleExt): + """Mirrors CarController's mixin composition (see carcontroller.py).""" + + def __init__(self, CP, CP_SP=None): + with mock.patch.object(lateral_curv_ext.messaging, 'SubMaster', _FakeSubMaster): + LateralCurvExt.__init__(self, CP, CP_SP) + LateralAngleExt.__init__(self, CP, CP_SP) + + +def _pinion_harness(flag): + """Harness with the STEER_ANGLE_CURVATURE flag set (or not) on CP_SP, detector stubbed.""" + CP = _explorer_cp() + CP_SP = structs.CarParamsSP() + if flag: + CP_SP.safetyParam |= FordSafetyFlagsSP.STEER_ANGLE_CURVATURE + ext = _Harness(CP, CP_SP) + ext.human_turn_detector = _ForcedDetector(False) + return ext, CP + + +class TestShadowCurvaturePublishing(unittest.TestCase): + V_EGO = 15.0 + YAW_RATE = 0.75 # rad/s -> measured curvature = -0.75 / 15 = -0.05 (OP convention) + + def setUp(self): + self.CP = _explorer_cp() + self.ext = _Harness(self.CP) + self.ext.human_turn_detector = _ForcedDetector(False) + self.cs = _CS(vEgoRaw=self.V_EGO, vEgo=self.V_EGO, yawRate=self.YAW_RATE) + self.measured = -self.YAW_RATE / self.V_EGO + + def _update(self, lat_active=True): + return self.ext.update_angle_strategy(_CC(latActive=lat_active), self.cs, _Actuators(curvature=0.01), self.CP) + + def test_inactive_publishes_measured(self): + result = self._update(lat_active=False) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_human_turn_override_publishes_measured(self): + self.ext.human_turn_detector = _ForcedDetector(True) + result = self._update() + self.assertTrue(self.ext.angle_human_turn_active) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_stall_blip_publishes_measured(self): + self.ext.stall_blip_frames_left = 3 + result = self._update() + self.assertTrue(self.ext.angle_stall_blip_active) + self.assertEqual(result.path_angle, 0.0) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_pressed_publishes_measured(self): + self.cs.out.steeringPressed = True + self._update() + self.assertFalse(self.ext.angle_human_turn_active) + self.assertAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + + def test_hands_off_publishes_clipped_planner_kappa(self): + # planner wants +0.01 while measured is -0.05: the deviation clip (active above 9 m/s) + # bounds the shadow to measured + CURVATURE_ERROR, not measured itself -- hands-off + # behavior is unchanged by the truthful-shadow sites. + self._update() + expected = self.measured + CarControllerParams.CURVATURE_ERROR + self.assertAlmostEqual(self.ext.bp_kappa_cmd, expected) + self.assertNotAlmostEqual(self.ext.bp_kappa_cmd, self.measured) + self.assertTrue(self.ext.bp_curvature_deviation_limited) + + +class TestMeasurementSelection(unittest.TestCase): + """get_current_curvature must select by the CP_SP STEER_ANGLE_CURVATURE flag: yaw rate + by default (stock ford.h angle_meas family), pinion angle via the vehicle model when + the steering-angle curvature measurement is enabled (pinion ford.h angle_meas family). + """ + + V_EGO = 15.0 + + def test_default_is_yaw_rate(self): + ext, _ = _pinion_harness(flag=False) + cs = _CS(vEgoRaw=self.V_EGO, yawRate=0.75, steeringAngleDeg=30.0) + self.assertFalse(ext.bp_pinion_curvature_enabled) + self.assertAlmostEqual(ext.get_current_curvature(cs), -0.75 / self.V_EGO) + + def test_flag_selects_pinion_vehicle_model(self): + from opendbc.car.vehicle_model import VehicleModel + ext, CP = _pinion_harness(flag=True) + cs = _CS(vEgoRaw=self.V_EGO, yawRate=0.75, steeringAngleDeg=30.0) + self.assertTrue(ext.bp_pinion_curvature_enabled) + expected = -VehicleModel(CP).calc_curvature(math.radians(30.0), self.V_EGO, 0.0) + self.assertAlmostEqual(ext.get_current_curvature(cs), expected) + self.assertNotAlmostEqual(ext.get_current_curvature(cs), -0.75 / self.V_EGO) + + +class TestInitializeFord(unittest.TestCase): + def test_safety_param_stays_a_plain_int(self): + """card serializes CP_SP to capnp, which rejects enum subclasses of int -- an + IntFlag-typed safetyParam crashed card on-device. Pin the exact type.""" + from opendbc.sunnypilot.car.interfaces import _initialize_ford + CP = structs.CarParams() + CP.brand = 'ford' + CP.carFingerprint = 'FORD_EXPLORER_MK6' + CP_SP = structs.CarParamsSP() + _initialize_ford(CP, CP_SP, {"FordPrefSteerAngleCurvature": True}) + self.assertEqual(CP_SP.safetyParam, 0xb) # flag | (explorer index 5 << 1) + self.assertIs(type(CP_SP.safetyParam), int) + + +if __name__ == '__main__': + unittest.main() diff --git a/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py b/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py index 5d7ff8de7b..a748a6f97c 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py +++ b/opendbc_repo/opendbc/sunnypilot/car/ford/values_ext.py @@ -9,6 +9,7 @@ from opendbc.car import structs from opendbc.car.docs_definitions import CarParts, Device +from opendbc.car.ford.values import CAR from opendbc.car.lateral import AngleSteeringLimits ButtonType = structs.CarState.ButtonEvent.Type @@ -41,6 +42,44 @@ ] +class FordSafetyFlagsSP: + """Sunnypilot-level safety flags for Ford. + + Carried in CP_SP.safetyParam and delivered to the safety firmware as + current_safety_param_sp (the separate SP uint16, USB control 0xdf) -- NOT the main + safetyConfigs[].safetyParam. ford_init reads it with GET_FLAG(current_safety_param_sp, + ...), same pattern as Subaru STOP_AND_GO (subaru_common.h). Plain int constants, not + IntFlag: CP_SP.safetyParam must stay a plain int through capnp serialization in card. + """ + STEER_ANGLE_CURVATURE = 1 + + +# Geometry-table index for the steering-angle curvature measurement, packed into +# CP_SP.safetyParam bits 1-4 when STEER_ANGLE_CURVATURE is set. Must match the +# ford_pinion_geometry table in safety/modes/ford.h row for row (enforced by +# test_ford.py's geometry-consistency test against CarSpecs + calc_slip_factor). +# Index 0 is reserved as invalid: the firmware treats flag-set-but-no-index as feature +# off, so a half-configured param can never select the wrong geometry silently. +# FORD_EDGE_MK2 is deliberately absent: ALT_STEER_ANGLE platforms read a RELATIVE pinion +# angle (SteeringPinion_Data_Alt + learned offset) and lack the absolute measurement +# this feature needs -- the toggle no-ops there and yaw behavior is kept. +FORD_PINION_GEOMETRY_SHIFT = 1 +FORD_PINION_GEOMETRY_INDEX = { + CAR.FORD_BRONCO_SPORT_MK1: 1, + CAR.FORD_ESCAPE_MK4: 2, + CAR.FORD_ESCAPE_MK4_5: 3, + CAR.FORD_EXPEDITION_MK4: 4, + CAR.FORD_EXPLORER_MK6: 5, + CAR.FORD_FOCUS_MK4: 6, + CAR.FORD_F_150_LIGHTNING_MK1: 7, + CAR.FORD_F_150_MK14: 8, + CAR.FORD_MAVERICK_MK1: 9, + CAR.FORD_MONDEO_MK5: 10, + CAR.FORD_MUSTANG_MACH_E_MK1: 11, + CAR.FORD_RANGER_MK2: 12, +} + + # BluePilot: Max curvature for steering command (m^-1), from DBC file limits CURVATURE_MAX = 0.02 diff --git a/opendbc_repo/opendbc/sunnypilot/car/interfaces.py b/opendbc_repo/opendbc/sunnypilot/car/interfaces.py index 623ad0cc22..b235fccfb5 100644 --- a/opendbc_repo/opendbc/sunnypilot/car/interfaces.py +++ b/opendbc_repo/opendbc/sunnypilot/car/interfaces.py @@ -14,6 +14,7 @@ from opendbc.car.hyundai.values import HyundaiFlags from opendbc.car.subaru.values import SubaruFlags from opendbc.car.toyota.values import ToyotaSafetyFlags +from opendbc.sunnypilot.car.ford.values_ext import FORD_PINION_GEOMETRY_INDEX, FORD_PINION_GEOMETRY_SHIFT, FordSafetyFlagsSP from opendbc.sunnypilot.car.hyundai.enable_radar_tracks import enable_radar_tracks as hyundai_enable_radar_tracks from opendbc.sunnypilot.car.hyundai.longitudinal.helpers import LongitudinalTuningType from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP @@ -88,6 +89,7 @@ def setup_interfaces(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP, _initialize_radar_tracks(CP, CP_SP, can_recv, can_send) _initialize_stop_and_go(CP, CP_SP, params_dict) _initialize_toyota(CP, CP_SP, params_dict) + _initialize_ford(CP, CP_SP, params_dict) def _initialize_custom_longitudinal_tuning(CI, CP: structs.CarParams, CP_SP: structs.CarParamsSP, @@ -133,6 +135,21 @@ def _initialize_stop_and_go(CP: structs.CarParams, CP_SP: structs.CarParamsSP, p CP_SP.safetyParam |= SubaruSafetyFlagsSP.STOP_AND_GO +def _initialize_ford(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None: + # BluePilot: steering-angle curvature measurement (bad-yaw-sensor workaround). Sets the + # STEER_ANGLE_CURVATURE flag + the platform geometry-table index on CP_SP.safetyParam, + # which reaches the safety firmware as current_safety_param_sp (USB 0xdf); the control + # side mirrors the same flag (lateral_curv_ext.get_current_curvature). Platforms without + # a geometry row (FORD_EDGE_MK2: ALT_STEER_ANGLE reads a relative pinion angle) silently + # keep stock yaw behavior -- the toggle no-ops rather than half-configuring. + if CP.brand == 'ford': + steer_angle_curvature = int(params_dict.get("FordPrefSteerAngleCurvature", 0) or 0) == 1 + if steer_angle_curvature: + geometry_index = FORD_PINION_GEOMETRY_INDEX.get(CP.carFingerprint) + if geometry_index is not None: + CP_SP.safetyParam |= FordSafetyFlagsSP.STEER_ANGLE_CURVATURE | (geometry_index << FORD_PINION_GEOMETRY_SHIFT) + + def _initialize_toyota(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params_dict: dict[str, str]) -> None: if CP.brand == 'toyota': toyota_stock_long = int(params_dict.get("ToyotaEnforceStockLongitudinal", 0)) == 1 diff --git a/selfdrive/ui/bp/layouts/settings/bluepilot.py b/selfdrive/ui/bp/layouts/settings/bluepilot.py index 09652d2729..858b6b3efd 100644 --- a/selfdrive/ui/bp/layouts/settings/bluepilot.py +++ b/selfdrive/ui/bp/layouts/settings/bluepilot.py @@ -69,6 +69,7 @@ def __init__(self): # Toggle refresh list self._refresh_toggles = ( ("send_hands_free_cluster_msg", self._show_hands_free_ui), + ("FordPrefSteerAngleCurvature", self._steer_angle_curvature), ("BPDisableLaneLineStatusColor", self._disable_lane_line_status_color), ("BPHideCameraView", self._hide_camera_view), ("BPRadRacerTheme", self._rad_racer_theme), @@ -109,6 +110,17 @@ def _initialize_items(self): icon="monitoring.png" ) + # Ford steering-angle curvature measurement (bad-yaw-sensor workaround). Init-time: + # card reads it once at car init and mirrors it into the panda safety firmware, so a + # flip only takes effect after the next restart (safe to toggle any time). + self._steer_angle_curvature = toggle_item( + lambda: tr("Use Pinion Yaw Sensor"), + lambda: tr('Measures how the car is turning from the steering pinion angle sensor instead of a faulty RCM yaw sensor (symptoms: "Turn Exceeds Steering Limit" warnings, weak curve tracking, "Service AdvanceTrac"). Check with tools/ford_yaw_health_check.py. Applies the next time the car starts. Not available on the Edge.'), + initial_state=self._safe_get_bool(self._params, "FordPrefSteerAngleCurvature"), + callback=lambda state: self._toggle_callback(state, "FordPrefSteerAngleCurvature"), + icon="monitoring.png" + ) + # Lane line status color toggle (issue #109: option to keep lane lines grey instead of green when engaged) self._disable_lane_line_status_color = toggle_item( lambda: tr("Disable Lane Line Status Color"), @@ -642,6 +654,7 @@ def _section(title: str, items: list) -> list: ]) + _section(tr("Vehicle"), [ self._show_hands_free_ui, + self._steer_angle_curvature, self._vbatt_pause_charging, ]) + _section(tr("Audio"), [ diff --git a/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py b/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py index 9fd29be96f..454c482338 100644 --- a/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py +++ b/selfdrive/ui/bp/mici/layouts/settings/vehicle_mici.py @@ -17,15 +17,19 @@ def __init__(self, back_callback: Callable[[], None] | None = None): self._params = Params() self.show_hands_free_ui = BigParamControlBP("Show BlueCruise UI on Cluster", "send_hands_free_cluster_msg") + # Init-time param (read once at car init, mirrored into panda safety); takes effect after restart + self.steer_angle_curvature = BigParamControlBP("Use Pinion Yaw Sensor", "FordPrefSteerAngleCurvature") self.vbatt_pause_charging = BigParamFloatControl("12V Battery Limit", "vbatt_pause_charging", min=11.0, max=14.0, step=0.1) self._scroller.add_widgets([ self.show_hands_free_ui, + self.steer_angle_curvature, self.vbatt_pause_charging, ]) self._refresh_toggles = ( ("send_hands_free_cluster_msg", self.show_hands_free_ui), + ("FordPrefSteerAngleCurvature", self.steer_angle_curvature), ) ui_state.add_offroad_transition_callback(self._update_toggles) diff --git a/sunnypilot/selfdrive/car/interfaces.py b/sunnypilot/selfdrive/car/interfaces.py index 5be227c262..a1c0ab7c43 100644 --- a/sunnypilot/selfdrive/car/interfaces.py +++ b/sunnypilot/selfdrive/car/interfaces.py @@ -109,6 +109,11 @@ def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: def initialize_params(params) -> list[dict[str, Any]]: keys: list = [] + # ford + keys.extend([ + "FordPrefSteerAngleCurvature", + ]) + # hyundai keys.extend([ "HyundaiLongitudinalTuning", diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index ad5399196e..501833e3d1 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -2118,6 +2118,11 @@ "unit": "V" }, { + "key": "FordPrefSteerAngleCurvature", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "[Vehicle] Use Pinion Yaw Sensor", + "description": "Measures how the car is turning from the steering pinion angle sensor instead of the RCM yaw sensor, in both the control software and the panda safety firmware. Use this if your Ford has a faulty yaw sensor: frequent \"Turn Exceeds Steering Limit\" warnings and weak curve tracking with healthy steering, often alongside \"Service AdvanceTrac\" messages. Run tools/ford_yaw_health_check.py on a few drives to check. Takes effect the next time the car starts. Not available on the Edge (its pinion sensor only reports a relative angle).", "key": "BPUseCustomSounds", "widget": "toggle", "needs_onroad_cycle": true, diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index 95106e6577..3952730016 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -44,6 +44,19 @@ sections: max: 14.0 step: 0.1 unit: V + - key: FordPrefSteerAngleCurvature + widget: toggle + needs_onroad_cycle: true + title: '[Vehicle] Use Pinion Yaw Sensor' + description: 'Measures how the car is turning from the steering pinion angle sensor + instead of the RCM yaw sensor, in both the control software and the panda safety + firmware. Use this if your Ford has a faulty yaw sensor: frequent "Turn Exceeds + Steering Limit" warnings and weak curve tracking with healthy steering, often + alongside "Service AdvanceTrac" messages. Run tools/ford_yaw_health_check.py on a + few drives to check. Takes effect the next time the car starts. Not available on + the Edge (its pinion sensor only reports a relative angle).' + enablement: + - $ref: '#/macros/offroad' # --- Audio --- - key: BPUseCustomSounds widget: toggle diff --git a/tools/ford_lmc_safety_replay.py b/tools/ford_lmc_safety_replay.py new file mode 100644 index 0000000000..8621dbd9a3 --- /dev/null +++ b/tools/ford_lmc_safety_replay.py @@ -0,0 +1,488 @@ +#!/usr/bin/env python3 +"""Frame-exact replay of ford.h's steering TX-hook against real rlogs. + +Attributes real panda `safetyTxBlocked` increments to specific safety checks, with the +reset-bypass latch modeled. (An earlier shadow-check-only analysis omitted the latch and +mis-attributed re-engage-edge frames as blocked; this tool exists so firmware semantics +can't silently drift from the analysis again.) + +Ported line-for-line from opendbc/safety/modes/ford.h (CAN path). Models the +configuration the analyzed road-test routes were recorded with: angle_meas sourced from +SteeringPinion_Data (Explorer geometry) and the 0.003 error band of the fork's opt-in +steering-angle measurement. Stock yaw-sourced firmware differs only in the angle_meas +source and a 0.002 band -- swap rx_steering_pinion and MAX_ANGLE_ERROR to replay stock +routes. + rx state: vehicle_speed (BrakeSysFeatures, QF==3), angle_meas (SteeringPinion_Data, + QF==3, Explorer geometry) + LKA hook (0x3CA): action!=0 block; latches angle_mode_engaged + shadow_curvature + LMC hook (0x3D3): value limits, steer_angle_cmd_checks (curvature mode) + explicit + controls gate at curvature==0, shadow-curvature check (angle mode), + path_angle/path_offset/curvature_rate ROC checks, reset-bypass latch + +Ground truth is independent of the sim: a `sendcan` frame with no matching TX loopback +echo in `can` (src >= 128) was actually blocked by panda; pandaStates.safetyTxBlocked +counter deltas cross-check the totals. The sim then explains each real block (which +check fired) and a no-latch counterfactual shows what the latch masked. + +Usage: + FORD_REPLAY_DONGLE_ID= tools/ford_lmc_safety_replay.py [ ...] + e.g. tools/ford_lmc_safety_replay.py 00000002--71f65bbf45 +Optional: --json to dump per-frame records for further analysis. +""" +import argparse +import json +import math +import os +from collections import defaultdict, deque + +from openpilot.tools.lib.logreader import LogReader + +DONGLE = os.environ.get('FORD_REPLAY_DONGLE_ID', '') + +# ---- ford.h constants (CAN path, FORD_LIMITS / FORD_*_LIMITS) ---- +VEHICLE_SPEED_FACTOR = 1000.0 +MAX_SAMPLE_VALS = 6 + +FORD_INACTIVE_CURVATURE = 1000 +FORD_INACTIVE_CURVATURE_RATE = 4096 +FORD_INACTIVE_PATH_OFFSET = 512 +FORD_INACTIVE_PATH_ANGLE = 1000 + +STEERING = dict( # FORD_LIMITS values, with the 150-unit (0.003) pinion band; stock band is 100 + max_angle=1000, deg_to_can=50000, max_angle_error=150, + rate_up=([5., 16., 25.], [0.0025, 0.0014, 0.00018]), + rate_down=([5., 16., 25.], [0.0025, 0.0014, 0.00018]), + angle_error_min_speed=10.0, +) +PATH_ANGLE = dict( # FORD_PATH_ANGLE_LIMITS + deg_to_can=2000, rate_up=([10., 15., 25.], [0.0561, 0.04335, 0.00918]), +) +PATH_OFFSET = dict( # FORD_PATH_OFFSET_LIMITS + deg_to_can=100, rate_up=([5., 15., 25.], [0.05, 0.025, 0.01]), +) +CURV_RATE = dict( # FORD_CURVATURE_RATE_LIMITS_CAN + deg_to_can=4000000, rate_up=([5., 15., 25.], [0.05, 0.025, 0.01]), +) + +FORD_CURVATURE_MIN, FORD_CURVATURE_MAX = -0.02, 0.02 +FORD_CURVATURE_RATE_MIN, FORD_CURVATURE_RATE_MAX = -0.001024, 0.00102375 +FORD_PATH_OFFSET_MIN, FORD_PATH_OFFSET_MAX = -1.0, 1.0 +FORD_PATH_ANGLE_MIN, FORD_PATH_ANGLE_MAX = -0.25, 0.25 +FORD_DBC_PATH_ANGLE_MIN, FORD_DBC_PATH_ANGLE_MAX = -0.5, 0.5235 + +RESET_BYPASS_LATCH_DURATION = 60 + +# Explorer pinion->curvature geometry (slip factor / steer ratio / wheelbase) +SLIP, SR, WB = -0.00055447339, 16.8, 3.025 + +ADDR_LKA, ADDR_LMC = 0x3CA, 0x3D3 +ADDR_PINION, ADDR_BRAKE_SYS = 0x7E, 0x415 +ECHO_TIMEOUT_S = 0.5 + +CHECK_KEYS = ['v_curv_val', 'v_curv_rate_val', 'v_po_val', 'v_pa_val', 'v_curv_check', + 'v_controls_gate', 'v_shadow', 'v_pa_roc', 'v_po_roc', 'v_curv_rate_roc'] + + +def interp_hold(xy, x): + xs, ys = xy + if x <= xs[0]: + return ys[0] + for i in range(len(xs) - 1): + if x < xs[i + 1]: + dx = max(xs[i + 1] - xs[i], 0.0001) + return ys[i] + (ys[i + 1] - ys[i]) * (x - xs[i]) / dx + return ys[-1] + + +def limit_check(val, max_val, min_val): + return (val > max_val) or (val < min_val) + + +class Sample: + def __init__(self): + self.values = deque([0] * MAX_SAMPLE_VALS, maxlen=MAX_SAMPLE_VALS) + + def update(self, v): + self.values.appendleft(int(v)) + + @property + def min(self): + return min(self.values) + + @property + def max(self): + return max(self.values) + + @property + def latest(self): + return self.values[0] + + +class FordLmcSafetySim: + """Firmware state machine for the CAN LMC/LKA tx hooks + relevant rx state.""" + + def __init__(self): + self.vehicle_speed = Sample() + self.angle_meas = Sample() + self.desired_angle_last = 0 + self.desired_path_angle_last = 0 + self.desired_path_offset_last = 0 + self.desired_curvature_rate_last = 0 + self.reset_bypass_latch_counter = 0 + self.angle_mode_engaged = False + self.shadow_curvature_raw = 0 + # externally-fed panda state (from pandaStates log) + self.controls_allowed = False + self.controls_allowed_lateral = False + + # ---- rx side ---- + def rx_brake_sys_features(self, d): + if (d[2] >> 6) == 0x3: # VehVActlBrk_D_Qf + speed_ms = ((d[0] << 8) | d[1]) * 0.01 / 3.6 + self.vehicle_speed.update(round(speed_ms * VEHICLE_SPEED_FACTOR)) + + def rx_steering_pinion(self, d): + if ((d[5] >> 2) & 0x3) != 0x3: # StePinCompAnEst_D_Qf + return + angle_raw = ((d[2] & 0x7F) << 8) | d[3] + pinion_angle_rad = math.radians((angle_raw * 0.1) - 1600.0) + speed = max(self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR, 0.1) + curvature_factor = 1. / (1. - (SLIP * speed * speed)) / WB + current_curvature = pinion_angle_rad * curvature_factor / SR + self.angle_meas.update(round(current_curvature * STEERING['deg_to_can'])) + + # ---- tx side ---- + def tx_lka(self, d, pressed=False, truthful_shadow=False): + """Returns True if blocked. Latches angle-mode statics regardless (as firmware does). + + truthful_shadow: counterfactual for the truthful-shadow control fix -- on frames the + fix would publish the shadow from measured curvature (driver pressing, or the old + code's zeroed override/inactive frames), latch the measured value instead, at real + LKA cadence so latch-age timing skew is modeled faithfully. + """ + action = d[0] >> 5 + self.angle_mode_engaged = (d[4] & 0x1) != 0 + raw = (d[5] << 8) | d[6] + raw = raw - 0x10000 if raw >= 0x8000 else raw # int16 + if truthful_shadow and (pressed or raw == 0): + raw = int(self.angle_meas.latest * 20) # CAN units (2e-5) -> shadow raw units (1e-6) + self.shadow_curvature_raw = raw + return action != 0 + + def _steer_angle_cmd_checks(self, desired_angle, en, lim): + """lateral.h steer_angle_cmd_checks, angle_is_curvature=false, inactive_angle_is_zero=true.""" + violation = False + if (self.controls_allowed or self.controls_allowed_lateral) and en: + fudged_speed = (self.vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1. + delta_up = int(interp_hold(lim['rate_up'], fudged_speed) * lim['deg_to_can'] + 1.) + delta_down = int(interp_hold(lim['rate_down'], fudged_speed) * lim['deg_to_can'] + 1.) + last = self.desired_angle_last + highest = last + (delta_up if last > 0 else delta_down) + lowest = last - (delta_down if last >= 0 else delta_up) + if (self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR) > lim['angle_error_min_speed']: + fudged_speed_error = (self.vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1. + delta_up_rlx = int(interp_hold(lim['rate_up'], fudged_speed_error) * lim['deg_to_can'] - 1.) + delta_down_rlx = int(interp_hold(lim['rate_down'], fudged_speed_error) * lim['deg_to_can'] - 1.) + lowest_err = self.angle_meas.min - lim['max_angle_error'] - 1 + highest_err = self.angle_meas.max + lim['max_angle_error'] + 1 + if last > highest_err: + delta = delta_down_rlx if last >= 0 else delta_up_rlx + highest = max(last - delta, highest_err) + elif last < lowest_err: + delta = delta_down_rlx if last <= 0 else delta_up_rlx + lowest = min(last + delta, lowest_err) + else: + highest = min(highest, highest_err) + lowest = max(lowest, lowest_err) + lowest = min(max(lowest, -lim['max_angle']), lim['max_angle']) + highest = min(max(highest, -lim['max_angle']), lim['max_angle']) + violation |= limit_check(desired_angle, highest, lowest) + self.desired_angle_last = desired_angle + if not en: + violation |= desired_angle != 0 + # No angle control allowed when controls are not allowed (lateral.h:267-269) + if not (self.controls_allowed or self.controls_allowed_lateral): + violation |= en + # reset on violation or controls-not-allowed (lateral.h:271-277, inactive_angle_is_zero); + # firmware does this BEFORE the reset-bypass latch can clear the violation + if violation or not (self.controls_allowed or self.controls_allowed_lateral): + self.desired_angle_last = 0 + return violation + + def _roc_check(self, desired, last_attr, en, lim): + violation = False + if en: + speed = (self.vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1. + delta = int(interp_hold(lim['rate_up'], speed) * lim['deg_to_can'] + 1.) + last = getattr(self, last_attr) + violation |= limit_check(desired, last + delta, last - delta) + setattr(self, last_attr, desired) + if not en: + violation |= desired != 0 + return violation + + def _shadow_check(self, shadow_can, en, lim): + if en and (self.vehicle_speed.latest / VEHICLE_SPEED_FACTOR) > lim['angle_error_min_speed']: + return limit_check(shadow_can, self.angle_meas.max + lim['max_angle_error'] + 1, + self.angle_meas.min - lim['max_angle_error'] - 1) + return False + + def tx_lmc(self, d): + """Full LMC tx-hook. Returns a dict of per-check verdicts + final pre/post-latch.""" + en = ((d[4] >> 2) & 0x7) != 0 + raw_curvature = (d[0] << 3) | (d[1] >> 5) + raw_curvature_rate = ((d[1] & 0x1F) << 8) | d[2] + raw_path_angle = (d[3] << 3) | (d[4] >> 5) + raw_path_offset = (d[5] << 2) | (d[6] >> 6) + + curv = raw_curvature - FORD_INACTIVE_CURVATURE + curv_rate = raw_curvature_rate - FORD_INACTIVE_CURVATURE_RATE + pa = raw_path_angle - FORD_INACTIVE_PATH_ANGLE + po = raw_path_offset - FORD_INACTIVE_PATH_OFFSET + + r = {'en': en, 'curv': curv, 'pa': pa, 'po': po, 'curv_rate': curv_rate, + 'engaged': self.angle_mode_engaged, 'shadow_raw': self.shadow_curvature_raw, + 'shadow_can': None, 'meas_min': self.angle_meas.min, 'meas_max': self.angle_meas.max, + 'latch_pre': self.reset_bypass_latch_counter} + + # value limits + r['v_curv_val'] = limit_check(curv, int(FORD_CURVATURE_MAX * STEERING['deg_to_can']), + int(FORD_CURVATURE_MIN * STEERING['deg_to_can'])) + r['v_curv_rate_val'] = limit_check(curv_rate, int(FORD_CURVATURE_RATE_MAX * CURV_RATE['deg_to_can']), + int(FORD_CURVATURE_RATE_MIN * CURV_RATE['deg_to_can'])) + r['v_po_val'] = limit_check(po, int(FORD_PATH_OFFSET_MAX * PATH_OFFSET['deg_to_can']), + int(FORD_PATH_OFFSET_MIN * PATH_OFFSET['deg_to_can'])) + pa_min = FORD_DBC_PATH_ANGLE_MIN if self.angle_mode_engaged else FORD_PATH_ANGLE_MIN + pa_max = FORD_DBC_PATH_ANGLE_MAX if self.angle_mode_engaged else FORD_PATH_ANGLE_MAX + r['v_pa_val'] = limit_check(pa, int(pa_max * PATH_ANGLE['deg_to_can']), + int(pa_min * PATH_ANGLE['deg_to_can'])) + + # curvature checks: always call (keeps desired_angle_last in sync), apply if curv != 0 + curv_violation = self._steer_angle_cmd_checks(curv, en, STEERING) + if curv != 0: + r['v_curv_check'] = curv_violation + r['v_controls_gate'] = False + else: + r['v_curv_check'] = False + r['v_controls_gate'] = en and not (self.controls_allowed or self.controls_allowed_lateral) + + # angle mode's shadow-curvature deviation check + r['v_shadow'] = False + if curv == 0 and self.angle_mode_engaged: + shadow_can = int(float(self.shadow_curvature_raw) * 0.05) + r['shadow_can'] = shadow_can + r['v_shadow'] = self._shadow_check(shadow_can, en, STEERING) + + # ROC checks + r['v_pa_roc'] = self._roc_check(pa, 'desired_path_angle_last', en, PATH_ANGLE) + r['v_po_roc'] = self._roc_check(po, 'desired_path_offset_last', en, PATH_OFFSET) + r['v_curv_rate_roc'] = self._roc_check(curv_rate, 'desired_curvature_rate_last', en, CURV_RATE) + + violation = any(r[k] for k in CHECK_KEYS) + r['pre_latch_violation'] = violation + + # reset-bypass latch + if curv == 0 and pa == 0: + self.reset_bypass_latch_counter = RESET_BYPASS_LATCH_DURATION + violation = False + elif self.reset_bypass_latch_counter > 0: + self.reset_bypass_latch_counter -= 1 + violation = False + r['blocked'] = violation + return r + + +class EchoMatcher: + """Ground truth: sent frames that never echo back (src >= 128) were blocked by panda.""" + + def __init__(self): + self.pending = defaultdict(deque) # addr -> deque of (t, dat, seq) + self.recent_echoes = defaultdict(deque) # addr -> deque of (t, dat), reorder tolerance + self.blocked = [] # (t, addr, dat, seq) + self.sent = defaultdict(int) + self.echoed = defaultdict(int) + + def on_send(self, t, addr, dat, seq): + self.sent[addr] += 1 + dat = bytes(dat) + # tolerate log reordering: echo may have been logged just before the sendcan event + for i, (te, de) in enumerate(self.recent_echoes[addr]): + if de == dat and (t - te) < 0.2: + del self.recent_echoes[addr][i] + return + self.pending[addr].append((t, dat, seq)) + + def on_echo(self, t, addr, dat): + self.echoed[addr] += 1 + dat = bytes(dat) + q = self.pending[addr] + for i, (_ts, d, _seq) in enumerate(q): + if d == dat: + for _ in range(i): # frames sent before this one and never echoed -> blocked + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + q.popleft() + return + re = self.recent_echoes[addr] + re.append((t, dat)) + while len(re) > 8: + re.popleft() + + def expire(self, now): + for addr, q in self.pending.items(): + while q and (now - q[0][0]) > ECHO_TIMEOUT_S: + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + + def finish(self): + for addr, q in self.pending.items(): + while q: + tb, db, sb = q.popleft() + self.blocked.append((tb, addr, db, sb)) + + +def iter_route(route): + """Yield log events segment by segment, skipping segments that were never uploaded.""" + misses = 0 + seg = 0 + while misses < 3 and seg < 100: + try: + lr = LogReader(f'{DONGLE}|{route}/{seg}') + yield from lr + misses = 0 + except Exception as e: + print(f' (seg {seg} unavailable: {type(e).__name__})') + misses += 1 + seg += 1 + + +def run_route(route, json_path=None, truthful_shadow=False): + assert DONGLE, 'set FORD_REPLAY_DONGLE_ID' + sim = FordLmcSafetySim() + echo = EchoMatcher() + ctx = {'v_ego': 0.0, 'pressed': False, 'mads': False, 'safety_model': '', + 'controls_allowed': False, 'controls_allowed_lat': False} + t0 = None + seq = 0 + lmc_records = [] # (t, seq, record, ctx snapshot) + lka_blocks = [] + tx_blocked_counter = [] # (t, value) + latch_empty_frames = 0 + + for m in iter_route(route): + w = m.which() + t = m.logMonoTime * 1e-9 + if t0 is None: + t0 = t + + if w == 'can': + for c in m.can: + if c.src == 0: + if c.address == ADDR_BRAKE_SYS: + sim.rx_brake_sys_features(c.dat) + elif c.address == ADDR_PINION: + sim.rx_steering_pinion(c.dat) + elif c.src >= 128: + echo.on_echo(t, c.address, c.dat) + echo.expire(t) + elif w == 'sendcan': + for c in m.sendcan: + seq += 1 + echo.on_send(t, c.address, c.dat, seq) + if c.address == ADDR_LKA: + if sim.tx_lka(c.dat, pressed=ctx['pressed'], truthful_shadow=truthful_shadow): + lka_blocks.append((t, seq)) + elif c.address == ADDR_LMC: + if sim.reset_bypass_latch_counter == 0: + latch_empty_frames += 1 + rec = sim.tx_lmc(c.dat) + lmc_records.append((t, seq, rec, dict(ctx))) + elif w == 'carState': + ctx['v_ego'] = m.carState.vEgo + ctx['pressed'] = m.carState.steeringPressed + elif w == 'selfdriveStateSP': + ctx['mads'] = m.selfdriveStateSP.mads.active + elif w == 'pandaStates': + if len(m.pandaStates) > 0: + ps = m.pandaStates[0] + ctx['safety_model'] = str(ps.safetyModel) + ctx['controls_allowed'] = bool(ps.controlsAllowed) + ctx['controls_allowed_lat'] = bool(ps.controlsAllowedLateral) + sim.controls_allowed = ctx['controls_allowed'] + sim.controls_allowed_lateral = ctx['controls_allowed_lat'] + if not tx_blocked_counter or tx_blocked_counter[-1][1] != ps.safetyTxBlocked: + tx_blocked_counter.append((t, int(ps.safetyTxBlocked))) + echo.finish() + + # ---- report ---- + mode = ' [truthful-shadow counterfactual]' if truthful_shadow else '' + print(f'\n===== {route} ====={mode} (t0 mono = {t0:.1f}s)') + def rt(t): + return t - t0 + n = len(lmc_records) + pre = [x for x in lmc_records if x[2]['pre_latch_violation']] + post = [x for x in lmc_records if x[2]['blocked']] + print(f'LMC frames: {n}; latch empty at frame: {latch_empty_frames} ({100.0 * latch_empty_frames / max(n, 1):.1f}%)') + print(f'sim violations pre-latch (no-latch counterfactual): {len(pre)}; post-latch (predicted real blocks): {len(post)}') + for name, group in [('pre-latch', pre), ('post-latch', post)]: + if group: + counts = {k: sum(1 for _, _, r, _ in group if r[k]) for k in CHECK_KEYS} + print(f' {name} by check: ' + ', '.join(f'{k}={v}' for k, v in counts.items() if v)) + print(f'LKA action blocks (sim): {len(lka_blocks)}') + + print('echo ground truth per addr (sent / echoed / no-echo):') + blocked_by_addr = defaultdict(list) + for tb, addr, db, sb in echo.blocked: + blocked_by_addr[addr].append((tb, db, sb)) + for addr in sorted(echo.sent): + print(f' 0x{addr:X}: sent={echo.sent[addr]} echoed={echo.echoed[addr]} no-echo={len(blocked_by_addr.get(addr, []))}') + + if tx_blocked_counter: + print(f'safetyTxBlocked counter: start={tx_blocked_counter[0][1]}, end={tx_blocked_counter[-1][1]}') + for i in range(1, len(tx_blocked_counter)): + tprev, vprev = tx_blocked_counter[i - 1] + tcur, vcur = tx_blocked_counter[i] + print(f' t_route={rt(tcur):8.1f}s (mono {tcur:.1f}) counter {vprev} -> {vcur}') + + # join: actually-blocked LMC frames vs sim verdicts + blocked_seqs = {sb for _, addr, _, sb in echo.blocked if addr == ADDR_LMC} + print(f'actually-blocked LMC frames (no echo): {len(blocked_seqs)}') + for t, s, r, c in lmc_records: + if s in blocked_seqs or r['blocked'] or r['pre_latch_violation']: + fired = [k for k in CHECK_KEYS if r[k]] or ['NONE(unexplained)'] + tag = ('REAL+SIM' if (s in blocked_seqs and r['blocked']) else + 'REAL only' if s in blocked_seqs else + 'SIM block' if r['blocked'] else 'SIM pre-latch only') + line = (f' [{tag}] t_route={rt(t):8.1f}s en={int(r["en"])} curv={r["curv"]} pa={r["pa"]}' + + f' shadow={r["shadow_can"]} meas=[{r["meas_min"]},{r["meas_max"]}] latch={r["latch_pre"]}' + + f' checks={fired} v={c["v_ego"]:.1f} pressed={int(c["pressed"])} mads={int(c["mads"])}' + + f' ctl={int(c["controls_allowed"])}/{int(c["controls_allowed_lat"])} sm={c["safety_model"]}') + print(line) + # non-LMC real blocks, grouped + for addr, items in sorted(blocked_by_addr.items()): + if addr == ADDR_LMC: + continue + times = ', '.join(f'{rt(tb):.1f}' for tb, _, _ in items[:20]) + print(f' non-LMC no-echo 0x{addr:X}: n={len(items)} t_route=[{times}{", ..." if len(items) > 20 else ""}]') + + if json_path: + with open(json_path, 'w') as f: + json.dump({'route': route, 't0': t0, + 'lmc': [{'t': t, 'seq': s, **r, 'ctx': c} for t, s, r, c in lmc_records], + 'blocked': [[tb, addr, sb] for tb, addr, _, sb in echo.blocked], + 'tx_blocked_counter': tx_blocked_counter}, f) + print(f'wrote {json_path}') + return lmc_records, echo, tx_blocked_counter, t0 + + +if __name__ == '__main__': + ap = argparse.ArgumentParser() + ap.add_argument('routes', nargs='+') + ap.add_argument('--json', help='dump per-frame records (one file per route, suffixed)') + ap.add_argument('--truthful-shadow', action='store_true', + help=('counterfactual: latch the shadow from measured curvature on frames the ' + + 'truthful-shadow fix would republish (pressed / previously-zeroed)')) + args = ap.parse_args() + for route in args.routes: + jp = f'{args.json}.{route}.json' if args.json else None + run_route(route, jp, truthful_shadow=args.truthful_shadow) diff --git a/tools/ford_pinion_replay.py b/tools/ford_pinion_replay.py new file mode 100644 index 0000000000..4757c46a21 --- /dev/null +++ b/tools/ford_pinion_replay.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +"""Replay validation of the pinion-sourced curvature measurement against real rlogs. + +Simulates the STEER_ANGLE_CURVATURE firmware semantics per lateral frame: + - angle_meas: 6-sample buffer of pinion-derived curvature CAN units (Explorer + geometry row of the ford.h table, QF-gated) + - steer_angle_cmd_checks error section (curvature mode, gate 10 m/s, band 0.003): + outside [meas±err] AND not converging -> violation + - ford_shadow_curvature_error_check (angle mode, same gate/band, no converge term) + +Also compares the Python-layer deviation-clip bite rate between the vehicle-model +(pinion) measurement and the yaw-rate measurement. + +Usage: + FORD_REPLAY_DONGLE_ID= tools/ford_pinion_replay.py [:] ... + e.g. tools/ford_pinion_replay.py 00000001--255eabb4d0 000000d9--89f2063192:0:61:2 +""" +import os +import sys + +import numpy as np +from opendbc.can.parser import CANParser +from openpilot.tools.lib.logreader import LogReader + +DONGLE = os.environ.get('FORD_REPLAY_DONGLE_ID', '') # set to your comma dongle ID +DEG_TO_CAN = 50000 +MAX_ANGLE_ERR = 150 # 0.003, the pinion-path band +GATE = 10.0 # m/s +SLIP, SR, WB = -0.00055447339, 16.8, 3.025 # ford.h geometry table, FORD_EXPLORER_MK6 row +CURVATURE_ERROR = 0.002 # python layer band + + +def pinion_curv(angle_deg, speed): + s = np.maximum(speed, 0.1) + cf = 1. / (1. - (SLIP * s * s)) / WB + return np.radians(angle_deg) * cf / SR + + +def run_route(route, segs): + rows = [] + for seg in segs: + try: + lr = LogReader(f'{DONGLE}|{route}/{seg}') + except Exception: + continue + cp_tx = CANParser('ford_lincoln_base_pt', [('LateralMotionControl', 0)], 0) + cp_rx = CANParser('ford_lincoln_base_pt', [('SteeringPinion_Data', 0)], 0) + last = {'pin': None, 'qf': 0, 'v': 0.0, 'eng': False, 'sp': False, + 'des': 0.0, 'yaw': 0.0, 'sa': 0.0} + for m in lr: + w = m.which() + if w == 'can': + frames = [(c.address, c.dat, c.src) for c in m.can] + if cp_rx.update([(m.logMonoTime, frames)]): + last['pin'] = cp_rx.vl['SteeringPinion_Data']['StePinComp_An_Est'] + last['qf'] = int(cp_rx.vl['SteeringPinion_Data']['StePinCompAnEst_D_Qf']) + elif w == 'carState': + last['v'] = m.carState.vEgo + last['sp'] = m.carState.steeringPressed + last['yaw'] = m.carState.yawRate + last['sa'] = m.carState.steeringAngleDeg + elif w == 'selfdriveStateSP': + last['eng'] = m.selfdriveStateSP.mads.active + elif w == 'carControl': + last['des'] = m.carControl.actuators.curvature + elif w == 'sendcan': + frames = [(c.address, c.dat, c.src) for c in m.sendcan] + if cp_tx.update([(m.logMonoTime, frames)]) and last['pin'] is not None: + vl = cp_tx.vl['LateralMotionControl'] + rows.append((last['v'], last['pin'], last['qf'], + vl['LatCtlCurv_No_Actl'], vl['LatCtlPath_An_Actl'], + last['eng'], last['sp'], last['des'], last['yaw'], last['sa'])) + if not rows: + return None + a = np.array(rows, dtype=float) + v, pin, qf, cmd, pa, eng, sp, des, yaw, sa = (a[:, i] for i in range(10)) + eng = eng.astype(bool) + sp = sp.astype(bool) + + # firmware measurement: pinion -> curvature CAN units (sign: pinion correlates + with + # wire cmd; firmware keeps native sign) + k_pin = pinion_curv(pin, v) + # empirical sign alignment to wire cmd (like firmware: both Ford-native convention) + al = eng & (np.abs(cmd) > 0.001) + s = 1.0 + if al.sum() > 200: + s = np.sign(np.corrcoef(k_pin[al], cmd[al])[0, 1]) + meas_can = s * k_pin * DEG_TO_CAN + + # 6-frame rolling min/max of measurement (angle_meas buffer @ ~100 Hz vs 20 Hz cmd -- + # conservative: use per-cmd-frame value, window 6 cmd frames = wider window) + n = len(meas_can) + mn = np.empty(n) + mx = np.empty(n) + for i in range(n): + lo = max(0, i - 5) + mn[i] = meas_can[lo:i+1].min() + mx[i] = meas_can[lo:i+1].max() + + cmd_can = cmd * DEG_TO_CAN + dcmd = np.diff(cmd_can, prepend=cmd_can[0]) + hands_off = eng & ~sp & (v > GATE) & (qf == 3) + + # curvature mode check (with converge semantics) + above = cmd_can > (mx + MAX_ANGLE_ERR + 1) + below = cmd_can < (mn - MAX_ANGLE_ERR - 1) + viol_curv = (above & (dcmd >= 0)) | (below & (dcmd <= 0)) + m_curv = hands_off & (np.abs(cmd) > 1e-5) # curvature actually commanded + # shadow check (angle mode: cmd==0; shadow = desired kappa, no converge term) + shadow_can = -des * DEG_TO_CAN * s * np.sign(np.corrcoef(-des[al], cmd[al])[0, 1]) if al.sum() > 200 else -des * DEG_TO_CAN + viol_shadow = (shadow_can > (mx + MAX_ANGLE_ERR + 1)) | (shadow_can < (mn - MAX_ANGLE_ERR - 1)) + m_shadow = hands_off & (np.abs(cmd) <= 1e-5) & (np.abs(pa) > 1e-4) # angle mode active frames + + out = {} + out['frames'] = n + out['curv_mode_frames'] = int(m_curv.sum()) + out['curv_fault_pct'] = round(100 * float(viol_curv[m_curv].mean()), 3) if m_curv.sum() else None + out['angle_mode_frames'] = int(m_shadow.sum()) + out['shadow_fault_pct'] = round(100 * float(viol_shadow[m_shadow].mean()), 3) if m_shadow.sum() else None + # speed split for curvature mode (the historical 20-35mph zone) + for lo, hi, lbl in [(10, 15.6, '22-35mph'), (15.6, 30, '>35mph')]: + mm = m_curv & (v >= lo) & (v < hi) + out[f'curv_fault_{lbl}'] = round(100 * float(viol_curv[mm].mean()), 3) if mm.sum() > 100 else f'n={mm.sum()}' + ms = m_shadow & (v >= lo) & (v < hi) + out[f'shadow_fault_{lbl}'] = round(100 * float(viol_shadow[ms].mean()), 3) if ms.sum() > 100 else f'n={ms.sum()}' + + # Python layer: clip-bite with the VM measurement (no offset -- conservative) vs yaw + k_vm = -np.radians(sa) / (SR * WB) / np.maximum(1 - SLIP * v * v, 0.3) + k_yaw = -yaw / np.maximum(v, 0.1) + mm = hands_off & (np.abs(des) > 0.0015) & (v > 9) + for name, k in [('vm', k_vm), ('yaw', k_yaw)]: + clipped = (des > k + CURVATURE_ERROR) | (des < k - CURVATURE_ERROR) + out[f'py_clip_bite_{name}_pct'] = round(100 * float(clipped[mm].mean()), 1) if mm.sum() else None + return out + + +def _parse_route_arg(arg): + parts = arg.split(':') + segs = range(100) if len(parts) == 1 else range(*[int(x) for x in parts[1:]]) + return parts[0], segs + + +if __name__ == '__main__': + if len(sys.argv) < 2 or not DONGLE: + print(__doc__) + sys.exit(2) + for route_arg in sys.argv[1:]: + route, segs = _parse_route_arg(route_arg) + print(f'\n===== {route} =====') + r = run_route(route, segs) + if r is None: + print(' no data loaded (are the rlogs uploaded?)') + continue + for key, val in r.items(): + print(f' {key}: {val}') diff --git a/tools/ford_yaw_health_check.py b/tools/ford_yaw_health_check.py new file mode 100644 index 0000000000..63b78dab90 --- /dev/null +++ b/tools/ford_yaw_health_check.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +"""Ford RCM yaw-sensor health check. + +Some Ford RCMs broadcast implausible yaw rate (e.g. sign-inverted vs the IMU and the +steering geometry) while the signal's quality flag still reads OK. Since the stock +safety firmware and control code measure curvature from this yaw signal, a bad sensor +shows up as "steering limit exceeded" alerts at 20-35 mph and weak curve tracking. + +This tool answers "is MY yaw sensor healthy?" from any drive's logs: + - yaw_geo: yaw rate implied by the steering angle via the vehicle model (reference) + - yaw_can: the RCM yaw rate the car broadcasts (what stock openpilot trusts) + - yaw_imu: the comma device gyro (independent cross-check) + +A healthy sensor tracks the steering geometry in BOTH correlation AND gain: +corr(yaw_can, yaw_geo) > +0.9 with a median ratio near +1.0. The failure mode measured +on a faulty 2021 Explorer RCM was unstable gain -- across five months the ratio wandered ++0.42 / +0.45 / +0.62 / +0.71 / +0.92 / +1.80 / +2.11 (including two drives on the SAME +day at +2.11 and +0.45), with high positive correlation throughout -- which corrupts the +curvature measurement just as badly as a sign flip (a 2x gain error at curvature 0.005 +is 2.5x the safety check's whole error band). A negative correlation (sign inversion) +is also broken. Because the gain wanders BETWEEN drives and occasionally passes through ++1.0 (a known-broken sensor measured +0.92 on one drive), ALWAYS run this on two or +three different routes: a ratio that moves is itself the fault signature. + +Usage: + tools/ford_yaw_health_check.py '|' + (works with rlogs; for qlog fallback append the LogReader qlog selector to the route) +""" +import math +import sys + +import numpy as np + +from openpilot.tools.lib.logreader import LogReader + +MIN_SPEED = 5.0 # m/s; below this, geometry and yaw are both noise +SIGN_THRESH = 0.01 # rad/s; only count sign agreement when clearly turning +HEALTHY_CORR = 0.9 +BROKEN_CORR = 0.5 +# gain (median yaw_can / yaw_geo): healthy sits within vehicle-model tolerance of +1.0; +# bounds set from the faulty-RCM measurements in the module docstring +HEALTHY_RATIO = (0.85, 1.2) +BROKEN_RATIO = (0.65, 1.5) + + +def run(identifier): + CP = None + angle_offset_deg = 0.0 + rows = [] # v, steering_angle_deg, yaw_can, yaw_imu + yaw_imu = float('nan') + + for m in LogReader(identifier): + w = m.which() + if w == 'carParams' and CP is None: + CP = m.carParams + elif w == 'liveParameters': + angle_offset_deg = m.liveParameters.angleOffsetAverageDeg + elif w == 'livePose': + yaw_imu = m.livePose.angularVelocityDevice.z + elif w == 'carState': + cs = m.carState + rows.append((cs.vEgo, cs.steeringAngleDeg - angle_offset_deg, cs.yawRate, yaw_imu)) + + if CP is None or not rows: + print('no carParams/carState in logs -- is this a full route identifier?') + return 2 + if CP.brand != 'ford': + print(f'not a Ford route (brand={CP.brand})') + return 2 + + from opendbc.car.vehicle_model import VehicleModel + VM = VehicleModel(CP) + + a = np.array(rows, dtype=float) + v, sa, yaw_can, imu = a.T + moving = v > MIN_SPEED + if moving.sum() < 500: + print(f'only {moving.sum()} moving samples -- drive longer for a reliable verdict') + return 2 + + yaw_geo = np.array([VM.calc_curvature(math.radians(s), vv, 0.0) * vv for s, vv in zip(sa, v, strict=True)]) + + def corr(x, y, mask): + mask = mask & np.isfinite(x) & np.isfinite(y) + if mask.sum() < 100 or np.std(x[mask]) < 1e-9 or np.std(y[mask]) < 1e-9: + return float('nan') + return float(np.corrcoef(x[mask], y[mask])[0, 1]) + + c_can = corr(yaw_can, yaw_geo, moving) + c_imu_raw = corr(imu, yaw_geo, moving) + # the device gyro's z sign depends on mount orientation; orient it to the geometry + imu_oriented = imu * (1.0 if (np.isnan(c_imu_raw) or c_imu_raw >= 0) else -1.0) + c_imu = corr(imu_oriented, yaw_geo, moving) + + turning = moving & (np.abs(yaw_geo) > SIGN_THRESH) + sign_disagree = float(np.mean(np.sign(yaw_can[turning]) != np.sign(yaw_geo[turning]))) if turning.sum() > 100 else float('nan') + with np.errstate(divide='ignore', invalid='ignore'): + ratio = yaw_can[turning] / yaw_geo[turning] + median_ratio = float(np.median(ratio)) if turning.sum() > 100 else float('nan') + + print(f'route: {identifier}') + print(f'platform: {CP.carFingerprint} moving samples: {int(moving.sum())} turning samples: {int(turning.sum())}') + print(f'corr(RCM yaw, steering geometry): {c_can:+.3f} (healthy > {HEALTHY_CORR:+.1f})') + print(f'corr(IMU yaw, steering geometry): {c_imu:+.3f} (sanity reference; should be > +0.9)') + print(f'sign disagreement while turning: {100.0 * sign_disagree:.1f}%') + print(f'median yaw_can / yaw_geo ratio: {median_ratio:+.2f} (healthy ~ +1.0)') + + if not np.isnan(c_imu) and c_imu < HEALTHY_CORR: + print('\nVERDICT: INCONCLUSIVE -- the IMU itself disagrees with steering geometry;') + print('check device mount/calibration before trusting this run.') + return 2 + if c_can < BROKEN_CORR: + print('\nVERDICT: BROKEN -- your RCM yaw output does not track the steering geometry') + print('(inverted or uncorrelated). Enable the "Use Pinion Yaw Sensor" toggle;') + print('expect "Service AdvanceTrac"-style symptoms to correlate.') + return 1 + if not np.isnan(median_ratio) and not (BROKEN_RATIO[0] <= median_ratio <= BROKEN_RATIO[1]): + print('\nVERDICT: BROKEN -- your RCM yaw tracks the steering geometry in shape but at') + print(f'the wrong gain ({median_ratio:+.2f}x instead of ~+1.0x). This is the measured') + print('faulty-Explorer-RCM signature (gain wandered between +0.45x and +2.11x across') + print('same-day drives) and corrupts the curvature measurement as badly as a sign flip.') + print('Enable the "Use Pinion Yaw Sensor" toggle.') + return 1 + if c_can >= HEALTHY_CORR and HEALTHY_RATIO[0] <= median_ratio <= HEALTHY_RATIO[1]: + print('\nVERDICT: HEALTHY -- your yaw sensor tracks the steering geometry in shape and') + print('gain. You do not need the "Use Pinion Yaw Sensor" toggle. (Consider') + print('re-running on one or two more drives: a gain that moves between drives is the') + print('fault signature even when a single drive looks acceptable.)') + return 0 + print('\nVERDICT: MARGINAL -- correlation or gain is off but not conclusively broken;') + print('re-run on a longer, curvier drive (and compare the gain across drives) before deciding.') + return 2 + + +if __name__ == '__main__': + if len(sys.argv) != 2: + print(__doc__) + sys.exit(2) + sys.exit(run(sys.argv[1]))