mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-01 03:24:05 +08:00
Preserve Bolt lateral intent during driver overrides
This commit is contained in:
firestar5683
@@ -61,7 +61,7 @@ from openpilot.starpilot.aol.wire import IntentState, encode_intent
|
||||
from openpilot.starpilot.conditional_mode.manual import Button, ButtonTracker, IoniqMediaMapCache, WheelMapCache, Press, ioniq6_media_eligible
|
||||
from openpilot.starpilot.longitudinal.ioniq6_start import eligible as ioniq6_long_eligible
|
||||
from opendbc.car.gm.aol import native_bootstrap_supported, qualified_gm
|
||||
from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, is_volt_ascm_longitudinal, is_volt_sdgm_profile
|
||||
from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, is_volt_ascm_longitudinal, is_volt_sdgm_profile, is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.gm.lateral import lane_centering_supported as gm_lane_centering_supported
|
||||
from opendbc.car.gm.ordinary_cc import control_transport_required as gm_cc_transport_required
|
||||
from openpilot.starpilot.longitudinal.toyota_output_policy import CLOCK_PAIR_MAX_SKEW_NS, clock_pair_ns
|
||||
@@ -461,7 +461,9 @@ class Car:
|
||||
self.timing_mark('interface_update_end')
|
||||
cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME)
|
||||
if (getattr(self.CI.CS, 'ev6_aol_source_healthy', False) or
|
||||
qualified_honda_stock_aol(self.CP, marked_only=True)) else 0)
|
||||
qualified_honda_stock_aol(self.CP, marked_only=True) or
|
||||
qualified_gm(self.CP) and is_bolt_present_no_acc_pedal_profile(self.CP) and
|
||||
int(self.CP.alternativeExperience) == 32) else 0)
|
||||
self.observe_ioniq6_long_authority(can_list, CS)
|
||||
self.timing_mark('ioniq_authority_end')
|
||||
media_owner = getattr(self, 'ioniq6_media', None)
|
||||
@@ -534,6 +536,7 @@ class Car:
|
||||
transport_reset = self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
|
||||
latched=self.aol_card_intent.allowed_latch, cancel_source_ns=self.aol_cancel_source(CS, now_ns),
|
||||
brake_source_ns=self.aol_honda_brake_source(CS, now_ns),
|
||||
gm_withdrawal_source_ns=self.aol_gm_withdrawal_source(CS, now_ns),
|
||||
intent_producer_session=getattr(self, 'slc_producer_session', ''))
|
||||
fault_active = True if transport_reset else None
|
||||
event_ns = int(self.sm.logMonoTime['onroadEvents'])
|
||||
@@ -762,6 +765,15 @@ class Car:
|
||||
return 0
|
||||
return received_ns
|
||||
|
||||
def aol_gm_withdrawal_source(self, CS, now_ns: int) -> int:
|
||||
if self.CP.brand != 'gm':
|
||||
return 0
|
||||
from opendbc.car import Bus
|
||||
from openpilot.starpilot.car.gm.aol import physical_longitudinal_withdrawal_source
|
||||
parser = getattr(getattr(self, 'CI', None), 'can_parsers', {}).get(Bus.pt)
|
||||
return physical_longitudinal_withdrawal_source(self.CP, CS, parser,
|
||||
getattr(self, '_aol_can_observation', None), now_ns)
|
||||
|
||||
def aol_disarming_fault(self, CS, event_ns: int, now_ns: int) -> bool:
|
||||
from openpilot.starpilot.car.gm.aol import intent_disarming_fault
|
||||
if getattr(self, 'aol_transport_feedback', None) is None:
|
||||
@@ -769,6 +781,7 @@ class Car:
|
||||
if self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns,
|
||||
latched=getattr(getattr(self, 'aol_card_intent', None), 'allowed_latch', True),
|
||||
cancel_source_ns=self.aol_cancel_source(CS, now_ns), brake_source_ns=self.aol_honda_brake_source(CS, now_ns),
|
||||
gm_withdrawal_source_ns=self.aol_gm_withdrawal_source(CS, now_ns),
|
||||
intent_producer_session=getattr(self, 'slc_producer_session', '')):
|
||||
return True
|
||||
events = self.sm['onroadEvents']
|
||||
|
||||
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_8771745385019272911) {
|
||||
out_8771745385019272911[0] = delta_x[0] + nom_x[0];
|
||||
out_8771745385019272911[1] = delta_x[1] + nom_x[1];
|
||||
out_8771745385019272911[2] = delta_x[2] + nom_x[2];
|
||||
out_8771745385019272911[3] = delta_x[3] + nom_x[3];
|
||||
out_8771745385019272911[4] = delta_x[4] + nom_x[4];
|
||||
out_8771745385019272911[5] = delta_x[5] + nom_x[5];
|
||||
out_8771745385019272911[6] = delta_x[6] + nom_x[6];
|
||||
out_8771745385019272911[7] = delta_x[7] + nom_x[7];
|
||||
out_8771745385019272911[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7126582961958922662) {
|
||||
out_7126582961958922662[0] = delta_x[0] + nom_x[0];
|
||||
out_7126582961958922662[1] = delta_x[1] + nom_x[1];
|
||||
out_7126582961958922662[2] = delta_x[2] + nom_x[2];
|
||||
out_7126582961958922662[3] = delta_x[3] + nom_x[3];
|
||||
out_7126582961958922662[4] = delta_x[4] + nom_x[4];
|
||||
out_7126582961958922662[5] = delta_x[5] + nom_x[5];
|
||||
out_7126582961958922662[6] = delta_x[6] + nom_x[6];
|
||||
out_7126582961958922662[7] = delta_x[7] + nom_x[7];
|
||||
out_7126582961958922662[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_3231363094273967832) {
|
||||
out_3231363094273967832[0] = -nom_x[0] + true_x[0];
|
||||
out_3231363094273967832[1] = -nom_x[1] + true_x[1];
|
||||
out_3231363094273967832[2] = -nom_x[2] + true_x[2];
|
||||
out_3231363094273967832[3] = -nom_x[3] + true_x[3];
|
||||
out_3231363094273967832[4] = -nom_x[4] + true_x[4];
|
||||
out_3231363094273967832[5] = -nom_x[5] + true_x[5];
|
||||
out_3231363094273967832[6] = -nom_x[6] + true_x[6];
|
||||
out_3231363094273967832[7] = -nom_x[7] + true_x[7];
|
||||
out_3231363094273967832[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_945533236592444203) {
|
||||
out_945533236592444203[0] = -nom_x[0] + true_x[0];
|
||||
out_945533236592444203[1] = -nom_x[1] + true_x[1];
|
||||
out_945533236592444203[2] = -nom_x[2] + true_x[2];
|
||||
out_945533236592444203[3] = -nom_x[3] + true_x[3];
|
||||
out_945533236592444203[4] = -nom_x[4] + true_x[4];
|
||||
out_945533236592444203[5] = -nom_x[5] + true_x[5];
|
||||
out_945533236592444203[6] = -nom_x[6] + true_x[6];
|
||||
out_945533236592444203[7] = -nom_x[7] + true_x[7];
|
||||
out_945533236592444203[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_5279329364050306118) {
|
||||
out_5279329364050306118[0] = 1.0;
|
||||
out_5279329364050306118[1] = 0.0;
|
||||
out_5279329364050306118[2] = 0.0;
|
||||
out_5279329364050306118[3] = 0.0;
|
||||
out_5279329364050306118[4] = 0.0;
|
||||
out_5279329364050306118[5] = 0.0;
|
||||
out_5279329364050306118[6] = 0.0;
|
||||
out_5279329364050306118[7] = 0.0;
|
||||
out_5279329364050306118[8] = 0.0;
|
||||
out_5279329364050306118[9] = 0.0;
|
||||
out_5279329364050306118[10] = 1.0;
|
||||
out_5279329364050306118[11] = 0.0;
|
||||
out_5279329364050306118[12] = 0.0;
|
||||
out_5279329364050306118[13] = 0.0;
|
||||
out_5279329364050306118[14] = 0.0;
|
||||
out_5279329364050306118[15] = 0.0;
|
||||
out_5279329364050306118[16] = 0.0;
|
||||
out_5279329364050306118[17] = 0.0;
|
||||
out_5279329364050306118[18] = 0.0;
|
||||
out_5279329364050306118[19] = 0.0;
|
||||
out_5279329364050306118[20] = 1.0;
|
||||
out_5279329364050306118[21] = 0.0;
|
||||
out_5279329364050306118[22] = 0.0;
|
||||
out_5279329364050306118[23] = 0.0;
|
||||
out_5279329364050306118[24] = 0.0;
|
||||
out_5279329364050306118[25] = 0.0;
|
||||
out_5279329364050306118[26] = 0.0;
|
||||
out_5279329364050306118[27] = 0.0;
|
||||
out_5279329364050306118[28] = 0.0;
|
||||
out_5279329364050306118[29] = 0.0;
|
||||
out_5279329364050306118[30] = 1.0;
|
||||
out_5279329364050306118[31] = 0.0;
|
||||
out_5279329364050306118[32] = 0.0;
|
||||
out_5279329364050306118[33] = 0.0;
|
||||
out_5279329364050306118[34] = 0.0;
|
||||
out_5279329364050306118[35] = 0.0;
|
||||
out_5279329364050306118[36] = 0.0;
|
||||
out_5279329364050306118[37] = 0.0;
|
||||
out_5279329364050306118[38] = 0.0;
|
||||
out_5279329364050306118[39] = 0.0;
|
||||
out_5279329364050306118[40] = 1.0;
|
||||
out_5279329364050306118[41] = 0.0;
|
||||
out_5279329364050306118[42] = 0.0;
|
||||
out_5279329364050306118[43] = 0.0;
|
||||
out_5279329364050306118[44] = 0.0;
|
||||
out_5279329364050306118[45] = 0.0;
|
||||
out_5279329364050306118[46] = 0.0;
|
||||
out_5279329364050306118[47] = 0.0;
|
||||
out_5279329364050306118[48] = 0.0;
|
||||
out_5279329364050306118[49] = 0.0;
|
||||
out_5279329364050306118[50] = 1.0;
|
||||
out_5279329364050306118[51] = 0.0;
|
||||
out_5279329364050306118[52] = 0.0;
|
||||
out_5279329364050306118[53] = 0.0;
|
||||
out_5279329364050306118[54] = 0.0;
|
||||
out_5279329364050306118[55] = 0.0;
|
||||
out_5279329364050306118[56] = 0.0;
|
||||
out_5279329364050306118[57] = 0.0;
|
||||
out_5279329364050306118[58] = 0.0;
|
||||
out_5279329364050306118[59] = 0.0;
|
||||
out_5279329364050306118[60] = 1.0;
|
||||
out_5279329364050306118[61] = 0.0;
|
||||
out_5279329364050306118[62] = 0.0;
|
||||
out_5279329364050306118[63] = 0.0;
|
||||
out_5279329364050306118[64] = 0.0;
|
||||
out_5279329364050306118[65] = 0.0;
|
||||
out_5279329364050306118[66] = 0.0;
|
||||
out_5279329364050306118[67] = 0.0;
|
||||
out_5279329364050306118[68] = 0.0;
|
||||
out_5279329364050306118[69] = 0.0;
|
||||
out_5279329364050306118[70] = 1.0;
|
||||
out_5279329364050306118[71] = 0.0;
|
||||
out_5279329364050306118[72] = 0.0;
|
||||
out_5279329364050306118[73] = 0.0;
|
||||
out_5279329364050306118[74] = 0.0;
|
||||
out_5279329364050306118[75] = 0.0;
|
||||
out_5279329364050306118[76] = 0.0;
|
||||
out_5279329364050306118[77] = 0.0;
|
||||
out_5279329364050306118[78] = 0.0;
|
||||
out_5279329364050306118[79] = 0.0;
|
||||
out_5279329364050306118[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_2778542718740683729) {
|
||||
out_2778542718740683729[0] = 1.0;
|
||||
out_2778542718740683729[1] = 0.0;
|
||||
out_2778542718740683729[2] = 0.0;
|
||||
out_2778542718740683729[3] = 0.0;
|
||||
out_2778542718740683729[4] = 0.0;
|
||||
out_2778542718740683729[5] = 0.0;
|
||||
out_2778542718740683729[6] = 0.0;
|
||||
out_2778542718740683729[7] = 0.0;
|
||||
out_2778542718740683729[8] = 0.0;
|
||||
out_2778542718740683729[9] = 0.0;
|
||||
out_2778542718740683729[10] = 1.0;
|
||||
out_2778542718740683729[11] = 0.0;
|
||||
out_2778542718740683729[12] = 0.0;
|
||||
out_2778542718740683729[13] = 0.0;
|
||||
out_2778542718740683729[14] = 0.0;
|
||||
out_2778542718740683729[15] = 0.0;
|
||||
out_2778542718740683729[16] = 0.0;
|
||||
out_2778542718740683729[17] = 0.0;
|
||||
out_2778542718740683729[18] = 0.0;
|
||||
out_2778542718740683729[19] = 0.0;
|
||||
out_2778542718740683729[20] = 1.0;
|
||||
out_2778542718740683729[21] = 0.0;
|
||||
out_2778542718740683729[22] = 0.0;
|
||||
out_2778542718740683729[23] = 0.0;
|
||||
out_2778542718740683729[24] = 0.0;
|
||||
out_2778542718740683729[25] = 0.0;
|
||||
out_2778542718740683729[26] = 0.0;
|
||||
out_2778542718740683729[27] = 0.0;
|
||||
out_2778542718740683729[28] = 0.0;
|
||||
out_2778542718740683729[29] = 0.0;
|
||||
out_2778542718740683729[30] = 1.0;
|
||||
out_2778542718740683729[31] = 0.0;
|
||||
out_2778542718740683729[32] = 0.0;
|
||||
out_2778542718740683729[33] = 0.0;
|
||||
out_2778542718740683729[34] = 0.0;
|
||||
out_2778542718740683729[35] = 0.0;
|
||||
out_2778542718740683729[36] = 0.0;
|
||||
out_2778542718740683729[37] = 0.0;
|
||||
out_2778542718740683729[38] = 0.0;
|
||||
out_2778542718740683729[39] = 0.0;
|
||||
out_2778542718740683729[40] = 1.0;
|
||||
out_2778542718740683729[41] = 0.0;
|
||||
out_2778542718740683729[42] = 0.0;
|
||||
out_2778542718740683729[43] = 0.0;
|
||||
out_2778542718740683729[44] = 0.0;
|
||||
out_2778542718740683729[45] = 0.0;
|
||||
out_2778542718740683729[46] = 0.0;
|
||||
out_2778542718740683729[47] = 0.0;
|
||||
out_2778542718740683729[48] = 0.0;
|
||||
out_2778542718740683729[49] = 0.0;
|
||||
out_2778542718740683729[50] = 1.0;
|
||||
out_2778542718740683729[51] = 0.0;
|
||||
out_2778542718740683729[52] = 0.0;
|
||||
out_2778542718740683729[53] = 0.0;
|
||||
out_2778542718740683729[54] = 0.0;
|
||||
out_2778542718740683729[55] = 0.0;
|
||||
out_2778542718740683729[56] = 0.0;
|
||||
out_2778542718740683729[57] = 0.0;
|
||||
out_2778542718740683729[58] = 0.0;
|
||||
out_2778542718740683729[59] = 0.0;
|
||||
out_2778542718740683729[60] = 1.0;
|
||||
out_2778542718740683729[61] = 0.0;
|
||||
out_2778542718740683729[62] = 0.0;
|
||||
out_2778542718740683729[63] = 0.0;
|
||||
out_2778542718740683729[64] = 0.0;
|
||||
out_2778542718740683729[65] = 0.0;
|
||||
out_2778542718740683729[66] = 0.0;
|
||||
out_2778542718740683729[67] = 0.0;
|
||||
out_2778542718740683729[68] = 0.0;
|
||||
out_2778542718740683729[69] = 0.0;
|
||||
out_2778542718740683729[70] = 1.0;
|
||||
out_2778542718740683729[71] = 0.0;
|
||||
out_2778542718740683729[72] = 0.0;
|
||||
out_2778542718740683729[73] = 0.0;
|
||||
out_2778542718740683729[74] = 0.0;
|
||||
out_2778542718740683729[75] = 0.0;
|
||||
out_2778542718740683729[76] = 0.0;
|
||||
out_2778542718740683729[77] = 0.0;
|
||||
out_2778542718740683729[78] = 0.0;
|
||||
out_2778542718740683729[79] = 0.0;
|
||||
out_2778542718740683729[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_9065464123133416094) {
|
||||
out_9065464123133416094[0] = state[0];
|
||||
out_9065464123133416094[1] = state[1];
|
||||
out_9065464123133416094[2] = state[2];
|
||||
out_9065464123133416094[3] = state[3];
|
||||
out_9065464123133416094[4] = state[4];
|
||||
out_9065464123133416094[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_9065464123133416094[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_9065464123133416094[7] = state[7];
|
||||
out_9065464123133416094[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_3017733201534411708) {
|
||||
out_3017733201534411708[0] = state[0];
|
||||
out_3017733201534411708[1] = state[1];
|
||||
out_3017733201534411708[2] = state[2];
|
||||
out_3017733201534411708[3] = state[3];
|
||||
out_3017733201534411708[4] = state[4];
|
||||
out_3017733201534411708[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_3017733201534411708[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_3017733201534411708[7] = state[7];
|
||||
out_3017733201534411708[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_4008021677178468012) {
|
||||
out_4008021677178468012[0] = 1;
|
||||
out_4008021677178468012[1] = 0;
|
||||
out_4008021677178468012[2] = 0;
|
||||
out_4008021677178468012[3] = 0;
|
||||
out_4008021677178468012[4] = 0;
|
||||
out_4008021677178468012[5] = 0;
|
||||
out_4008021677178468012[6] = 0;
|
||||
out_4008021677178468012[7] = 0;
|
||||
out_4008021677178468012[8] = 0;
|
||||
out_4008021677178468012[9] = 0;
|
||||
out_4008021677178468012[10] = 1;
|
||||
out_4008021677178468012[11] = 0;
|
||||
out_4008021677178468012[12] = 0;
|
||||
out_4008021677178468012[13] = 0;
|
||||
out_4008021677178468012[14] = 0;
|
||||
out_4008021677178468012[15] = 0;
|
||||
out_4008021677178468012[16] = 0;
|
||||
out_4008021677178468012[17] = 0;
|
||||
out_4008021677178468012[18] = 0;
|
||||
out_4008021677178468012[19] = 0;
|
||||
out_4008021677178468012[20] = 1;
|
||||
out_4008021677178468012[21] = 0;
|
||||
out_4008021677178468012[22] = 0;
|
||||
out_4008021677178468012[23] = 0;
|
||||
out_4008021677178468012[24] = 0;
|
||||
out_4008021677178468012[25] = 0;
|
||||
out_4008021677178468012[26] = 0;
|
||||
out_4008021677178468012[27] = 0;
|
||||
out_4008021677178468012[28] = 0;
|
||||
out_4008021677178468012[29] = 0;
|
||||
out_4008021677178468012[30] = 1;
|
||||
out_4008021677178468012[31] = 0;
|
||||
out_4008021677178468012[32] = 0;
|
||||
out_4008021677178468012[33] = 0;
|
||||
out_4008021677178468012[34] = 0;
|
||||
out_4008021677178468012[35] = 0;
|
||||
out_4008021677178468012[36] = 0;
|
||||
out_4008021677178468012[37] = 0;
|
||||
out_4008021677178468012[38] = 0;
|
||||
out_4008021677178468012[39] = 0;
|
||||
out_4008021677178468012[40] = 1;
|
||||
out_4008021677178468012[41] = 0;
|
||||
out_4008021677178468012[42] = 0;
|
||||
out_4008021677178468012[43] = 0;
|
||||
out_4008021677178468012[44] = 0;
|
||||
out_4008021677178468012[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_4008021677178468012[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_4008021677178468012[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4008021677178468012[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4008021677178468012[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_4008021677178468012[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_4008021677178468012[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_4008021677178468012[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_4008021677178468012[53] = -9.8100000000000005*dt;
|
||||
out_4008021677178468012[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_4008021677178468012[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_4008021677178468012[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4008021677178468012[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4008021677178468012[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_4008021677178468012[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_4008021677178468012[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_4008021677178468012[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_4008021677178468012[62] = 0;
|
||||
out_4008021677178468012[63] = 0;
|
||||
out_4008021677178468012[64] = 0;
|
||||
out_4008021677178468012[65] = 0;
|
||||
out_4008021677178468012[66] = 0;
|
||||
out_4008021677178468012[67] = 0;
|
||||
out_4008021677178468012[68] = 0;
|
||||
out_4008021677178468012[69] = 0;
|
||||
out_4008021677178468012[70] = 1;
|
||||
out_4008021677178468012[71] = 0;
|
||||
out_4008021677178468012[72] = 0;
|
||||
out_4008021677178468012[73] = 0;
|
||||
out_4008021677178468012[74] = 0;
|
||||
out_4008021677178468012[75] = 0;
|
||||
out_4008021677178468012[76] = 0;
|
||||
out_4008021677178468012[77] = 0;
|
||||
out_4008021677178468012[78] = 0;
|
||||
out_4008021677178468012[79] = 0;
|
||||
out_4008021677178468012[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_9105766054919985506) {
|
||||
out_9105766054919985506[0] = 1;
|
||||
out_9105766054919985506[1] = 0;
|
||||
out_9105766054919985506[2] = 0;
|
||||
out_9105766054919985506[3] = 0;
|
||||
out_9105766054919985506[4] = 0;
|
||||
out_9105766054919985506[5] = 0;
|
||||
out_9105766054919985506[6] = 0;
|
||||
out_9105766054919985506[7] = 0;
|
||||
out_9105766054919985506[8] = 0;
|
||||
out_9105766054919985506[9] = 0;
|
||||
out_9105766054919985506[10] = 1;
|
||||
out_9105766054919985506[11] = 0;
|
||||
out_9105766054919985506[12] = 0;
|
||||
out_9105766054919985506[13] = 0;
|
||||
out_9105766054919985506[14] = 0;
|
||||
out_9105766054919985506[15] = 0;
|
||||
out_9105766054919985506[16] = 0;
|
||||
out_9105766054919985506[17] = 0;
|
||||
out_9105766054919985506[18] = 0;
|
||||
out_9105766054919985506[19] = 0;
|
||||
out_9105766054919985506[20] = 1;
|
||||
out_9105766054919985506[21] = 0;
|
||||
out_9105766054919985506[22] = 0;
|
||||
out_9105766054919985506[23] = 0;
|
||||
out_9105766054919985506[24] = 0;
|
||||
out_9105766054919985506[25] = 0;
|
||||
out_9105766054919985506[26] = 0;
|
||||
out_9105766054919985506[27] = 0;
|
||||
out_9105766054919985506[28] = 0;
|
||||
out_9105766054919985506[29] = 0;
|
||||
out_9105766054919985506[30] = 1;
|
||||
out_9105766054919985506[31] = 0;
|
||||
out_9105766054919985506[32] = 0;
|
||||
out_9105766054919985506[33] = 0;
|
||||
out_9105766054919985506[34] = 0;
|
||||
out_9105766054919985506[35] = 0;
|
||||
out_9105766054919985506[36] = 0;
|
||||
out_9105766054919985506[37] = 0;
|
||||
out_9105766054919985506[38] = 0;
|
||||
out_9105766054919985506[39] = 0;
|
||||
out_9105766054919985506[40] = 1;
|
||||
out_9105766054919985506[41] = 0;
|
||||
out_9105766054919985506[42] = 0;
|
||||
out_9105766054919985506[43] = 0;
|
||||
out_9105766054919985506[44] = 0;
|
||||
out_9105766054919985506[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_9105766054919985506[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_9105766054919985506[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9105766054919985506[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9105766054919985506[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_9105766054919985506[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_9105766054919985506[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_9105766054919985506[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_9105766054919985506[53] = -9.8100000000000005*dt;
|
||||
out_9105766054919985506[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_9105766054919985506[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_9105766054919985506[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9105766054919985506[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9105766054919985506[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_9105766054919985506[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_9105766054919985506[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_9105766054919985506[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_9105766054919985506[62] = 0;
|
||||
out_9105766054919985506[63] = 0;
|
||||
out_9105766054919985506[64] = 0;
|
||||
out_9105766054919985506[65] = 0;
|
||||
out_9105766054919985506[66] = 0;
|
||||
out_9105766054919985506[67] = 0;
|
||||
out_9105766054919985506[68] = 0;
|
||||
out_9105766054919985506[69] = 0;
|
||||
out_9105766054919985506[70] = 1;
|
||||
out_9105766054919985506[71] = 0;
|
||||
out_9105766054919985506[72] = 0;
|
||||
out_9105766054919985506[73] = 0;
|
||||
out_9105766054919985506[74] = 0;
|
||||
out_9105766054919985506[75] = 0;
|
||||
out_9105766054919985506[76] = 0;
|
||||
out_9105766054919985506[77] = 0;
|
||||
out_9105766054919985506[78] = 0;
|
||||
out_9105766054919985506[79] = 0;
|
||||
out_9105766054919985506[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_5404965656887369328) {
|
||||
out_5404965656887369328[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_614805040009735499) {
|
||||
out_614805040009735499[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_5246076676685385693) {
|
||||
out_5246076676685385693[0] = 0;
|
||||
out_5246076676685385693[1] = 0;
|
||||
out_5246076676685385693[2] = 0;
|
||||
out_5246076676685385693[3] = 0;
|
||||
out_5246076676685385693[4] = 0;
|
||||
out_5246076676685385693[5] = 0;
|
||||
out_5246076676685385693[6] = 1;
|
||||
out_5246076676685385693[7] = 0;
|
||||
out_5246076676685385693[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_2273208276540387017) {
|
||||
out_2273208276540387017[0] = 0;
|
||||
out_2273208276540387017[1] = 0;
|
||||
out_2273208276540387017[2] = 0;
|
||||
out_2273208276540387017[3] = 0;
|
||||
out_2273208276540387017[4] = 0;
|
||||
out_2273208276540387017[5] = 0;
|
||||
out_2273208276540387017[6] = 1;
|
||||
out_2273208276540387017[7] = 0;
|
||||
out_2273208276540387017[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_1243581068366124229) {
|
||||
out_1243581068366124229[0] = state[4];
|
||||
out_1243581068366124229[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_217533166938587881) {
|
||||
out_217533166938587881[0] = state[4];
|
||||
out_217533166938587881[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_1688151077883062557) {
|
||||
out_1688151077883062557[0] = 0;
|
||||
out_1688151077883062557[1] = 0;
|
||||
out_1688151077883062557[2] = 0;
|
||||
out_1688151077883062557[3] = 0;
|
||||
out_1688151077883062557[4] = 1;
|
||||
out_1688151077883062557[5] = 0;
|
||||
out_1688151077883062557[6] = 0;
|
||||
out_1688151077883062557[7] = 0;
|
||||
out_1688151077883062557[8] = 0;
|
||||
out_1688151077883062557[9] = 0;
|
||||
out_1688151077883062557[10] = 0;
|
||||
out_1688151077883062557[11] = 0;
|
||||
out_1688151077883062557[12] = 0;
|
||||
out_1688151077883062557[13] = 0;
|
||||
out_1688151077883062557[14] = 1;
|
||||
out_1688151077883062557[15] = 0;
|
||||
out_1688151077883062557[16] = 0;
|
||||
out_1688151077883062557[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_1728503802090121616) {
|
||||
out_1728503802090121616[0] = 0;
|
||||
out_1728503802090121616[1] = 0;
|
||||
out_1728503802090121616[2] = 0;
|
||||
out_1728503802090121616[3] = 0;
|
||||
out_1728503802090121616[4] = 1;
|
||||
out_1728503802090121616[5] = 0;
|
||||
out_1728503802090121616[6] = 0;
|
||||
out_1728503802090121616[7] = 0;
|
||||
out_1728503802090121616[8] = 0;
|
||||
out_1728503802090121616[9] = 0;
|
||||
out_1728503802090121616[10] = 0;
|
||||
out_1728503802090121616[11] = 0;
|
||||
out_1728503802090121616[12] = 0;
|
||||
out_1728503802090121616[13] = 0;
|
||||
out_1728503802090121616[14] = 1;
|
||||
out_1728503802090121616[15] = 0;
|
||||
out_1728503802090121616[16] = 0;
|
||||
out_1728503802090121616[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_4545117735558442296) {
|
||||
out_4545117735558442296[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_13386882396371945) {
|
||||
out_13386882396371945[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_8672971066896557725) {
|
||||
out_8672971066896557725[0] = 0;
|
||||
out_8672971066896557725[1] = 0;
|
||||
out_8672971066896557725[2] = 0;
|
||||
out_8672971066896557725[3] = 0;
|
||||
out_8672971066896557725[4] = 1;
|
||||
out_8672971066896557725[5] = 0;
|
||||
out_8672971066896557725[6] = 0;
|
||||
out_8672971066896557725[7] = 0;
|
||||
out_8672971066896557725[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_4643482064951229738) {
|
||||
out_4643482064951229738[0] = 0;
|
||||
out_4643482064951229738[1] = 0;
|
||||
out_4643482064951229738[2] = 0;
|
||||
out_4643482064951229738[3] = 0;
|
||||
out_4643482064951229738[4] = 1;
|
||||
out_4643482064951229738[5] = 0;
|
||||
out_4643482064951229738[6] = 0;
|
||||
out_4643482064951229738[7] = 0;
|
||||
out_4643482064951229738[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_456555532102403349) {
|
||||
out_456555532102403349[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_4988286385264473700) {
|
||||
out_4988286385264473700[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_8987579995559441917) {
|
||||
out_8987579995559441917[0] = 0;
|
||||
out_8987579995559441917[1] = 0;
|
||||
out_8987579995559441917[2] = 0;
|
||||
out_8987579995559441917[3] = 0;
|
||||
out_8987579995559441917[4] = 0;
|
||||
out_8987579995559441917[5] = 0;
|
||||
out_8987579995559441917[6] = 0;
|
||||
out_8987579995559441917[7] = 1;
|
||||
out_8987579995559441917[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_6014711595414443241) {
|
||||
out_6014711595414443241[0] = 0;
|
||||
out_6014711595414443241[1] = 0;
|
||||
out_6014711595414443241[2] = 0;
|
||||
out_6014711595414443241[3] = 0;
|
||||
out_6014711595414443241[4] = 0;
|
||||
out_6014711595414443241[5] = 0;
|
||||
out_6014711595414443241[6] = 0;
|
||||
out_6014711595414443241[7] = 1;
|
||||
out_6014711595414443241[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_338548138169026605) {
|
||||
out_338548138169026605[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_5616478307670581144) {
|
||||
out_5616478307670581144[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_6498207755096132814) {
|
||||
out_6498207755096132814[0] = 0;
|
||||
out_6498207755096132814[1] = 0;
|
||||
out_6498207755096132814[2] = 0;
|
||||
out_6498207755096132814[3] = 1;
|
||||
out_6498207755096132814[4] = 0;
|
||||
out_6498207755096132814[5] = 0;
|
||||
out_6498207755096132814[6] = 0;
|
||||
out_6498207755096132814[7] = 0;
|
||||
out_6498207755096132814[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_2468718753150804827) {
|
||||
out_2468718753150804827[0] = 0;
|
||||
out_2468718753150804827[1] = 0;
|
||||
out_2468718753150804827[2] = 0;
|
||||
out_2468718753150804827[3] = 1;
|
||||
out_2468718753150804827[4] = 0;
|
||||
out_2468718753150804827[5] = 0;
|
||||
out_2468718753150804827[6] = 0;
|
||||
out_2468718753150804827[7] = 0;
|
||||
out_2468718753150804827[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_3601607882656779861) {
|
||||
out_3601607882656779861[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_8968582289610468820) {
|
||||
out_8968582289610468820[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_9183202411210949909) {
|
||||
out_9183202411210949909[0] = 0;
|
||||
out_9183202411210949909[1] = 1;
|
||||
out_9183202411210949909[2] = 0;
|
||||
out_9183202411210949909[3] = 0;
|
||||
out_9183202411210949909[4] = 0;
|
||||
out_9183202411210949909[5] = 0;
|
||||
out_9183202411210949909[6] = 0;
|
||||
out_9183202411210949909[7] = 0;
|
||||
out_9183202411210949909[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_5153713409265621922) {
|
||||
out_5153713409265621922[0] = 0;
|
||||
out_5153713409265621922[1] = 1;
|
||||
out_5153713409265621922[2] = 0;
|
||||
out_5153713409265621922[3] = 0;
|
||||
out_5153713409265621922[4] = 0;
|
||||
out_5153713409265621922[5] = 0;
|
||||
out_5153713409265621922[6] = 0;
|
||||
out_5153713409265621922[7] = 0;
|
||||
out_5153713409265621922[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_8457761326096970535) {
|
||||
out_8457761326096970535[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_7604358550203645525) {
|
||||
out_7604358550203645525[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_4100803394141419335) {
|
||||
out_4100803394141419335[0] = 1;
|
||||
out_4100803394141419335[1] = 0;
|
||||
out_4100803394141419335[2] = 0;
|
||||
out_4100803394141419335[3] = 0;
|
||||
out_4100803394141419335[4] = 0;
|
||||
out_4100803394141419335[5] = 0;
|
||||
out_4100803394141419335[6] = 0;
|
||||
out_4100803394141419335[7] = 0;
|
||||
out_4100803394141419335[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_71314392196091348) {
|
||||
out_71314392196091348[0] = 1;
|
||||
out_71314392196091348[1] = 0;
|
||||
out_71314392196091348[2] = 0;
|
||||
out_71314392196091348[3] = 0;
|
||||
out_71314392196091348[4] = 0;
|
||||
out_71314392196091348[5] = 0;
|
||||
out_71314392196091348[6] = 0;
|
||||
out_71314392196091348[7] = 0;
|
||||
out_71314392196091348[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_1509086364917783500) {
|
||||
out_1509086364917783500[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_3022644488244286851) {
|
||||
out_3022644488244286851[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_8832955975916758223) {
|
||||
out_8832955975916758223[0] = 0;
|
||||
out_8832955975916758223[1] = 0;
|
||||
out_8832955975916758223[2] = 0;
|
||||
out_8832955975916758223[3] = 0;
|
||||
out_8832955975916758223[4] = 0;
|
||||
out_8832955975916758223[5] = 0;
|
||||
out_8832955975916758223[6] = 0;
|
||||
out_8832955975916758223[7] = 0;
|
||||
out_8832955975916758223[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_2242562314663426589) {
|
||||
out_2242562314663426589[0] = 0;
|
||||
out_2242562314663426589[1] = 0;
|
||||
out_2242562314663426589[2] = 0;
|
||||
out_2242562314663426589[3] = 0;
|
||||
out_2242562314663426589[4] = 0;
|
||||
out_2242562314663426589[5] = 0;
|
||||
out_2242562314663426589[6] = 0;
|
||||
out_2242562314663426589[7] = 0;
|
||||
out_2242562314663426589[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8771745385019272911) {
|
||||
err_fun(nom_x, delta_x, out_8771745385019272911);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7126582961958922662) {
|
||||
err_fun(nom_x, delta_x, out_7126582961958922662);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3231363094273967832) {
|
||||
inv_err_fun(nom_x, true_x, out_3231363094273967832);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_945533236592444203) {
|
||||
inv_err_fun(nom_x, true_x, out_945533236592444203);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_5279329364050306118) {
|
||||
H_mod_fun(state, out_5279329364050306118);
|
||||
void car_H_mod_fun(double *state, double *out_2778542718740683729) {
|
||||
H_mod_fun(state, out_2778542718740683729);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_9065464123133416094) {
|
||||
f_fun(state, dt, out_9065464123133416094);
|
||||
void car_f_fun(double *state, double dt, double *out_3017733201534411708) {
|
||||
f_fun(state, dt, out_3017733201534411708);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_4008021677178468012) {
|
||||
F_fun(state, dt, out_4008021677178468012);
|
||||
void car_F_fun(double *state, double dt, double *out_9105766054919985506) {
|
||||
F_fun(state, dt, out_9105766054919985506);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_5404965656887369328) {
|
||||
h_25(state, unused, out_5404965656887369328);
|
||||
void car_h_25(double *state, double *unused, double *out_614805040009735499) {
|
||||
h_25(state, unused, out_614805040009735499);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_5246076676685385693) {
|
||||
H_25(state, unused, out_5246076676685385693);
|
||||
void car_H_25(double *state, double *unused, double *out_2273208276540387017) {
|
||||
H_25(state, unused, out_2273208276540387017);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_1243581068366124229) {
|
||||
h_24(state, unused, out_1243581068366124229);
|
||||
void car_h_24(double *state, double *unused, double *out_217533166938587881) {
|
||||
h_24(state, unused, out_217533166938587881);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_1688151077883062557) {
|
||||
H_24(state, unused, out_1688151077883062557);
|
||||
void car_H_24(double *state, double *unused, double *out_1728503802090121616) {
|
||||
H_24(state, unused, out_1728503802090121616);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_4545117735558442296) {
|
||||
h_30(state, unused, out_4545117735558442296);
|
||||
void car_h_30(double *state, double *unused, double *out_13386882396371945) {
|
||||
h_30(state, unused, out_13386882396371945);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_8672971066896557725) {
|
||||
H_30(state, unused, out_8672971066896557725);
|
||||
void car_H_30(double *state, double *unused, double *out_4643482064951229738) {
|
||||
H_30(state, unused, out_4643482064951229738);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_456555532102403349) {
|
||||
h_26(state, unused, out_456555532102403349);
|
||||
void car_h_26(double *state, double *unused, double *out_4988286385264473700) {
|
||||
h_26(state, unused, out_4988286385264473700);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_8987579995559441917) {
|
||||
H_26(state, unused, out_8987579995559441917);
|
||||
void car_H_26(double *state, double *unused, double *out_6014711595414443241) {
|
||||
H_26(state, unused, out_6014711595414443241);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_338548138169026605) {
|
||||
h_27(state, unused, out_338548138169026605);
|
||||
void car_h_27(double *state, double *unused, double *out_5616478307670581144) {
|
||||
h_27(state, unused, out_5616478307670581144);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_6498207755096132814) {
|
||||
H_27(state, unused, out_6498207755096132814);
|
||||
void car_H_27(double *state, double *unused, double *out_2468718753150804827) {
|
||||
H_27(state, unused, out_2468718753150804827);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_3601607882656779861) {
|
||||
h_29(state, unused, out_3601607882656779861);
|
||||
void car_h_29(double *state, double *unused, double *out_8968582289610468820) {
|
||||
h_29(state, unused, out_8968582289610468820);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_9183202411210949909) {
|
||||
H_29(state, unused, out_9183202411210949909);
|
||||
void car_H_29(double *state, double *unused, double *out_5153713409265621922) {
|
||||
H_29(state, unused, out_5153713409265621922);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_8457761326096970535) {
|
||||
h_28(state, unused, out_8457761326096970535);
|
||||
void car_h_28(double *state, double *unused, double *out_7604358550203645525) {
|
||||
h_28(state, unused, out_7604358550203645525);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_4100803394141419335) {
|
||||
H_28(state, unused, out_4100803394141419335);
|
||||
void car_H_28(double *state, double *unused, double *out_71314392196091348) {
|
||||
H_28(state, unused, out_71314392196091348);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_1509086364917783500) {
|
||||
h_31(state, unused, out_1509086364917783500);
|
||||
void car_h_31(double *state, double *unused, double *out_3022644488244286851) {
|
||||
h_31(state, unused, out_3022644488244286851);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_8832955975916758223) {
|
||||
H_31(state, unused, out_8832955975916758223);
|
||||
void car_H_31(double *state, double *unused, double *out_2242562314663426589) {
|
||||
H_31(state, unused, out_2242562314663426589);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_8771745385019272911);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3231363094273967832);
|
||||
void car_H_mod_fun(double *state, double *out_5279329364050306118);
|
||||
void car_f_fun(double *state, double dt, double *out_9065464123133416094);
|
||||
void car_F_fun(double *state, double dt, double *out_4008021677178468012);
|
||||
void car_h_25(double *state, double *unused, double *out_5404965656887369328);
|
||||
void car_H_25(double *state, double *unused, double *out_5246076676685385693);
|
||||
void car_h_24(double *state, double *unused, double *out_1243581068366124229);
|
||||
void car_H_24(double *state, double *unused, double *out_1688151077883062557);
|
||||
void car_h_30(double *state, double *unused, double *out_4545117735558442296);
|
||||
void car_H_30(double *state, double *unused, double *out_8672971066896557725);
|
||||
void car_h_26(double *state, double *unused, double *out_456555532102403349);
|
||||
void car_H_26(double *state, double *unused, double *out_8987579995559441917);
|
||||
void car_h_27(double *state, double *unused, double *out_338548138169026605);
|
||||
void car_H_27(double *state, double *unused, double *out_6498207755096132814);
|
||||
void car_h_29(double *state, double *unused, double *out_3601607882656779861);
|
||||
void car_H_29(double *state, double *unused, double *out_9183202411210949909);
|
||||
void car_h_28(double *state, double *unused, double *out_8457761326096970535);
|
||||
void car_H_28(double *state, double *unused, double *out_4100803394141419335);
|
||||
void car_h_31(double *state, double *unused, double *out_1509086364917783500);
|
||||
void car_H_31(double *state, double *unused, double *out_8832955975916758223);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7126582961958922662);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_945533236592444203);
|
||||
void car_H_mod_fun(double *state, double *out_2778542718740683729);
|
||||
void car_f_fun(double *state, double dt, double *out_3017733201534411708);
|
||||
void car_F_fun(double *state, double dt, double *out_9105766054919985506);
|
||||
void car_h_25(double *state, double *unused, double *out_614805040009735499);
|
||||
void car_H_25(double *state, double *unused, double *out_2273208276540387017);
|
||||
void car_h_24(double *state, double *unused, double *out_217533166938587881);
|
||||
void car_H_24(double *state, double *unused, double *out_1728503802090121616);
|
||||
void car_h_30(double *state, double *unused, double *out_13386882396371945);
|
||||
void car_H_30(double *state, double *unused, double *out_4643482064951229738);
|
||||
void car_h_26(double *state, double *unused, double *out_4988286385264473700);
|
||||
void car_H_26(double *state, double *unused, double *out_6014711595414443241);
|
||||
void car_h_27(double *state, double *unused, double *out_5616478307670581144);
|
||||
void car_H_27(double *state, double *unused, double *out_2468718753150804827);
|
||||
void car_h_29(double *state, double *unused, double *out_8968582289610468820);
|
||||
void car_H_29(double *state, double *unused, double *out_5153713409265621922);
|
||||
void car_h_28(double *state, double *unused, double *out_7604358550203645525);
|
||||
void car_H_28(double *state, double *unused, double *out_71314392196091348);
|
||||
void car_h_31(double *state, double *unused, double *out_3022644488244286851);
|
||||
void car_H_31(double *state, double *unused, double *out_2242562314663426589);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_4948547112888214127);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1637666684293454320);
|
||||
void pose_H_mod_fun(double *state, double *out_2192054780204158214);
|
||||
void pose_f_fun(double *state, double dt, double *out_8699512147542263638);
|
||||
void pose_F_fun(double *state, double dt, double *out_5585604130229028810);
|
||||
void pose_h_4(double *state, double *unused, double *out_7040990451462861704);
|
||||
void pose_H_4(double *state, double *unused, double *out_8084524700677117702);
|
||||
void pose_h_10(double *state, double *unused, double *out_5385759793768973690);
|
||||
void pose_H_10(double *state, double *unused, double *out_522586112484741399);
|
||||
void pose_h_13(double *state, double *unused, double *out_1659773189667427592);
|
||||
void pose_H_13(double *state, double *unused, double *out_4872250875344784901);
|
||||
void pose_h_14(double *state, double *unused, double *out_6941415956428537191);
|
||||
void pose_H_14(double *state, double *unused, double *out_8519641227322001301);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_8909220829729456592);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8068156223905085053);
|
||||
void pose_H_mod_fun(double *state, double *out_3205830260465553476);
|
||||
void pose_f_fun(double *state, double dt, double *out_4238396293587501899);
|
||||
void pose_F_fun(double *state, double dt, double *out_6003141237729326165);
|
||||
void pose_h_4(double *state, double *unused, double *out_6516003908446642780);
|
||||
void pose_H_4(double *state, double *unused, double *out_8358348645504634211);
|
||||
void pose_h_10(double *state, double *unused, double *out_5069419514865957544);
|
||||
void pose_H_10(double *state, double *unused, double *out_2285557584559061390);
|
||||
void pose_h_13(double *state, double *unused, double *out_3114619426317878324);
|
||||
void pose_H_13(double *state, double *unused, double *out_6876121602872584604);
|
||||
void pose_h_14(double *state, double *unused, double *out_5969212145833405866);
|
||||
void pose_H_14(double *state, double *unused, double *out_6125154571865432876);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -1341,7 +1341,8 @@ class SelfdriveD:
|
||||
self.conditional_status = result.status
|
||||
self.conditional_result = result
|
||||
|
||||
def _decide_aol_axes(self, CS, intent, native):
|
||||
def _decide_aol_axes(self, CS, intent, native, *, now_ns=0):
|
||||
from openpilot.starpilot.car.gm.aol import physical_cancel_lateral_allowed
|
||||
return decide_axes(
|
||||
standard_lateral=self.active, standard_longitudinal=bool(self.enabled and self.CP.openpilotLongitudinalControl and
|
||||
not any(sample.seatbeltUnlatched for sample, _ in getattr(self, 'car_state_samples', ()))),
|
||||
@@ -1350,7 +1351,9 @@ class SelfdriveD:
|
||||
self.sm['extrinsicsCalibration'].calStatus == log.ExtrinsicsCalibration.Status.calibrated),
|
||||
no_entry=aol_no_entry(self.events.names, CS, paddle_only_cancel=self.nostalgia_paddle_cancel,
|
||||
allow_lateral_cancel=bool(intent is not None and intent.allowedLatch and intent.optionalSetRelease
|
||||
and ev6_aol_qualified(self.CP, marked_only=True)),
|
||||
and ev6_aol_qualified(self.CP, marked_only=True)) or
|
||||
physical_cancel_lateral_allowed(self.CP, CS, intent, native,
|
||||
source_ns=getattr(self, 'aol_car_state_log_ns', 0), now_ns=now_ns),
|
||||
cruise_main_required=getattr(self, 'aol_cruise_main_required', True),
|
||||
allow_below_engage_speed=bool(is_volt_ascm_longitudinal(self.CP) and
|
||||
not self.CP.flags & GMFlags.PEDAL_LONG)),
|
||||
@@ -1475,7 +1478,7 @@ class SelfdriveD:
|
||||
elif intent is not None and not intent.allowedLatch:
|
||||
self.aol_dm_lateral_inhibit = False
|
||||
previous = self.aol_axis_decision
|
||||
requested = self._decide_aol_axes(CS, intent, native)
|
||||
requested = self._decide_aol_axes(CS, intent, native, now_ns=now_ns)
|
||||
source_current = bool(intent is not None and getattr(self, 'conditional_car_state_valid', False) and
|
||||
CS.canValid and not CS.canTimeout)
|
||||
driver_override = bool(CS.brakePressed or CS.regenBraking or CS.gasPressed or
|
||||
@@ -1586,7 +1589,7 @@ class SelfdriveD:
|
||||
self.state_machine.soft_disable_timer > 0 and
|
||||
ET.SOFT_DISABLE not in self.state_machine.current_alert_types):
|
||||
self.state_machine.current_alert_types.append(ET.SOFT_DISABLE)
|
||||
self.aol_axis_decision = self._decide_aol_axes(CS, intent, native)
|
||||
self.aol_axis_decision = self._decide_aol_axes(CS, intent, native, now_ns=now_ns)
|
||||
if availability:
|
||||
self.aol_axis_decision = AxisDecision(desired_lateral=True, native_acknowledged=True)
|
||||
elif getattr(self, 'aol_availability_long_inhibit', False):
|
||||
|
||||
@@ -0,0 +1,427 @@
|
||||
"""Real GM parser/Card sources with controlled native peers; no transport claim."""
|
||||
from dataclasses import replace
|
||||
from types import SimpleNamespace
|
||||
from unittest.mock import patch
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus
|
||||
from opendbc.car.gm.bolt_cc import button_bytes
|
||||
from opendbc.car.gm.gmcan import pedal_crc
|
||||
from opendbc.car.gm.tests.test_bolt_cc import feed
|
||||
from opendbc.car.gm.tests.test_bolt_pedal import params
|
||||
from opendbc.car.gm.values import CAR, DBC, CruiseButtons
|
||||
from openpilot.cereal import messaging
|
||||
from openpilot.common.params import Params
|
||||
from openpilot.common.prefix import OpenpilotPrefix
|
||||
from openpilot.starpilot.aol.runtime import decide_axes
|
||||
from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback
|
||||
from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety
|
||||
|
||||
NOW = 10_000_000_000
|
||||
|
||||
|
||||
class Peers:
|
||||
def __init__(self, messages):
|
||||
self.messages = {m.which(): m for m in messages}
|
||||
self.logMonoTime = {n: int(m.logMonoTime) for n, m in self.messages.items()}
|
||||
self.seen = dict.fromkeys(self.messages, True)
|
||||
self.valid = dict(self.seen)
|
||||
self.alive = dict(self.seen)
|
||||
|
||||
def __getitem__(self, name):
|
||||
return getattr(self.messages[name], name)
|
||||
|
||||
|
||||
def parse(card, now, gesture=None, counter=0):
|
||||
packer = CANPacker(DBC[card.CP.carFingerprint][Bus.pt])
|
||||
_, packets = feed(SimpleNamespace(update=lambda _: None), packer, now, counter=counter,
|
||||
active=False, speed=20., brake=gesture == 'brake', regen=gesture == 'paddle')
|
||||
packets = [p for p in packets if p[0] not in (0x1E1, 0x1F5)]
|
||||
packets += [packer.make_can_msg('BCMBlindSpotMonitor', 0, {}),
|
||||
packer.make_can_msg('ECMPRDNL2', 0, {'PRNDL2': 6}),
|
||||
(0x1E1, button_bytes(CruiseButtons.CANCEL if gesture == 'cancel' else CruiseButtons.UNPRESS, counter), 0)]
|
||||
sensor = bytearray(packer.make_can_msg('GAS_SENSOR', 0,
|
||||
{'INTERCEPTOR_GAS': 0., 'INTERCEPTOR_GAS2': 0., 'STATE': 0, 'COUNTER_PEDAL': counter})[1])
|
||||
sensor[5] = pedal_crc(sensor)
|
||||
packets.append((0x201, bytes(sensor), 0))
|
||||
if now == NOW:
|
||||
card.CI.update([(now - 1_000_000, packets)])
|
||||
cs = card.CI.update([(now, packets)])
|
||||
assert cs.canValid and not cs.canTimeout
|
||||
assert cs.brakePressed == (gesture == 'brake')
|
||||
assert cs.regenBraking == (gesture == 'paddle')
|
||||
card._aol_can_observation = (cs, now, now, now)
|
||||
return cs
|
||||
|
||||
|
||||
def peers(cp, producer, now, *, controls=True, native=None, sequence=1, long_active=True):
|
||||
config = cp.safetyConfigs[0]
|
||||
if native is None:
|
||||
slot = PandaSlot(0, 'gm-unit', config.safetyModel.raw, config.safetyParam, 32, True)
|
||||
native = SafetyState(1, True, now, now + 200_000_000, config.safetyModel.raw, config.safetyParam,
|
||||
True, True, True, True, 'gm-unit', 'gm-session',
|
||||
sourcePandaStatesMonoTime=now, pandaInventory=(slot,))
|
||||
wire = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime)
|
||||
wire.aolSafetyWire = encode_safety(native)
|
||||
pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now)
|
||||
p = pandas.pandaStates[0]
|
||||
p.safetyModel, p.safetyParam, p.alternativeExperience = config.safetyModel, config.safetyParam, 32
|
||||
p.controlsAllowed = controls
|
||||
axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=now)
|
||||
a = axis.aolAxisState
|
||||
a.sessionId = a.faultSessionId = 'gm-session'
|
||||
a.sequence, a.qualified, a.faultReason = sequence, True, 'none'
|
||||
a.observedMonoTime, a.validUntilMonoTime, a.sourceCarStateMonoTime = now, now + 30_000_000, now
|
||||
a.desiredLateral = a.lateralActive = a.nativeAcknowledged = True
|
||||
a.desiredLongitudinal = a.longitudinalActive = long_active
|
||||
a.intentProducerSessionId, a.intentIntegrityEpoch = producer, 1
|
||||
events = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now)
|
||||
return Peers([wire.as_reader(), pandas.as_reader(), axis.as_reader(), events.as_reader()]), native
|
||||
|
||||
|
||||
def baseline():
|
||||
from openpilot.starpilot.aol.tests.test_gm import TestGmAol
|
||||
settings = Params()
|
||||
settings.put_bool('AlwaysOnLateral', True, block=True)
|
||||
settings.put_bool('AlphaLongitudinalEnabled', True, block=True)
|
||||
cp = params(CAR.CHEVROLET_BOLT_CC_2018_2021, pedal=True, camera=True)
|
||||
card = TestGmAol.card(cp, settings)
|
||||
cs = parse(card, NOW)
|
||||
card.aol_card_intent.update(cs, fault_active=False, now_ns=NOW)
|
||||
assert card.aol_card_intent.allowed_latch
|
||||
sm, native = peers(card.CP, card.slc_producer_session, NOW)
|
||||
feedback = TransportPauseFeedback(card.CP)
|
||||
assert not feedback.observe(sm, card.CP, cs, NOW, latched=True,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
assert feedback.session == 'gm-session' and feedback.healthy_inventory
|
||||
return card, feedback, native
|
||||
|
||||
|
||||
@pytest.mark.parametrize('gesture', ['brake', 'paddle', 'cancel'])
|
||||
def test_real_physical_withdrawal_retains_latch_with_held_long_wire(gesture):
|
||||
with OpenpilotPrefix():
|
||||
card, feedback, native = baseline()
|
||||
now = NOW + 10_000_000
|
||||
cs = parse(card, now, gesture, counter=1)
|
||||
source = card.aol_gm_withdrawal_source(cs, now)
|
||||
assert source == now
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, now, controls=False, native=native, sequence=2)
|
||||
reset = feedback.observe(sm, card.CP, cs, now, latched=True, gm_withdrawal_source_ns=source,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
card.aol_card_intent.update(cs, fault_active=reset, now_ns=now)
|
||||
assert not reset and not feedback.failed and card.aol_card_intent.allowed_latch
|
||||
assert feedback.gm_withdrawal_ns == source
|
||||
# The prior LONG wire and latest raw withdrawal are untouched. Ordinary
|
||||
# runtime keeps LONG off even though accepted driver LAT intent survives.
|
||||
intent = SimpleNamespace(allowedLatch=True, pauseLateral=False, pauseLongitudinal=False)
|
||||
decision = decide_axes(standard_lateral=False, standard_longitudinal=False, intent=intent,
|
||||
native=native, car_state=cs, initialized=True, model_ready=True, no_entry=False,
|
||||
immediate_disable=False, dm_lockout=False, pause_brake_mps=5.)
|
||||
assert decision.lateral_active and not decision.longitudinal_active
|
||||
assert native.requestedLongitudinal and native.longitudinalAllowed
|
||||
assert native.validUntilMonoTime == NOW + 200_000_000
|
||||
# Held and release samples retain one original occurrence; the actual
|
||||
# native LAT-only acknowledgement ends the exception on the normal path.
|
||||
for offset, physical in ((20_000_000, gesture), (30_000_000, None)):
|
||||
later = NOW + offset
|
||||
cs = parse(card, later, physical, counter=2)
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, later, controls=False, native=native,
|
||||
sequence=offset // 10_000_000 + 1, long_active=False)
|
||||
assert not feedback.observe(sm, card.CP, cs, later, latched=True,
|
||||
gm_withdrawal_source_ns=card.aol_gm_withdrawal_source(cs, later), intent_producer_session=card.slc_producer_session)
|
||||
assert feedback.gm_withdrawal_ns == source
|
||||
lat_only = replace(native, observedMonoTime=NOW + 40_000_000, validUntilMonoTime=NOW + 240_000_000,
|
||||
requestedLongitudinal=False, longitudinalAllowed=False,
|
||||
pandaInventory=(replace(native.pandaInventory[0], controlsAllowed=False),))
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, NOW + 40_000_000, controls=False,
|
||||
native=lat_only, sequence=5, long_active=False)
|
||||
assert not feedback.observe(sm, card.CP, cs, NOW + 40_000_000, latched=True,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
assert feedback.gm_withdrawal_ns == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('fault', ['no_source', 'stale_source', 'future_source', 'producer', 'epoch',
|
||||
'session', 'serial', 'inventory', 'owner', 'config', 'rx_fault', 'heartbeat', 'lat_denied', 'no_lat',
|
||||
'native_expired', 'can_invalid', 'eps', 'event', 'axis_expired', 'sequence', 'main',
|
||||
'seatbelt', 'seatbelt_event', 'door', 'gear'])
|
||||
def test_physical_withdrawal_keeps_identity_freshness_and_health_vetoes(fault):
|
||||
with OpenpilotPrefix():
|
||||
card, feedback, native = baseline()
|
||||
now = NOW + 10_000_000
|
||||
cs = parse(card, now, 'brake', counter=1)
|
||||
source = card.aol_gm_withdrawal_source(cs, now)
|
||||
if fault == 'no_source':
|
||||
source = 0
|
||||
elif fault == 'stale_source':
|
||||
source = now - 30_000_001
|
||||
elif fault == 'future_source':
|
||||
source = now + 1
|
||||
elif fault == 'session':
|
||||
native = replace(native, axisSessionId='other')
|
||||
elif fault == 'serial':
|
||||
native = replace(native, pandaSerial='other')
|
||||
elif fault == 'inventory':
|
||||
native = replace(native, pandaInventory=(replace(native.pandaInventory[0], hardwareSerial='other'),))
|
||||
elif fault == 'owner':
|
||||
card.CP.alternativeExperience = 0
|
||||
elif fault == 'no_lat':
|
||||
native = replace(native, requestedLateral=False)
|
||||
elif fault == 'native_expired':
|
||||
native = replace(native, validUntilMonoTime=now - 1)
|
||||
elif fault == 'main':
|
||||
cs.cruiseState.available = False
|
||||
elif fault == 'lat_denied':
|
||||
native = replace(native, lateralAllowed=False)
|
||||
elif fault == 'can_invalid':
|
||||
cs.canValid = False
|
||||
elif fault == 'eps':
|
||||
cs.steerFaultPermanent = True
|
||||
elif fault == 'seatbelt':
|
||||
cs.seatbeltUnlatched = True
|
||||
elif fault == 'door':
|
||||
cs.doorOpen = True
|
||||
elif fault == 'gear':
|
||||
from opendbc.car.structs import car
|
||||
cs.gearShifter = car.CarState.GearShifter.park
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, now, controls=False, native=native, sequence=2)
|
||||
axis = sm.messages['aolAxisState'].as_builder()
|
||||
pandas = sm.messages['pandaStates'].as_builder()
|
||||
events = sm.messages['onroadEvents'].as_builder()
|
||||
if fault == 'producer':
|
||||
axis.aolAxisState.intentProducerSessionId = 'other'
|
||||
elif fault == 'epoch':
|
||||
axis.aolAxisState.intentIntegrityEpoch = 2
|
||||
elif fault == 'sequence':
|
||||
axis.aolAxisState.sequence = 1
|
||||
elif fault == 'axis_expired':
|
||||
axis.aolAxisState.validUntilMonoTime = now - 1
|
||||
elif fault == 'config':
|
||||
pandas.pandaStates[0].safetyParam ^= 1
|
||||
elif fault == 'rx_fault':
|
||||
pandas.pandaStates[0].safetyRxChecksInvalid = True
|
||||
elif fault == 'heartbeat':
|
||||
pandas.pandaStates[0].heartbeatLost = True
|
||||
elif fault == 'seatbelt_event':
|
||||
events.init('onroadEvents', 1)
|
||||
events.onroadEvents[0].name, events.onroadEvents[0].softDisable = 'seatbeltNotLatched', True
|
||||
elif fault == 'event':
|
||||
events.init('onroadEvents', 1)
|
||||
events.onroadEvents[0].name, events.onroadEvents[0].immediateDisable = 'controlsMismatch', True
|
||||
sm = Peers([sm.messages['aolSafetyWire'], pandas.as_reader(), axis.as_reader(), events.as_reader()])
|
||||
assert feedback.observe(sm, card.CP, cs, now, latched=True, gm_withdrawal_source_ns=source,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
assert feedback.failed
|
||||
|
||||
|
||||
def test_held_and_duplicate_samples_cannot_extend_withdrawal_deadline():
|
||||
with OpenpilotPrefix():
|
||||
card, feedback, native = baseline()
|
||||
occurrence = NOW + 10_000_000
|
||||
for tick in range(1, 23):
|
||||
now = NOW + tick * 10_000_000
|
||||
cs = parse(card, now, 'brake', counter=tick % 4)
|
||||
if tick > 1:
|
||||
native = replace(native, observedMonoTime=now, validUntilMonoTime=now + 200_000_000)
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, now, controls=False, native=native,
|
||||
sequence=tick + 1, long_active=tick == 1)
|
||||
source = card.aol_gm_withdrawal_source(cs, now)
|
||||
for _ in range(2):
|
||||
failed = feedback.observe(sm, card.CP, cs, now, latched=True, gm_withdrawal_source_ns=source,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
assert failed == (tick == 22)
|
||||
assert feedback.gm_withdrawal_ns == occurrence
|
||||
assert feedback.failed
|
||||
|
||||
|
||||
@pytest.mark.parametrize('fault', ['different_cs', 'old_receipt', 'future_receipt', 'stale_parser', 'future_parser'])
|
||||
def test_physical_receipt_requires_same_parsed_batch_and_separate_clock_freshness(fault):
|
||||
with OpenpilotPrefix():
|
||||
card, _, _ = baseline()
|
||||
now = NOW + 10_000_000
|
||||
cs = parse(card, now, 'brake', counter=1)
|
||||
assert card.aol_gm_withdrawal_source(cs, now) == now
|
||||
if fault == 'different_cs':
|
||||
card._aol_can_observation = (cs.as_reader().as_builder(), now, now, now)
|
||||
elif fault == 'old_receipt':
|
||||
card._aol_can_observation = (cs, now - 30_000_001, now, now)
|
||||
elif fault == 'future_receipt':
|
||||
card._aol_can_observation = (cs, now + 1, now, now)
|
||||
elif fault == 'stale_parser':
|
||||
card._aol_can_observation = (cs, now, now + 30_000_001, now + 30_000_001)
|
||||
elif fault == 'future_parser':
|
||||
card._aol_can_observation = (cs, now, now - 1, now)
|
||||
assert card.aol_gm_withdrawal_source(cs, now) == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('owner', ['qualified', 'unmarked', 'wrong_profile'])
|
||||
def test_actual_card_state_update_captures_boot_clock_only_for_physical_gm_owner(owner):
|
||||
# Stop immediately after the production clock gate. The real joined Card.step
|
||||
# transport cases separately exercise installation/consumption of the receipt.
|
||||
class ClockBoundaryReached(Exception):
|
||||
pass
|
||||
|
||||
def stop_after_clock(*_):
|
||||
raise ClockBoundaryReached
|
||||
|
||||
with OpenpilotPrefix():
|
||||
card, _, _ = baseline()
|
||||
cs = parse(card, NOW + 10_000_000, 'brake', counter=1)
|
||||
if owner == 'unmarked':
|
||||
card.CP.alternativeExperience = 0
|
||||
elif owner == 'wrong_profile':
|
||||
card.CP.flags = 0
|
||||
with patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[]), \
|
||||
patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(NOW + 10_000_000, [])]), \
|
||||
patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=NOW + 10_000_000), \
|
||||
patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=NOW + 10_000_000) as clock, \
|
||||
patch.object(card.CI, 'update', return_value=cs), \
|
||||
patch.object(card, 'observe_ioniq6_long_authority', side_effect=stop_after_clock):
|
||||
with pytest.raises(ClockBoundaryReached):
|
||||
card.state_update()
|
||||
if owner == 'qualified':
|
||||
from openpilot.selfdrive.car.card import time
|
||||
clock.assert_called_once_with(time.CLOCK_BOOTTIME)
|
||||
else:
|
||||
clock.assert_not_called()
|
||||
|
||||
|
||||
@pytest.mark.parametrize('fault', [None, 'unmarked', 'wrong_owner', 'unlatched', 'unarmed', 'source_mismatch',
|
||||
'source_expired', 'future_source', 'no_cancel', 'can_invalid', 'eps', 'no_native', 'lat_denied', 'native_inventory'])
|
||||
def test_gm_cancel_exempts_only_current_armed_lateral_no_entry(fault):
|
||||
from openpilot.starpilot.aol.wire import IntentState
|
||||
from openpilot.starpilot.car.gm.aol import physical_cancel_lateral_allowed
|
||||
from openpilot.selfdrive.selfdrived.events import ET, EVENTS, EventName
|
||||
from openpilot.starpilot.nostalgia import aol_no_entry
|
||||
with OpenpilotPrefix():
|
||||
card, _, native = baseline()
|
||||
now = NOW + 10_000_000
|
||||
cs = parse(card, now, 'cancel', counter=1)
|
||||
intent = IntentState(card.slc_producer_session, 2, now, now, now + 30_000_000, True, False, False, True, True)
|
||||
source = now
|
||||
if fault == 'unmarked':
|
||||
card.CP.alternativeExperience = 0
|
||||
elif fault == 'wrong_owner':
|
||||
card.CP.flags = 0
|
||||
elif fault == 'unlatched':
|
||||
intent = replace(intent, allowedLatch=False)
|
||||
elif fault == 'unarmed':
|
||||
intent = replace(intent, lateralArmed=False)
|
||||
elif fault == 'source_mismatch':
|
||||
source -= 1
|
||||
elif fault == 'source_expired':
|
||||
now += 30_000_001
|
||||
elif fault == 'future_source':
|
||||
source += 1
|
||||
elif fault == 'no_cancel':
|
||||
cs.buttonEvents = []
|
||||
elif fault == 'can_invalid':
|
||||
cs.canValid = False
|
||||
elif fault == 'eps':
|
||||
cs.steerFaultPermanent = True
|
||||
elif fault == 'no_native':
|
||||
native = None
|
||||
elif fault == 'lat_denied':
|
||||
native = replace(native, lateralAllowed=False)
|
||||
elif fault == 'native_inventory':
|
||||
native = replace(native, pandaInventory=(replace(native.pandaInventory[0], hardwareSerial='other'),))
|
||||
allowed = physical_cancel_lateral_allowed(card.CP, cs, intent, native, source_ns=source, now_ns=now)
|
||||
assert allowed == (fault is None)
|
||||
assert aol_no_entry([EventName.buttonCancel], cs, paddle_only_cancel=False)
|
||||
assert aol_no_entry([EventName.buttonCancel], cs, paddle_only_cancel=False, allow_lateral_cancel=allowed) == (fault is not None)
|
||||
assert ET.USER_DISABLE in EVENTS[EventName.buttonCancel]
|
||||
assert aol_no_entry([EventName.buttonCancel, EventName.doorOpen], cs,
|
||||
paddle_only_cancel=False, allow_lateral_cancel=allowed)
|
||||
|
||||
|
||||
def test_native_lat_only_after_long_belt_loss_keeps_saved_lateral_intent():
|
||||
from openpilot.starpilot.aol.intent import disarming_fault
|
||||
from openpilot.starpilot.nostalgia import aol_no_entry
|
||||
from openpilot.selfdrive.selfdrived.events import Events, EventName
|
||||
with OpenpilotPrefix():
|
||||
card, feedback, native = baseline()
|
||||
for tick, unlatched in ((1, True), (2, True), (3, False)):
|
||||
now = NOW + tick * 10_000_000
|
||||
cs = parse(card, now, counter=tick)
|
||||
cs.seatbeltUnlatched = unlatched
|
||||
native = replace(native, observedMonoTime=now, validUntilMonoTime=now + 200_000_000,
|
||||
requestedLongitudinal=False, longitudinalAllowed=False,
|
||||
pandaInventory=(replace(native.pandaInventory[0], controlsAllowed=False),))
|
||||
sm, _ = peers(card.CP, card.slc_producer_session, now, controls=False, native=native,
|
||||
sequence=tick + 1, long_active=False)
|
||||
events = Events()
|
||||
if unlatched:
|
||||
events.add(EventName.seatbeltNotLatched)
|
||||
wire_events = sm.messages['onroadEvents'].as_builder()
|
||||
wire_events.onroadEvents = events.to_msg()
|
||||
sm = Peers([sm.messages['aolSafetyWire'], sm.messages['pandaStates'],
|
||||
sm.messages['aolAxisState'], wire_events.as_reader()])
|
||||
reset = feedback.observe(sm, card.CP, cs, now, latched=True,
|
||||
intent_producer_session=card.slc_producer_session)
|
||||
card.aol_card_intent.update(cs, fault_active=reset or disarming_fault(events.to_msg(), cs), now_ns=now)
|
||||
assert not reset and not feedback.failed and card.aol_card_intent.allowed_latch
|
||||
allowed, pause_lat, pause_long = card.aol_card_intent.output(cs)
|
||||
decision = decide_axes(standard_lateral=True, standard_longitudinal=unlatched,
|
||||
intent=SimpleNamespace(allowedLatch=allowed, pauseLateral=pause_lat, pauseLongitudinal=pause_long),
|
||||
native=native, car_state=cs, initialized=True, model_ready=True,
|
||||
no_entry=aol_no_entry(events.names, cs, paddle_only_cancel=False),
|
||||
immediate_disable=False, dm_lockout=False, pause_brake_mps=0.)
|
||||
assert decision.lateral_active
|
||||
assert not decision.desired_longitudinal and not decision.longitudinal_active
|
||||
assert feedback.gm_withdrawal_ns == 0
|
||||
|
||||
|
||||
@pytest.mark.parametrize('fault', [None, 'stale_source', 'future_source', 'future_observation',
|
||||
'excessive_lease', 'expired', 'no_expiry'])
|
||||
def test_real_card_published_cancel_intent_preserves_producer_lease_and_freshness(fault):
|
||||
from openpilot.starpilot.aol.runtime import current_intent
|
||||
from openpilot.starpilot.aol.wire import decode_intent
|
||||
from openpilot.starpilot.car.gm.aol import physical_cancel_lateral_allowed
|
||||
|
||||
class PublishedCarState(Exception):
|
||||
pass
|
||||
|
||||
with OpenpilotPrefix():
|
||||
card, _, native = baseline()
|
||||
now = NOW + 10_000_000
|
||||
cs = parse(card, now, 'cancel', counter=1)
|
||||
published = {}
|
||||
real_new_message = messaging.new_message
|
||||
|
||||
def stamped_message(service=None, size=None, **kwargs):
|
||||
return real_new_message(service, size, **({'logMonoTime': now} | kwargs))
|
||||
|
||||
def capture(service, message):
|
||||
wire = message if isinstance(message, bytes) else message.to_bytes()
|
||||
published[service] = messaging.log_from_bytes(wire)
|
||||
if service == 'carState':
|
||||
raise PublishedCarState
|
||||
|
||||
with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped_message), \
|
||||
patch.object(card.pm, 'send', side_effect=capture):
|
||||
with pytest.raises(PublishedCarState):
|
||||
card.state_publish(cs, None)
|
||||
source = int(published['carState'].logMonoTime)
|
||||
cs = published['carState'].carState
|
||||
intent = decode_intent(published['aolIntentWire'].aolIntentWire)
|
||||
assert intent is not None
|
||||
assert intent.carStateLogMonoTime == intent.observedMonoTime == source == now
|
||||
assert intent.validUntilMonoTime - intent.observedMonoTime == 200_000_000
|
||||
admitted = current_intent(Peers([published['aolIntentWire']]), car_state_ns=source, now_ns=now)
|
||||
assert admitted == intent
|
||||
# Faults modify only the negative cases after the actual producer boundary.
|
||||
if fault == 'stale_source':
|
||||
now += 30_000_001
|
||||
elif fault == 'future_source':
|
||||
source += 1
|
||||
elif fault == 'future_observation':
|
||||
intent = replace(intent, observedMonoTime=now + 1)
|
||||
elif fault == 'excessive_lease':
|
||||
intent = replace(intent, validUntilMonoTime=intent.observedMonoTime + 200_000_001)
|
||||
elif fault == 'expired':
|
||||
intent = replace(intent, validUntilMonoTime=now - 1)
|
||||
elif fault == 'no_expiry':
|
||||
intent = replace(intent, validUntilMonoTime=0)
|
||||
assert physical_cancel_lateral_allowed(card.CP, cs, intent, native,
|
||||
source_ns=source, now_ns=now) == (fault is None)
|
||||
@@ -222,6 +222,7 @@ class TransportPauseFeedback:
|
||||
self.longitudinal_cancel_ns = 0
|
||||
self.last_cancel_source_ns = 0
|
||||
self.honda_brake_ns = 0
|
||||
self.gm_withdrawal_ns = 0
|
||||
self.retention_qualified = False
|
||||
self.retention_pending = False
|
||||
self.retention_event_ns = 0
|
||||
@@ -498,7 +499,7 @@ class TransportPauseFeedback:
|
||||
self.awaiting = True
|
||||
return True
|
||||
|
||||
def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, brake_source_ns=0, intent_producer_session=''):
|
||||
def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, brake_source_ns=0, gm_withdrawal_source_ns=0, intent_producer_session=''):
|
||||
self.qualified = False
|
||||
self.awaiting = False
|
||||
self.retention_qualified = False
|
||||
@@ -534,6 +535,17 @@ class TransportPauseFeedback:
|
||||
self.healthy_native_ns = int(brake_native.observedMonoTime)
|
||||
self.sequence, self.signature = int(axis.sequence), signature
|
||||
return False
|
||||
if current and continuity and (gm_withdrawal_source_ns or self.gm_withdrawal_ns):
|
||||
from openpilot.starpilot.car.gm.aol import retained_physical_longitudinal_intent
|
||||
gm_native = retained_physical_longitudinal_intent(self, sm, cp, cs, axis, now_ns,
|
||||
latched=latched, source_ns=gm_withdrawal_source_ns, producer=intent_producer_session)
|
||||
if gm_native is not None:
|
||||
# Existing GM intent only; independent runtime/native TX still owns
|
||||
# both axes and every original source/native/output deadline.
|
||||
self.healthy_feedback_ns = stamp
|
||||
self.healthy_native_ns = int(gm_native.observedMonoTime)
|
||||
self.sequence, self.signature = int(axis.sequence), signature
|
||||
return False
|
||||
if self._source_availability_retention(sm, cp, cs, axis, stamp, now_ns,
|
||||
latched=latched, continuity=continuity, producer=intent_producer_session, signature=signature):
|
||||
self.sequence, self.signature = int(axis.sequence), signature
|
||||
@@ -586,6 +598,8 @@ class TransportPauseFeedback:
|
||||
self.signature = signature
|
||||
return False
|
||||
if transport_healthy:
|
||||
if not latched or native is not None and not native.requestedLongitudinal:
|
||||
self.gm_withdrawal_ns = 0
|
||||
if native is not None and not native.requestedLongitudinal:
|
||||
self.honda_brake_ns = 0
|
||||
if (not latched or native is not None and not native.requestedLongitudinal or
|
||||
|
||||
@@ -91,3 +91,98 @@ def intent_disarming_fault(cp, events, state, **context):
|
||||
if qualified_gm(cp) and camera_acc_pedal_profile(cp) is None and is_volt_ascm_longitudinal(cp):
|
||||
events = [event for event in events if event.name != log.OnroadEvent.EventName.accFaulted]
|
||||
return disarming_fault(events, state, **context)
|
||||
|
||||
|
||||
def physical_longitudinal_withdrawal_source(cp, cs, parser, observation, now_ns) -> int:
|
||||
"""Bind a Bolt pedal-owner withdrawal to its parsed CAN and local receipt."""
|
||||
from opendbc.car.gm.values import CruiseButtons, is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
try:
|
||||
if (not qualified_gm(cp) or not is_bolt_present_no_acc_pedal_profile(cp) or
|
||||
int(cp.alternativeExperience) != GM_AOL_ALTERNATIVE_EXPERIENCE or
|
||||
observation is None or observation[0] is not cs or not cs.canValid or cs.canTimeout):
|
||||
return 0
|
||||
fields = []
|
||||
if cs.brakePressed and parser.vl['ECMEngineStatus']['BrakePressed']:
|
||||
fields.append(('ECMEngineStatus', 'BrakePressed'))
|
||||
if cs.regenBraking and parser.vl['EBCMRegenPaddle']['RegenPaddle']:
|
||||
fields.append(('EBCMRegenPaddle', 'RegenPaddle'))
|
||||
if (parser.vl['ASCMSteeringButton']['ACCButtons'] == CruiseButtons.CANCEL and
|
||||
any(e.type == car.CarState.ButtonEvent.Type.cancel and e.pressed for e in cs.buttonEvents)):
|
||||
fields.append(('ASCMSteeringButton', 'ACCButtons'))
|
||||
_, received_ns, can_source_ns, boot_ns = observation
|
||||
if not fields or not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000:
|
||||
return 0
|
||||
# Parser/CAN batch use BOOTTIME. The fixed deadline uses the original
|
||||
# local MONOTONIC receipt; neither clock is converted or restamped.
|
||||
stamps = [int(parser.ts_nanos[message][signal]) for message, signal in fields]
|
||||
if not all(0 < stamp <= can_source_ns <= boot_ns and boot_ns - stamp <= 30_000_000 and
|
||||
can_source_ns - stamp <= 30_000_000 for stamp in stamps):
|
||||
return 0
|
||||
return received_ns
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return 0
|
||||
|
||||
|
||||
def retained_physical_longitudinal_intent(feedback, sm, cp, cs, axis, now_ns, *, latched, source_ns, producer):
|
||||
"""Retain accepted LAT intent through a bounded physical LONG withdrawal."""
|
||||
from opendbc.car.gm.values import is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS
|
||||
from openpilot.starpilot.aol.transport_pause import independent_lateral_health
|
||||
occurrence = feedback.gm_withdrawal_ns or source_ns
|
||||
if (not qualified_gm(cp) or not is_bolt_present_no_acc_pedal_profile(cp) or
|
||||
int(cp.alternativeExperience) != GM_AOL_ALTERNATIVE_EXPERIENCE or
|
||||
not latched or feedback.failed or feedback.paused or feedback.pending_event or
|
||||
feedback.retention_started_ns or not feedback.session or str(axis.sessionId) != feedback.session or
|
||||
not feedback.healthy_feedback_ns or not 0 <= now_ns - feedback.healthy_feedback_ns <= AXIS_MAX_AGE_NS or
|
||||
not 0 < occurrence <= now_ns or now_ns - occurrence > SAFETY_MAX_AGE_NS or
|
||||
not feedback.gm_withdrawal_ns and now_ns - occurrence > AXIS_MAX_AGE_NS or
|
||||
not producer or producer != feedback.availability_producer or
|
||||
str(axis.intentProducerSessionId) != producer or int(axis.intentIntegrityEpoch) != feedback.availability_epoch or
|
||||
cs.accFaulted or cs.seatbeltUnlatched or cs.doorOpen or not cs.cruiseState.available or
|
||||
cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.low,
|
||||
car.CarState.GearShifter.sport, car.CarState.GearShifter.brake) or
|
||||
str(axis.faultReason) != 'none' or not axis.desiredLateral or not axis.lateralActive or
|
||||
not axis.nativeAcknowledged or
|
||||
not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime <= now_ns or
|
||||
now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS):
|
||||
return None
|
||||
event_ns = int(sm.logMonoTime['onroadEvents'])
|
||||
if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or
|
||||
not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000 or
|
||||
any(e.immediateDisable or e.softDisable for e in sm['onroadEvents'])):
|
||||
return None
|
||||
native = independent_lateral_health(sm, cp, cs, feedback.session, now_ns)
|
||||
if (native is None or not native.pandaInventory or not native.requestedLongitudinal or
|
||||
not any(not p.controlsAllowed for p in sm['pandaStates']) or
|
||||
native.observedMonoTime < feedback.healthy_native_ns or
|
||||
(native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != feedback.healthy_inventory):
|
||||
return None
|
||||
# One occurrence spans the held native request. Held/released samples and
|
||||
# duplicate Card calls cannot renew it; output/native leases remain original.
|
||||
feedback.gm_withdrawal_ns = occurrence
|
||||
return native
|
||||
|
||||
|
||||
def physical_cancel_lateral_allowed(cp, cs, intent, native, *, source_ns, now_ns) -> bool:
|
||||
"""Exact armed Bolt Cancel removes only the lateral NO_ENTRY restriction."""
|
||||
from opendbc.car.gm.values import is_bolt_present_no_acc_pedal_profile
|
||||
from opendbc.car.structs import car
|
||||
from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, SAFETY_MAX_AGE_NS, native_inventory_matches_cp
|
||||
try:
|
||||
return bool(qualified_gm(cp) and is_bolt_present_no_acc_pedal_profile(cp) and
|
||||
int(cp.alternativeExperience) == GM_AOL_ALTERNATIVE_EXPERIENCE and
|
||||
intent is not None and intent.settingsQualified and intent.allowedLatch and intent.lateralArmed and
|
||||
intent.producerSessionId and intent.carStateLogMonoTime == source_ns and
|
||||
0 < source_ns <= intent.observedMonoTime <= now_ns <= intent.validUntilMonoTime and
|
||||
now_ns - source_ns <= AXIS_MAX_AGE_NS and
|
||||
# Card declares a 200ms wire lease; admission and this source remain 30ms fresh.
|
||||
intent.validUntilMonoTime - intent.observedMonoTime <= SAFETY_MAX_AGE_NS and
|
||||
cs.canValid and not cs.canTimeout and not cs.steerFaultTemporary and not cs.steerFaultPermanent and
|
||||
any(e.type == car.CarState.ButtonEvent.Type.cancel for e in cs.buttonEvents) and
|
||||
native is not None and native.requestedLateral and native.lateralAllowed and
|
||||
native.pandaInventory and native_inventory_matches_cp(native, cp) and
|
||||
0 < native.observedMonoTime <= now_ns <= native.validUntilMonoTime)
|
||||
except (AttributeError, KeyError, IndexError, TypeError, ValueError):
|
||||
return False
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,4 +1,4 @@
|
||||
#pragma once
|
||||
|
||||
extern const uint8_t gitversion[29];
|
||||
const uint8_t gitversion[29] = "DEV-7f24dc5b-P92eb5651-DEBUG";
|
||||
const uint8_t gitversion[29] = "DEV-5f95832a-P92eb5651-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
DEV-7f24dc5b-P92eb5651-DEBUG
|
||||
DEV-5f95832a-P92eb5651-DEBUG
|
||||
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "311c3d2052bcf23f706cf30ea41b17ded44053d04e045c5fcd2fa76666bf7705", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "2dd7ad9f6f5f266e5b92c8f11e91983ffbc663ad544207e17f5a9f77fe0e6e0c", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user