diff --git a/RELEASES.md b/RELEASES.md index 9e298589..3acc674e 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,10 @@ +Carrot2-v9 (2025-10-17) +======================== +* Nuggets In Dijon model +* Setting: Adjust traffic stop distance +* liveLocationKalman -> livePose +* C3x lite + Carrot2-v9 (2025-09-21) ======================== * GWM Model diff --git a/cereal/log.capnp b/cereal/log.capnp index e3e600d0..99e4fc7b 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -1179,6 +1179,7 @@ struct ModelDataV2 { desire @14 :Desire; laneChangeProb @15 :Float32; desireLog @16 : Text; + modelTurnSpeed @17 :Float32; # deprecated diff --git a/cereal/services.py b/cereal/services.py index 186f63b7..2c3c80de 100644 --- a/cereal/services.py +++ b/cereal/services.py @@ -51,7 +51,7 @@ _services: dict[str, tuple] = { "gnssMeasurements": (True, 10., 10), "clocks": (True, 0.1, 1), "ubloxRaw": (True, 20.), - #"livePose": (True, 20., 4), + "livePose": (True, 20., 4), "liveLocationKalman": (True, 20., 5), "liveParameters": (True, 20., 5), "cameraOdometry": (True, 20., 10), diff --git a/common/mock/__init__.py b/common/mock/__init__.py index 8c86bbd3..86d44d1b 100644 --- a/common/mock/__init__.py +++ b/common/mock/__init__.py @@ -8,11 +8,12 @@ import functools import threading from cereal.messaging import PubMaster from cereal.services import SERVICE_LIST -from openpilot.common.mock.generators import generate_liveLocationKalman +from openpilot.common.mock.generators import generate_livePose, generate_liveLocationKalman from openpilot.common.realtime import Ratekeeper MOCK_GENERATOR = { + "livePose": generate_livePose "liveLocationKalman": generate_liveLocationKalman } diff --git a/common/mock/generators.py b/common/mock/generators.py index 40951faf..1758ed6a 100644 --- a/common/mock/generators.py +++ b/common/mock/generators.py @@ -18,3 +18,16 @@ def generate_liveLocationKalman(location=LOCATION1): msg.liveLocationKalman.status = 'valid' msg.liveLocationKalman.gpsOK = True return msg + + +def generate_livePose(): + msg = messaging.new_message('livePose') + meas = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'xStd': 0.0, 'yStd': 0.0, 'zStd': 0.0, 'valid': True} + msg.livePose.orientationNED = meas + msg.livePose.velocityDevice = meas + msg.livePose.angularVelocityDevice = meas + msg.livePose.accelerationDevice = meas + msg.livePose.inputsOK = True + msg.livePose.posenetOK = True + msg.livePose.sensorsOK = True + return msg diff --git a/common/params_keys.h b/common/params_keys.h index d0ef4c53..846c095e 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -184,6 +184,7 @@ inline static std::unordered_map keys = { {"AutoNaviCountDownMode", PERSISTENT}, {"TurnSpeedControlMode", PERSISTENT}, {"MapTurnSpeedFactor", PERSISTENT}, + {"ModelTurnSpeedFactor", PERSISTENT}, {"StoppingAccel", PERSISTENT}, {"AutoSpeedUptoRoadSpeedLimit", PERSISTENT}, {"AutoRoadSpeedAdjust", PERSISTENT}, @@ -208,6 +209,7 @@ inline static std::unordered_map keys = { {"MyDrivingModeAuto", PERSISTENT}, {"TrafficLightDetectMode", PERSISTENT}, {"SteerActuatorDelay", PERSISTENT}, + {"LatSmoothSec", PERSISTENT}, {"CruiseOnDist", PERSISTENT}, {"CruiseMaxVals0", PERSISTENT}, {"CruiseMaxVals1", PERSISTENT}, @@ -242,6 +244,8 @@ inline static std::unordered_map keys = { {"TFollowGap4", PERSISTENT}, {"DynamicTFollow", PERSISTENT}, {"DynamicTFollowLC", PERSISTENT}, + {"AChangeCostStarting", PERSISTENT}, + {"TrafficStopDistanceAdjust", PERSISTENT}, {"HapticFeedbackWhenSpeedCamera", PERSISTENT}, {"UseLaneLineSpeed", PERSISTENT}, {"UseLaneLineCurveSpeed", PERSISTENT}, @@ -289,4 +293,6 @@ inline static std::unordered_map keys = { {"NNFF", PERSISTENT}, {"NNFFLite", PERSISTENT}, {"NNFFModelName", CLEAR_ON_OFFROAD_TRANSITION}, + + {"HardwareC3xLite", PERSISTENT}, }; diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 11c36497..0901fea1 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -90,7 +90,6 @@ function launch { fi # events language init - #LANG=$(cat ${PARAMS_ROOT}/d/LanguageSetting) LANG=$(cat /data/params/d/LanguageSetting) EVENTSTAT=$(git status) @@ -102,6 +101,16 @@ function launch { cp -f $DIR/scripts/add/events_en.py $DIR/selfdrive/selfdrived/events.py fi + # c3xl amplifier file change + C3XL=$(cat /data/params/d/HardwareC3xLite) + + if [ "${C3XL}" = "1" ] && [[ ! "${EVENTSTAT}" == *"modified: system/hardware/tici/amplifier.py"* ]]; then + cp -f $DIR/system/hardware/tici/amplifier.py $DIR/scripts/add/amplifier_org.py + cp -f $DIR/scripts/add/amplifier_c3xl.py $DIR/system/hardware/tici/amplifier.py + elif [ "${C3XL}" = "0" ] && [[ "${EVENTSTAT}" == *"modified: system/hardware/tici/amplifier.py"* ]]; then + cp -f $DIR/scripts/add/amplifier_org.py $DIR/system/hardware/tici/amplifier.py + fi + # start manager cd system/manager if [ ! -f $DIR/prebuilt ]; then diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 55bb891b..6489aaf9 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -355,8 +355,9 @@ class CarController(CarControllerBase): # 5 Hz ACC options if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl: if camera_scc: - if CS.scc13 is not None: - can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer)) + #if CS.scc13 is not None: + # can_sends.append(hyundaican.create_acc_opt_copy(CS, self.packer)) + pass else: can_sends.extend(hyundaican.create_acc_opt(self.packer, self.CP)) diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 5e59650b..0704dd81 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -559,7 +559,7 @@ class CarState(CarStateBase): if lane_info is not None: left_lane_prob = lane_info["LEFT_LANE_PROB"] right_lane_prob = lane_info["RIGHT_LANE_PROB"] - left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 4: double solid, solid+dashed, 5:dashed + solid + left_lane_type = lane_info["LEFT_LANE_TYPE"] # 0: dashed, 1: solid, 2: undecided, 3: road edge, 4: DLM Inner Solid, 5: DLM InnerDashed, 6:DLM Inner Undecided, 7: Botts Dots, 8: Barrier right_lane_type = lane_info["RIGHT_LANE_TYPE"] left_lane_color = lane_info["LEFT_LANE_COLOR"] right_lane_color = lane_info["RIGHT_LANE_COLOR"] diff --git a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc index 9e9e0a2f..b3bde5fd 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_kia_generic.dbc @@ -1395,6 +1395,8 @@ BO_ 1290 SCC13: 8 SCC SG_ SCC_Equip : 3|1@1+ (1,0) [0|1] "" ESC SG_ AebDrvSetStatus : 4|3@1+ (1,0) [0|7] "" CLU,ESC SG_ Lead_Veh_Dep_Alert_USM : 13|2@0+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_1 : 23|8@0+ (1,0) [0|255] "" XXX + SG_ NEW_SIGNAL_2 : 31|8@0+ (1,0) [0|255] "" XXX BO_ 1287 TCS15: 4 ESC SG_ ABS_W_LAMP : 0|1@1+ (1.0,0.0) [0.0|1.0] "" _4WD,CLU,CUBIS,IBOX @@ -1489,8 +1491,10 @@ BO_ 905 SCC14: 8 SCC SG_ ComfortBandLower : 6|6@1+ (0.02,0) [0|1.26] "m/s^2" ESC SG_ JerkUpperLimit : 12|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC SG_ JerkLowerLimit : 19|7@1+ (0.1,0) [0|12.7] "m/s^3" ESC + SG_ NEW_SIGNAL_1 : 26|6@1+ (1,0) [0|63] "" XXX SG_ ACCMode : 32|3@1+ (1,0) [0|7] "" CLU,HUD,LDWS_LKAS,ESC SG_ ObjDistStat : 42|2@1+ (1,0) [0|3] "" XXX + SG_ NEW_SIGNAL_2 : 55|8@0+ (1,0) [0|255] "" XXX SG_ ObjGap : 56|8@1+ (1,0) [0|255] "" CLU,HUD,ESC BO_ 1157 LFAHDA_MFC: 8 XXX diff --git a/opendbc_repo/opendbc/safety/safety/safety_hyundai.h b/opendbc_repo/opendbc/safety/safety/safety_hyundai.h index b0964823..cbaa854b 100644 --- a/opendbc_repo/opendbc/safety/safety/safety_hyundai.h +++ b/opendbc_repo/opendbc/safety/safety/safety_hyundai.h @@ -197,6 +197,7 @@ static void hyundai_rx_hook(const CANPacket_t *to_push) { uint32_t last_ts_lkas11_from_op = 0; uint32_t last_ts_scc12_from_op = 0; +uint32_t last_ts_scc13_from_op = 0; uint32_t last_ts_mdps12_from_op = 0; uint32_t last_ts_fca11_from_op = 0; @@ -284,6 +285,8 @@ static bool hyundai_tx_hook(const CANPacket_t *to_send) { last_ts_mdps12_from_op = (tx == 0 ? 0 : microsecond_timer_get()); else if(addr == 909) last_ts_fca11_from_op = (tx == 0 ? 0 : microsecond_timer_get()); + else if(addr == 1290) + last_ts_scc13_from_op = (tx == 0 ? 0 : microsecond_timer_get()); return tx; } @@ -308,10 +311,11 @@ static int hyundai_fwd_hook(int bus_num, int addr) { if (bus_num == 2) { bool is_lkas_msg = addr == 832; bool is_lfahda_msg = addr == 1157; - bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 1290 || addr == 905; + bool is_scc_msg = addr == 1056 || addr == 1057 || addr == 905; + bool is_scc13_msg = addr == 1290; bool is_fca_msg = addr == 909 || addr == 1155; - bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg; //|| is_fca_msg; + bool block_msg = is_lkas_msg || is_lfahda_msg || is_scc_msg || is_scc13_msg; //|| is_fca_msg; if (!block_msg) { bus_fwd = 0; } @@ -321,8 +325,12 @@ static int hyundai_fwd_hook(int bus_num, int addr) { bus_fwd = 0; } } - else if(is_scc_msg) { - if(now - last_ts_scc12_from_op >= 400000) + else if (is_scc_msg) { + if (now - last_ts_scc12_from_op >= 400000) + bus_fwd = 0; + } + else if (is_scc13_msg) { + if (now - last_ts_scc13_from_op >= 400000) bus_fwd = 0; } else if(is_fca_msg) { diff --git a/scripts/add/amplifier_c3xl.py b/scripts/add/amplifier_c3xl.py new file mode 100644 index 00000000..4fee9831 --- /dev/null +++ b/scripts/add/amplifier_c3xl.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +import time +from smbus2 import SMBus +from collections import namedtuple + +# https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf + +AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask']) +EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2']) + +def configs_from_eq_params(base, eq_params): + return [ + AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF), + AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF), + AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF), + AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF), + AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF), + AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF), + AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF), + AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF), + AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF), + AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF), + ] + +BASE_CONFIG = [ + AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000), + AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000), + AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011), + AmpConfig("Enable PLL1", 0b1, 0x12, 7, 0b10000000), + AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000), + AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100), + AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100), + AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000), + AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000), + AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111), + AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111), + AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001), + AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000), + AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000), + AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000), + AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000), + AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001), + AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000), + AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111), + AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000), + AmpConfig("Excursion limiter lower corner freq", 0b00, 0x41, 0, 0b00000011), + AmpConfig("Excursion limiter threshold", 0b000, 0x42, 0, 0b00001111), + AmpConfig("Distortion limit (THDCLP)", 0x6, 0x46, 4, 0b11110000), + AmpConfig("Distortion limiter release time constant", 0b0, 0x46, 0, 0b00000001), + AmpConfig("Right DAC input mixer: DAI1 left", 0b0, 0x22, 3, 0b00001000), + AmpConfig("Right DAC input mixer: DAI1 right", 0b0, 0x22, 2, 0b00000100), + AmpConfig("Right DAC input mixer: DAI2 left", 0b1, 0x22, 1, 0b00000010), + AmpConfig("Right DAC input mixer: DAI2 right", 0b0, 0x22, 0, 0b00000001), + AmpConfig("DAI1 audio port selector", 0b10, 0x16, 6, 0b11000000), + AmpConfig("DAI2 audio port selector", 0b01, 0x1E, 6, 0b11000000), + AmpConfig("Enable left digital microphone", 0b1, 0x48, 5, 0b00100000), + AmpConfig("Enable right digital microphone", 0b1, 0x48, 4, 0b00010000), + AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000), + AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000), + AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000), +] + +CONFIGS = { + "tici": [ + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), + AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), + + *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)), + *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)), + *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)), + *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)), + *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)), + ], + "tizi": [ + AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), + AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), + + AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), + AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), + AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), + AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), + AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000), + AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111), + AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000), + AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001), + AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000), + AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000), + AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000), + AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000), + AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), + AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), + AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), + ], +} + +class Amplifier: + AMP_I2C_BUS = 0 + AMP_ADDRESS = 0x10 + + def __init__(self, debug=False): + self.debug = debug + + def _get_shutdown_config(self, amp_disabled: bool) -> AmpConfig: + return AmpConfig("Global shutdown", 0b0 if amp_disabled else 0b1, 0x51, 7, 0b10000000) + + def _set_configs(self, configs: list[AmpConfig]) -> None: + with SMBus(self.AMP_I2C_BUS) as bus: + for config in configs: + if self.debug: + print(f"Setting \"{config.name}\" to {config.value}:") + + old_value = bus.read_byte_data(self.AMP_ADDRESS, config.register, force=True) + new_value = (old_value & (~config.mask)) | ((config.value << config.offset) & config.mask) + bus.write_byte_data(self.AMP_ADDRESS, config.register, new_value, force=True) + + if self.debug: + print(f" Changed {hex(config.register)}: {hex(old_value)} -> {hex(new_value)}") + + def set_configs(self, configs: list[AmpConfig]) -> bool: + # retry in case panda is using the amp + tries = 1 + for i in range(1): + try: + self._set_configs(configs) + return True + except OSError: + print(f"Failed to set amp config, {tries - i - 1} retries left") + time.sleep(0.02) + return False + + def set_global_shutdown(self, amp_disabled: bool) -> bool: + return self.set_configs([self._get_shutdown_config(amp_disabled), ]) + + def initialize_configuration(self, model: str) -> bool: + cfgs = [ + self._get_shutdown_config(True), + *BASE_CONFIG, + *CONFIGS[model], + self._get_shutdown_config(False), + ] + return self.set_configs(cfgs) + + +#if __name__ == "__main__": + #with open("/sys/firmware/devicetree/base/model") as f: + # model = f.read().strip('\x00') + #model = model.split('comma ')[-1] + + #amp = Amplifier() + #amp.initialize_configuration(model) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 1f8a6312..fbb5ecf8 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -342,7 +342,7 @@ class VCruiseCarrot: self.v_cruise_kph = np.clip(v_cruise_kph, 30, self._cruise_speed_max) self.v_cruise_cluster_kph = self.v_cruise_kph else: - self.v_cruise_kph = max(20, self.v_ego_kph_set) #V_CRUISE_UNSET + self.v_cruise_kph = np.clip(v_cruise_kph, self._cruise_speed_min, self._cruise_speed_max) #max(20, self.v_ego_kph_set) #V_CRUISE_UNSET self.v_cruise_cluster_kph = self.v_cruise_kph #V_CRUISE_UNSET #if self.cruise_state_available_last: # 최초 한번이라도 cruiseState.available이 True였다면 # self._lat_enabled = False @@ -495,7 +495,7 @@ class VCruiseCarrot: if self._soft_hold_active > 0: self._soft_hold_active = 0 elif self._cruise_ready or not CC.enabled or CS.cruiseState.standstill or self.carrot_cruise_active: - if self._cruise_button_mode in [2, 3]: + if False: #self._cruise_button_mode in [2, 3]: road_limit_kph = self.nRoadLimitSpeed * self.autoSpeedUptoRoadSpeedLimit if road_limit_kph > 1.0: v_cruise_kph = max(v_cruise_kph, road_limit_kph) @@ -636,7 +636,7 @@ class VCruiseCarrot: v_cruise_kph = self.nRoadLimitSpeed + self.autoRoadSpeedLimitOffset elif self.nRoadLimitSpeed < self.nRoadLimitSpeed_last and self.autoRoadSpeedAdjust > 0: new_road_limit_kph = self.nRoadLimitSpeed * self.autoRoadSpeedAdjust + v_cruise_kph * (1 - self.autoRoadSpeedAdjust) - self._add_log(f"AutoSpeed change {v_cruise_kph} -> {new_road_limit_kph}") + self._add_log(f"AutoSpeed change {v_cruise_kph} -> {new_road_limit_kph:.1f}") v_cruise_kph = min(v_cruise_kph, new_road_limit_kph) self.road_limit_kph = road_limit_kph self.nRoadLimitSpeed_last = self.nRoadLimitSpeed diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index 9cb554ee..f5bb7dac 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -79,7 +79,7 @@ class CarrotPlanner: self.stopSignCount = 0 self.stop_distance = 6.0 - self.trafficStopDistanceAdjust = 2.0 #params.get_float("TrafficStopDistanceAdjust") / 100. + self.trafficStopDistanceAdjust = 2.5 #params.get_float("TrafficStopDistanceAdjust") / 100. self.comfortBrake = 2.4 self.comfort_brake = self.comfortBrake @@ -110,6 +110,8 @@ class CarrotPlanner: self.cruiseMaxVals5 = 0.7 self.cruiseMaxVals6 = 0.6 + self.aChangeCostStarting = 10.0 + self.trafficLightDetectMode = 2 # 0: None, 1:Stop, 2:Stop&Go self.trafficState_carrot = 0 self.carrot_stay_stop = False @@ -170,7 +172,8 @@ class CarrotPlanner: self.j_lead_factor = self.params.get_float("JLeadFactor3") / 100. self.eco_over_speed = self.params.get_int("CruiseEcoControl") self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01 - + self.aChangeCostStaring = self.params.get_float("AChangeCostStarting") + self.trafficStopDistanceAdjust = self.params.get_float("TrafficStopDistanceAdjust") / 100. elif self.params_count >= 100: self.params_count = 0 @@ -213,7 +216,7 @@ class CarrotPlanner: t_follow *= self.dynamicTFollowLC # 차선변경시 t_follow를 줄임. self.jerk_factor_apply = self.jerk_factor * self.dynamicTFollowLC # 차선변경시 jerk factor를 줄여 aggresive하게 elif lead.status: - t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.2, 0.0]) + t_follow += np.interp(prev_a[0], [-2.0, -0.5], [0.1, 0.0]) if self.dynamicTFollow > 0.0: gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0) * 0.1 t_follow += gap_dist_adjust @@ -350,9 +353,9 @@ class CarrotPlanner: leadOne = radarstate.leadOne self.mySafeFactor = 1.0 - if leadOne.status and leadOne.vLead < 5: - self.mySafeFactor = self.mySafeModeFactor - elif self.myDrivingMode == DrivingMode.Eco: # eco + if leadOne.status and leadOne.vLead < 5 and leadOne.aLead < 0.2 and v_ego > 1.0: # 앞차가 매우 느리거나 정지한경우 + self.myDrivingMode = DrivingMode.Safe + if self.myDrivingMode == DrivingMode.Eco: # eco self.mySafeFactor = self.myEcoModeFactor elif self.myDrivingMode == DrivingMode.Safe: #safe self.mySafeFactor = self.mySafeModeFactor diff --git a/selfdrive/carrot/carrot_serv.py b/selfdrive/carrot/carrot_serv.py index f7333293..bdd85ea5 100644 --- a/selfdrive/carrot/carrot_serv.py +++ b/selfdrive/carrot/carrot_serv.py @@ -979,10 +979,15 @@ class CarrotServ: if self.turnSpeedControlMode == 2: if -500 < self.xDistToTurn < 500: speed_n_sources.append((route_speed, "route")) - elif self.turnSpeedControlMode == 3: + elif self.turnSpeedControlMode in [3, 4]: speed_n_sources.append((route_speed, "route")) #speed_n_sources.append((self.calculate_current_speed(dist, speed * self.mapTurnSpeedFactor, 0, 1.2), "route")) + + model_turn_speed = max(sm['modelV2'].meta.modelTurnSpeed, self.autoCurveSpeedLowerLimit) + if model_turn_speed < 200 and abs(vturn_speed) < 120: + speed_n_sources.append((model_turn_speed, "model")) + desired_speed, source = min(speed_n_sources, key=lambda x: x[0]) if CS is not None: diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index 46352eeb..fc2f1a2f 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -261,6 +261,19 @@ "default": 30, "unit": 1 }, + { + "group": "조향튜닝", + "name": "LatSmoothSec", + "title": "LatSmoothSec(13)x0.01", + "descr": "조향필터링값\n 높으면 부드러운조향", + "egroup": "LAT", + "etitle": "LatSmoothSec(30)", + "edescr": "Lat smoothing values — Higher values smoother", + "min": 1, + "max": 30, + "default": 13, + "unit": 1 + }, { "group": "크루즈", "name": "CruiseOnDist", @@ -447,15 +460,41 @@ "group": "주행튜닝", "name": "StopDistanceCarrot", "title": "StopDistance(600)cm", - "descr": "신호등 감지 시 정지위치", + "descr": "정지위치 * 0.8", "egroup": "LONG", "etitle": "StopDistance(600)cm", - "edescr": "Stop Position When Traffic Light Detected", + "edescr": "Stop Distance * 0.8", "min": 400, "max": 1000, "default": 600, "unit": 10 }, + { + "group": "주행튜닝", + "name": "AChangeCostStarting", + "title": "AChangeCostStarting", + "descr": "출발가속도제한, 0:빠른가속도", + "egroup": "LONG", + "etitle": "AChangeCostStarting", + "edescr": "AccelCost Starting, 0:rapid starting", + "min": 0, + "max": 200, + "default": 10, + "unit": 10 + }, + { + "group": "주행튜닝", + "name": "TrafficStopDistanceAdjust", + "title": "TrafficStopDistanceAdjust(400)", + "descr": "신호정지위치 보정", + "egroup": "LONG", + "etitle": "TrafficStopDistanceAdjust", + "edescr": "", + "min": -600, + "max": 600, + "default": 400, + "unit": 100 + }, { "group": "주행튜닝", "name": "JLeadFactor3", @@ -1718,6 +1757,19 @@ "default": 100, "unit": 10 }, + { + "group": "감속제어", + "name": "ModelTurnSpeedFactor", + "title": "모델턴속도반영시간(0)x0.1sec", + "descr": "클수록 모델의 미래속도가 반영됩니다.", + "egroup": "SPEED", + "etitle": "ModelTurnSpeed time(0)x0.1sec", + "edescr": "future model speed", + "min": 0, + "max": 80, + "default": 0, + "unit": 10 + }, { "group": "감속제어", "name": "AutoNaviSpeedSafetyFactor", diff --git a/selfdrive/controls/beep.py b/selfdrive/controls/beep.py new file mode 100755 index 00000000..c6d2a9b3 --- /dev/null +++ b/selfdrive/controls/beep.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +import subprocess +import time +from cereal import car, messaging +from openpilot.common.realtime import Ratekeeper +import threading + +AudibleAlert = car.CarControl.HUDControl.AudibleAlert + +class Beepd: + def __init__(self): + self.current_alert = AudibleAlert.none + self.enable_gpio() + self.startup_beep() + + def enable_gpio(self): + # 尝试 export，忽略已 export 的错误 + try: + subprocess.run("echo 42 | sudo tee /sys/class/gpio/export", + shell=True, + stderr=subprocess.DEVNULL, + stdout=subprocess.DEVNULL, + encoding='utf8') + except Exception: + pass + subprocess.run("echo \"out\" | sudo tee /sys/class/gpio/gpio42/direction", + shell=True, + stderr=subprocess.DEVNULL, + stdout=subprocess.DEVNULL, + encoding='utf8') + + def _beep(self, on): + val = "1" if on else "0" + subprocess.run(f"echo \"{val}\" | sudo tee /sys/class/gpio/gpio42/value", + shell=True, + stderr=subprocess.DEVNULL, + stdout=subprocess.DEVNULL, + encoding='utf8') + + def engage(self): + self._beep(True) + time.sleep(0.05) + self._beep(False) + + def disengage(self): + for _ in range(2): + self._beep(True) + time.sleep(0.01) + self._beep(False) + time.sleep(0.01) + + def warning(self): + for _ in range(3): + self._beep(True) + time.sleep(0.01) + self._beep(False) + time.sleep(0.01) + + def startup_beep(self): + self._beep(True) + time.sleep(0.1) + self._beep(False) + + def ding(self): + self._beep(True) + time.sleep(0.02) + self._beep(False) + + def dong(self): + self._beep(True) + time.sleep(0.03) + self._beep(False) + + def beep(self): + self._beep(True) + time.sleep(0.04) + self._beep(False) + + def dispatch_beep(self, func): + threading.Thread(target=func, daemon=True).start() + + def update_alert(self, new_alert): + if new_alert != self.current_alert: + self.current_alert = new_alert + print(f"[BEEP] New alert: {new_alert}") + if new_alert == AudibleAlert.engage: + self.dispatch_beep(self.engage) + elif new_alert == AudibleAlert.disengage: + self.dispatch_beep(self.disengage) + elif new_alert in [AudibleAlert.refuse, AudibleAlert.prompt, AudibleAlert.warningImmediate,AudibleAlert.warningSoft]: + self.dispatch_beep(self.warning) + elif new_alert in [AudibleAlert.longEngaged, AudibleAlert.longDisengaged, AudibleAlert.trafficSignGreen, AudibleAlert.trafficSignChanged, AudibleAlert.trafficError, AudibleAlert.bsdWarning, AudibleAlert.laneChange]: + self.dispatch_beep(self.ding) + elif new_alert in [AudibleAlert.stopStop, AudibleAlert.stopping, AudibleAlert.autoHold, AudibleAlert.engage2, AudibleAlert.disengage2, AudibleAlert.speedDown, AudibleAlert.audioTurn, AudibleAlert.reverseGear]: + self.dispatch_beep(self.dong) + elif new_alert in [AudibleAlert.audio1, AudibleAlert.audio2, AudibleAlert.audio3, AudibleAlert.audio4, AudibleAlert.audio5, + AudibleAlert.audio6, AudibleAlert.audio7, AudibleAlert.audio8, AudibleAlert.audio9, AudibleAlert.audio10]: + self.dispatch_beep(self.beep) + + def get_audible_alert(self, sm): + if sm.updated['selfdriveState']: + new_alert = sm['selfdriveState'].alertSound.raw + self.update_alert(new_alert) + + def test_beepd_thread(self): + frame = 0 + rk = Ratekeeper(20) + pm = messaging.PubMaster(['selfdriveState']) + while True: + cs = messaging.new_message('selfdriveState') + if frame == 40: + cs.selfdriveState.alertSound = AudibleAlert.engage + if frame == 60: + cs.selfdriveState.alertSound = AudibleAlert.disengage + if frame == 80: + cs.selfdriveState.alertSound = AudibleAlert.prompt + + pm.send("selfdriveState", cs) + frame += 1 + rk.keep_time() + + def beepd_thread(self, test=False): + if test: + threading.Thread(target=self.test_beepd_thread, daemon=True).start() + + sm = messaging.SubMaster(['selfdriveState']) + rk = Ratekeeper(20) + + while True: + sm.update(0) + self.get_audible_alert(sm) + rk.keep_time() + +def main(): + s = Beepd() + s.beepd_thread(test=False) # 改成 True 可启用模拟测试数据 + +if __name__ == "__main__": + main() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2473bd03..5ed5fc95 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -26,6 +26,7 @@ from openpilot.common.realtime import DT_CTRL, DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState @@ -45,15 +46,19 @@ class Controls: self.disable_dm = False - self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', - 'liveCalibration', 'liveLocationKalman', 'longitudinalPlan', 'carState', 'carOutput', - 'liveDelay', 'carrotMan', 'lateralPlan', 'radarState', + self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', + 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', + 'carrotMan', 'lateralPlan', 'radarState', 'liveLocationKalman', 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) self.steer_limited_by_controls = False self.curvature = 0.0 self.desired_curvature = 0.0 + + self.pose_calibrator = PoseCalibrator() + self.calibrated_pose: Pose | None = None + self.yStd = 0.0 self.side_state = { @@ -73,6 +78,11 @@ class Controls: def update(self): self.sm.update(15) + if self.sm.updated["liveCalibration"]: + self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) + if self.sm.updated["livePose"]: + device_pose = Pose.from_live_pose(self.sm['livePose']) + self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) def state_control(self): CS = self.sm['carState'] @@ -138,7 +148,7 @@ class Controls: lat_plan = self.sm['lateralPlan'] curve_speed_abs = abs(self.sm['carrotMan'].vTurnSpeed) self.lanefull_mode_enabled = (lat_plan.useLaneLines and curve_speed_abs > self.params.get_int("UseLaneLineCurveSpeed")) - lat_smooth_seconds = LAT_SMOOTH_SECONDS #self.params.get_float("SteerSmoothSec") * 0.01 + lat_smooth_seconds = self.params.get_float("LatSmoothSec") * 0.01 steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01 if steer_actuator_delay == 0.0: steer_actuator_delay = self.sm['liveDelay'].lateralDelay @@ -227,6 +237,10 @@ class Controls: # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller + #if self.calibrated_pose is not None: + # CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() + # CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() + orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) if len(orientation_value) > 2: CC.orientationNED = orientation_value diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 6448dfe5..144202e3 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -133,6 +133,7 @@ class DesireHelper: self.object_detected_count = 0 self.lane_available_trigger = False self.lane_appeared = False + self.lane_line_info = 0 self.laneChangeNeedTorque = 0 self.laneChangeBsd = 0 @@ -152,6 +153,9 @@ class DesireHelper: self.auto_lane_change_enable = False self.next_lane_change = False + self.modelTurnSpeedFactor = 0.0 + self.model_turn_speed = 0.0 + def check_lane_state(self, modeldata): lane_width_left, self.distance_to_road_edge_left, self.distance_to_road_edge_left_far, lane_prob_left = calculate_lane_width(modeldata.laneLines[0], modeldata.laneLineProbs[0], modeldata.laneLines[1], modeldata.roadEdges[0]) @@ -192,14 +196,25 @@ class DesireHelper: self.turn_disable_count = max(0, self.turn_disable_count - 1) #print(f"desire_state = {desire_state}, turn_desire_state = {self.turn_desire_state}, disable_count = {self.desire_disable_count}") + def make_model_turn_speed(self, modeldata): + if self.modelTurnSpeedFactor > 0: + model_turn_speed = np.interp(self.modelTurnSpeedFactor, modeldata.velocity.t, modeldata.velocity.x) * CV.MS_TO_KPH * 1.2 + self.model_turn_speed = self.model_turn_speed * 0.9 + model_turn_speed * 0.1 + else: + self.model_turn_speed = 200.0 + def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState): if self.frame % 100 == 0: self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque") self.laneChangeBsd = self.params.get_int("LaneChangeBsd") self.laneChangeDelay = self.params.get_float("LaneChangeDelay") * 0.1 + self.modelTurnSpeedFactor= self.params.get_float("ModelTurnSpeedFactor") * 0.1 + self.frame += 1 + self.make_model_turn_speed(modeldata) + self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1) self.lane_change_delay = max(0, self.lane_change_delay - DT_MDL) @@ -268,7 +283,8 @@ class DesireHelper: blinker_state = driver_blinker_state if driver_desire_enabled else atc_blinker_state lane_line_info = carstate.leftLaneLine if blinker_state == BLINKER_LEFT else carstate.rightLaneLine - + + lane_line_info_edge_detect = False if desire_enabled: lane_exist_counter = self.lane_exist_left_count.counter if blinker_state == BLINKER_LEFT else self.lane_exist_right_count.counter lane_available = self.available_left_lane if blinker_state == BLINKER_LEFT else self.available_right_lane @@ -279,7 +295,9 @@ class DesireHelper: side_object_dist = radar.dRel + radar.vLead * 4.0 if radar.status else 255 object_detected = side_object_dist < v_ego * 3.0 self.object_detected_count = max(1, self.object_detected_count + 1) if object_detected else min(-1, self.object_detected_count - 1) - + if lane_line_info % 10 in [0, 5] and self.lane_line_info not in [0, 5]: + lane_line_info_edge_detect = True + self.lane_line_info = lane_line_info % 10 else: lane_exist_counter = 0 lane_available = True @@ -287,6 +305,7 @@ class DesireHelper: self.lane_appeared = False self.lane_available_trigger = False self.object_detected_count = 0 + self.lane_line_info = lane_line_info % 10 #lane_available_trigger = not self.lane_available_last and lane_available lane_change_available = (lane_available or edge_available) and lane_line_info < 20 # lane_line_info가 20보다 작으면 흰색라인임. @@ -371,7 +390,7 @@ class DesireHelper: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: - if lane_change_available and self.lane_change_delay == 0: + if (lane_change_available and self.lane_change_delay == 0) or lane_line_info_edge_detect: if self.blindspot_detected_counter > 0 and not ignore_bsd: # BSD검출시 if torque_applied and not block_lanechange_bsd: self.lane_change_state = LaneChangeState.laneChangeStarting @@ -382,7 +401,7 @@ class DesireHelper: self.lane_change_state = LaneChangeState.laneChangeStarting # ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작 # lane_appeared: 차선이 생기는건 안함.. 위험. - elif torque_applied or auto_lane_change_trigger: + elif torque_applied or auto_lane_change_trigger or lane_line_info_edge_detect: self.lane_change_state = LaneChangeState.laneChangeStarting # LaneChangeState.laneChangeStarting diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index ada1fb52..274bf68a 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -66,6 +66,10 @@ class LongControl: self.stopping_accel = 0 self.j_lead = 0.0 + self.use_accel_pid = False + if CP.brand == "toyota": + self.use_accel_pid = True + def reset(self): self.pid.reset() @@ -121,8 +125,10 @@ class LongControl: self.reset() else: # LongCtrlState.pid - #error = a_target_now - CS.aEgo - error = v_target_now - CS.vEgo + if self.use_accel_pid: + error = a_target_ff - CS.aEgo + else: + error = v_target_now - CS.vEgo output_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target_ff) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 7e9859f5..d061f4a3 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -33,13 +33,13 @@ COST_E_DIM = 5 COST_DIM = COST_E_DIM + 1 CONSTR_DIM = 4 -X_EGO_OBSTACLE_COST = 3. +X_EGO_OBSTACLE_COST = 5. X_EGO_COST = 0. V_EGO_COST = 0. A_EGO_COST = 0. J_EGO_COST = 5.0 -A_CHANGE_COST = 250. -A_CHANGE_COST_STARTING = 30. +A_CHANGE_COST = 200. +A_CHANGE_COST_STARTING = 10. #30. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .25 LEAD_DANGER_FACTOR = 0.8 # 0.75 @@ -294,10 +294,10 @@ class LongitudinalMpc: for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_factor=1.0): + def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, jerk_factor=1.0, a_change_cost_starting=A_CHANGE_COST_STARTING): #jerk_factor = get_jerk_factor(personality) if self.mode == 'acc': - a_change_cost = self.a_change_cost if prev_accel_constraint else A_CHANGE_COST_STARTING + a_change_cost = self.a_change_cost if prev_accel_constraint else a_change_cost_starting cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST] elif self.mode == 'blended': diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index a6fa67fb..f1e3132b 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -171,7 +171,7 @@ class LongitudinalPlanner: accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply) + self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality, jerk_factor = carrot.jerk_factor_apply, a_change_cost_starting = carrot.aChangeCostStarting) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) self.mpc.update(carrot, reset_state, sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality) diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 27cd4d5b..2bd90783 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -7,6 +7,13 @@ rednose_gen_dir = 'models/generated' rednose_gen_deps = [ "models/constants.py", ] +pose_ekf = env.RednoseCompileFilter( + target='pose', + filter_gen_script='models/pose_kf.py', + output_dir=rednose_gen_dir, + extra_gen_artifacts=[], + gen_script_deps=rednose_gen_deps, +) live_ekf = env.RednoseCompileFilter( target='live', filter_gen_script='models/live_kf.py', diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 939cd34c..d7834f7f 100644 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -11,7 +11,7 @@ from cereal.services import SERVICE_LIST from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.helpers import fft_next_good_size, parabolic_peak_interp +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp BLOCK_SIZE = 100 BLOCK_NUM = 50 @@ -151,14 +151,13 @@ class BlockAverage: class LateralLagEstimator: - inputs = {"carControl", "carState", "controlsState", "liveLocationKalman"} + inputs = {"carControl", "carState", "controlsState", "liveCalibration", "livePose"} def __init__(self, CP: car.CarParams, dt: float, block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE, window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC, min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC, - max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE, - enabled: bool = True): + max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE): self.dt = dt self.window_sec = window_sec self.okay_window_sec = okay_window_sec @@ -173,7 +172,6 @@ class LateralLagEstimator: self.min_confidence = min_confidence self.max_lat_accel = max_lat_accel self.max_lat_accel_diff = max_lat_accel_diff - self.enabled = enabled self.t = 0.0 self.lat_active = False @@ -191,7 +189,7 @@ class LateralLagEstimator: self.last_pose_invalid_t = 0.0 self.last_estimate_t = 0.0 - self.calib_valid = False + self.calibrator = PoseCalibrator() self.reset(self.initial_lag, 0) @@ -208,7 +206,7 @@ class LateralLagEstimator: liveDelay = msg.liveDelay valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get() - if self.enabled and self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): + if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std): if valid_std > MAX_LAG_STD: liveDelay.status = log.LiveDelayData.Status.invalid else: @@ -245,11 +243,14 @@ class LateralLagEstimator: elif which == "controlsState": self.steering_saturated = getattr(msg.lateralControlState, msg.lateralControlState.which()).saturated self.desired_curvature = msg.desiredCurvature - elif which == 'liveLocationKalman': - self.yaw_rate = msg.angularVelocityCalibrated.value[2] - self.yaw_rate_std = msg.angularVelocityCalibrated.std[2] - self.pose_valid = msg.angularVelocityCalibrated.valid and msg.posenetOK and msg.inputsOK - self.calib_valid = msg.status == log.LiveLocationKalman.Status.valid + elif which == "liveCalibration": + self.calibrator.feed_live_calib(msg) + elif which == "livePose": + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + self.yaw_rate = calibrated_pose.angular_velocity.yaw + self.yaw_rate_std = calibrated_pose.angular_velocity.yaw_std + self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK self.t = t def points_enough(self): @@ -266,7 +267,7 @@ class LateralLagEstimator: turning = np.abs(self.yaw_rate) >= self.min_yr sensors_valid = self.pose_valid and np.abs(self.yaw_rate) < MAX_YAW_RATE_SANITY_CHECK and self.yaw_rate_std < MAX_YAW_RATE_SANITY_CHECK la_valid = np.abs(la_actual_pose) <= self.max_lat_accel and np.abs(la_desired - la_actual_pose) <= self.max_lat_accel_diff - calib_valid = self.calib_valid #self.calibrator.calib_valid + calib_valid = self.calibrator.calib_valid if not self.lat_active: self.last_lat_inactive_t = self.t @@ -363,15 +364,12 @@ def main(): DEBUG = bool(int(os.getenv("DEBUG", "0"))) pm = messaging.PubMaster(['liveDelay']) - sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'controlsState', 'carControl'], poll='liveLocationKalman') + sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'controlsState', 'carControl'], poll='livePose') params = Params() CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - # TODO: remove me, lagd is in shadow mode on release - is_release = params.get_bool("IsReleaseBranch") - - lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['liveLocationKalman'].frequency, enabled=True) + lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency) if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None: lag, valid_blocks = initial_lag_params lag_learner.reset(lag, valid_blocks) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 5f40f19e..6db749b9 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -93,8 +93,9 @@ class CarKalman(KalmanFilter): dim_state = CarKalman.initial_x.shape[0] name = CarKalman.name - # vehicle models comes from The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars - # Model used is in 6.15 with formula from 6.198 + # Linearized single-track lateral dynamics, equations 7.211-7.213 + # Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars + # Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6 # globals global_vars = [sp.Symbol(name) for name in CarKalman.global_vars] @@ -160,19 +161,18 @@ class CarKalman(KalmanFilter): gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) - def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None): - dim_state = self.initial_x.shape[0] - dim_state_err = self.P_initial.shape[0] - x_init = self.initial_x - x_init[States.STEER_RATIO] = steer_ratio - x_init[States.STIFFNESS] = stiffness_factor - x_init[States.ANGLE_OFFSET] = angle_offset + def __init__(self, generated_dir): + dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0] + self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial, + dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog) - if P_initial is not None: - self.P_initial = P_initial - # init filter - self.filter = EKF_sym_pyx(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, - dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog) + def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear): + self.filter.set_global("mass", mass) + self.filter.set_global("rotational_inertia", rotational_inertia) + self.filter.set_global("center_to_front", center_to_front) + self.filter.set_global("center_to_rear", center_to_rear) + self.filter.set_global("stiffness_front", stiffness_front) + self.filter.set_global("stiffness_rear", stiffness_rear) if __name__ == "__main__": diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 16bebb0b..b166daed 100644 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -1,8 +1,10 @@ #!/usr/bin/env python3 import os -import math -import json import numpy as np +import capnp + +import json +import math import cereal.messaging as messaging from cereal import car, log @@ -10,14 +12,14 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.common.swaglog import cloudlog - MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits -ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) -ROLL_LOWERED_MAX = math.radians(8) -ROLL_STD_MAX = math.radians(1.5) +ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MIN, ROLL_MAX = np.radians(-10), np.radians(10) +ROLL_LOWERED_MAX = np.radians(8) +ROLL_STD_MAX = np.radians(1.5) LATERAL_ACC_SENSOR_THRESHOLD = 4.0 OFFSET_MAX = 10.0 OFFSET_LOWERED_MAX = 8.0 @@ -25,54 +27,82 @@ MIN_ACTIVE_SPEED = 1.0 LOW_ACTIVE_SPEED = 10.0 -class ParamsLearner: - def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None): - self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial) +class VehicleParamsLearner: + def __init__(self, CP: car.CarParams, steer_ratio: float, stiffness_factor: float, angle_offset: float, P_initial: np.ndarray | None = None): + self.kf = CarKalman(GENERATED_DIR) - self.kf.filter.set_global("mass", CP.mass) - self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia) - self.kf.filter.set_global("center_to_front", CP.centerToFront) - self.kf.filter.set_global("center_to_rear", CP.wheelbase - CP.centerToFront) - self.kf.filter.set_global("stiffness_front", CP.tireStiffnessFront) - self.kf.filter.set_global("stiffness_rear", CP.tireStiffnessRear) + self.x_initial = CarKalman.initial_x.copy() + self.x_initial[States.STEER_RATIO] = steer_ratio + self.x_initial[States.STIFFNESS] = stiffness_factor + self.x_initial[States.ANGLE_OFFSET] = angle_offset + self.P_initial = P_initial if P_initial is not None else CarKalman.P_initial - self.active = False + self.kf.set_globals( + mass=CP.mass, + rotational_inertia=CP.rotationalInertia, + center_to_front=CP.centerToFront, + center_to_rear=CP.wheelbase - CP.centerToFront, + stiffness_front=CP.tireStiffnessFront, + stiffness_rear=CP.tireStiffnessRear + ) - self.speed = 0.0 - self.yaw_rate = 0.0 - self.yaw_rate_std = 0.0 - self.roll = 0.0 - self.steering_angle = 0.0 + self.min_sr, self.max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio - def handle_log(self, t, which, msg): - if which == 'liveLocationKalman': - self.yaw_rate = msg.angularVelocityCalibrated.value[2] - self.yaw_rate_std = msg.angularVelocityCalibrated.std[2] + self.calibrator = PoseCalibrator() - localizer_roll = msg.orientationNED.value[0] - localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] + self.observed_speed = 0.0 + self.observed_yaw_rate = 0.0 + self.observed_roll = 0.0 + + self.avg_offset_valid = True + self.total_offset_valid = True + self.roll_valid = True + + self.reset(None) + + def reset(self, t: float | None): + self.kf.init_state(self.x_initial, covs=self.P_initial, filter_time=t) + + self.angle_offset, self.roll, self.active = np.degrees(self.x_initial[States.ANGLE_OFFSET].item()), 0.0, False + self.avg_angle_offset = self.angle_offset + + def handle_log(self, t: float, which: str, msg: capnp._DynamicStructReader): + if which == 'livePose': + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + + yaw_rate, yaw_rate_std = calibrated_pose.angular_velocity.z, calibrated_pose.angular_velocity.z_std + yaw_rate_valid = msg.angularVelocityDevice.valid + yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s + yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s + if not yaw_rate_valid: + # This is done to bound the yaw rate estimate when localizer values are invalid or calibrating + yaw_rate, yaw_rate_std = 0.0, np.radians(10.0) + self.observed_yaw_rate = yaw_rate + + localizer_roll, localizer_roll_std = device_pose.orientation.x, device_pose.orientation.x_std + localizer_roll_std = np.radians(1) if np.isnan(localizer_roll_std) else localizer_roll_std roll_valid = (localizer_roll_std < ROLL_STD_MAX) and (ROLL_MIN < localizer_roll < ROLL_MAX) and msg.sensorsOK if roll_valid: roll = localizer_roll # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? - #roll_std = 2 * localizer_roll_std - roll_std = 3 * localizer_roll_std + roll_std = 2 * localizer_roll_std else: # This is done to bound the road roll estimate when localizer values are invalid roll = 0.0 roll_std = np.radians(10.0) - self.roll = np.clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + self.observed_roll = np.clip(roll, self.observed_roll - ROLL_MAX_DELTA, self.observed_roll + ROLL_MAX_DELTA) if self.active: if msg.posenetOK: self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, - np.array([[-self.yaw_rate]]), - np.array([np.atleast_2d(self.yaw_rate_std**2)])) + np.array([[-self.observed_yaw_rate]]), + np.array([np.atleast_2d(yaw_rate_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ROAD_ROLL, - np.array([[self.roll]]), + np.array([[self.observed_roll]]), np.array([np.atleast_2d(roll_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) @@ -84,21 +114,83 @@ class ParamsLearner: self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) - elif which == 'carState': - self.steering_angle = msg.steeringAngleDeg - self.speed = msg.vEgo + elif which == 'liveCalibration': + self.calibrator.feed_live_calib(msg) - in_linear_region = abs(self.steering_angle) < 45 - self.active = self.speed > MIN_ACTIVE_SPEED and in_linear_region + elif which == 'carState': + steering_angle = msg.steeringAngleDeg + + in_linear_region = abs(steering_angle) < 45 + self.observed_speed = msg.vEgo + self.active = self.observed_speed > MIN_ACTIVE_SPEED and in_linear_region if self.active: - self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]])) - self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[np.radians(steering_angle)]])) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.observed_speed]])) if not self.active: # Reset time when stopped so uncertainty doesn't grow - self.kf.filter.set_filter_time(t) - self.kf.filter.reset_rewind() + self.kf.filter.set_filter_time(t) # type: ignore + self.kf.filter.reset_rewind() # type: ignore + + def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: + x = self.kf.x + P = np.sqrt(self.kf.P.diagonal()) + if not np.all(np.isfinite(x)): + cloudlog.error("NaN in liveParameters estimate. Resetting to default values") + self.reset(self.kf.t) + x = self.kf.x + + self.avg_angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item()), + self.avg_angle_offset - MAX_ANGLE_OFFSET_DELTA, self.avg_angle_offset + MAX_ANGLE_OFFSET_DELTA) + self.angle_offset = np.clip(np.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()), + self.angle_offset - MAX_ANGLE_OFFSET_DELTA, self.angle_offset + MAX_ANGLE_OFFSET_DELTA) + self.roll = np.clip(float(x[States.ROAD_ROLL].item()), self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + roll_std = float(P[States.ROAD_ROLL].item()) + if self.active and self.observed_speed > LOW_ACTIVE_SPEED: + # Account for the opposite signs of the yaw rates + # At low speeds, bumping into a curb can cause the yaw rate to be very high + sensors_valid = bool(abs(self.observed_speed * (x[States.YAW_RATE].item() + self.observed_yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) + else: + sensors_valid = True + self.avg_offset_valid = check_valid_with_hysteresis(self.avg_offset_valid, self.avg_angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) + self.total_offset_valid = check_valid_with_hysteresis(self.total_offset_valid, self.angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) + self.roll_valid = check_valid_with_hysteresis(self.roll_valid, self.roll, ROLL_MAX, ROLL_LOWERED_MAX) + + msg = messaging.new_message('liveParameters') + + msg.valid = valid + + liveParameters = msg.liveParameters + liveParameters.posenetValid = True + liveParameters.sensorValid = sensors_valid + liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) + liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) + liveParameters.roll = float(self.roll) + liveParameters.angleOffsetAverageDeg = float(self.avg_angle_offset) + liveParameters.angleOffsetDeg = float(self.angle_offset) + liveParameters.steerRatioValid = self.min_sr <= liveParameters.steerRatio <= self.max_sr + liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 + liveParameters.angleOffsetAverageValid = bool(self.avg_offset_valid) + liveParameters.angleOffsetValid = bool(self.total_offset_valid) + liveParameters.valid = all(( + liveParameters.angleOffsetAverageValid, + liveParameters.angleOffsetValid , + self.roll_valid, + roll_std < ROLL_STD_MAX, + liveParameters.stiffnessFactorValid, + liveParameters.steerRatioValid, + )) + liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) + liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) + liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) + liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item()) + if debug: + liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message() + liveParameters.debugFilterState.value = x.tolist() + liveParameters.debugFilterState.std = P.tolist() + + return msg def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: float, lowered_threshold: float): @@ -109,6 +201,67 @@ def check_valid_with_hysteresis(current_valid: bool, val: float, threshold: floa return current_valid +# TODO: Remove this function after few releases (added in 0.9.9) +def migrate_cached_vehicle_params_if_needed(params: Params): + last_parameters_data_old = params.get("LiveParameters") + last_parameters_data = params.get("LiveParametersV2") + if last_parameters_data_old is None or last_parameters_data is not None: + return + + try: + last_parameters_msg = messaging.new_message('liveParameters') + last_parameters_msg.liveParameters.valid = True + last_parameters_msg.liveParameters.steerRatio = last_parameters_data_old['steerRatio'] + last_parameters_msg.liveParameters.stiffnessFactor = last_parameters_data_old['stiffnessFactor'] + last_parameters_msg.liveParameters.angleOffsetAverageDeg = last_parameters_data_old['angleOffsetAverageDeg'] + params.put("LiveParametersV2", last_parameters_msg.to_bytes()) + except Exception as e: + cloudlog.error(f"Failed to perform parameter migration: {e}") + params.remove("LiveParameters") + + +def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: bool, debug: bool): + last_parameters_data = params.get("LiveParametersV2") + last_carparams_data = params.get("CarParamsPrevRoute") + + steer_ratio, stiffness_factor, angle_offset_deg, p_initial = CP.steerRatio, 1.0, 0.0, None + + retrieve_success = False + if last_parameters_data is not None and last_carparams_data is not None: + try: + with log.Event.from_bytes(last_parameters_data) as last_lp_msg, car.CarParams.from_bytes(last_carparams_data) as last_CP: + lp = last_lp_msg.liveParameters + # Check if car model matches + if last_CP.carFingerprint != CP.carFingerprint: + raise Exception("Car model mismatch") + + # Check if starting values are sane + min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio + steer_ratio_sane = min_sr <= lp.steerRatio <= max_sr + if not steer_ratio_sane: + raise Exception(f"Invalid starting values found {lp}") + + initial_filter_std = np.array(lp.debugFilterState.std) + if debug and len(initial_filter_std) != 0: + p_initial = np.diag(initial_filter_std) + + steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg + retrieve_success = True + except Exception as e: + cloudlog.error(f"Failed to retrieve initial values: {e}") + params.remove("LiveParametersV2") + + if not replay: + # When driving in wet conditions the stiffness can go down, and then be too low on the next drive + # Without a way to detect this we have to reset the stiffness every drive + stiffness_factor = 1.0 + + if not retrieve_success: + cloudlog.info("Parameter learner resetting to default values") + + return steer_ratio, stiffness_factor, angle_offset_deg, p_initial + + def main(): config_realtime_process([0, 1, 2, 3], 5) @@ -116,65 +269,18 @@ def main(): REPLAY = bool(int(os.getenv("REPLAY", "0"))) pm = messaging.PubMaster(['liveParameters']) - sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll='liveLocationKalman') + sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carState', 'liveLocationKalman'], poll='livePose') - params_reader = Params() - # wait for stats about the car to come in from controls - cloudlog.info("paramsd is waiting for CarParams") - CP = messaging.log_from_bytes(params_reader.get("CarParams", block=True), car.CarParams) - cloudlog.info("paramsd got CarParams") + params = Params() + CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) - min_sr, max_sr = 0.5 * CP.steerRatio, 2.0 * CP.steerRatio + migrate_cached_vehicle_params_if_needed(params) - params = params_reader.get("LiveParameters") + steer_ratio, stiffness_factor, angle_offset_deg, pInitial = retrieve_initial_vehicle_params(params, CP, REPLAY, DEBUG) + learner = VehicleParamsLearner(CP, steer_ratio, stiffness_factor, np.radians(angle_offset_deg), pInitial) - # Check if car model matches - if params is not None: - params = json.loads(params) - if params.get('carFingerprint', None) != CP.carFingerprint: - cloudlog.info("Parameter learner found parameters for wrong car.") - params = None - - # Check if starting values are sane - if params is not None: - try: - steer_ratio_sane = min_sr <= params['steerRatio'] <= max_sr - if not steer_ratio_sane: - cloudlog.info(f"Invalid starting values found {params}") - params = None - except Exception as e: - cloudlog.info(f"Error reading params {params}: {str(e)}") - params = None - - # TODO: cache the params with the capnp struct - if params is None: - params = { - 'carFingerprint': CP.carFingerprint, - 'steerRatio': CP.steerRatio, - 'stiffnessFactor': 1.0, - 'angleOffsetAverageDeg': 0.0, - } - cloudlog.info("Parameter learner resetting to default values") - - if not REPLAY: - # When driving in wet conditions the stiffness can go down, and then be too low on the next drive - # Without a way to detect this we have to reset the stiffness every drive - params['stiffnessFactor'] = 1.0 - - pInitial = None - if DEBUG: - pInitial = np.array(params['debugFilterState']['std']) if 'debugFilterState' in params else None - - learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']), pInitial) - angle_offset_average = params['angleOffsetAverageDeg'] - angle_offset = angle_offset_average - roll = 0.0 - avg_offset_valid = True - total_offset_valid = True - roll_valid = True params_memory = Params("/dev/shm/params") params_memory.remove("LastGPSPosition") - while True: sm.update() if sm.all_checks(): @@ -191,72 +297,15 @@ def main(): lon = location.positionGeodetic.value[1] params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing})) - x = learner.kf.x - P = np.sqrt(learner.kf.P.diagonal()) - if not all(map(math.isfinite, x)): - cloudlog.error("NaN in liveParameters estimate. Resetting to default values") - learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0) - x = learner.kf.x - angle_offset_average = np.clip(math.degrees(x[States.ANGLE_OFFSET].item()), - angle_offset_average - MAX_ANGLE_OFFSET_DELTA, angle_offset_average + MAX_ANGLE_OFFSET_DELTA) - angle_offset = np.clip(math.degrees(x[States.ANGLE_OFFSET].item() + x[States.ANGLE_OFFSET_FAST].item()), - angle_offset - MAX_ANGLE_OFFSET_DELTA, angle_offset + MAX_ANGLE_OFFSET_DELTA) - roll = np.clip(float(x[States.ROAD_ROLL].item()), roll - ROLL_MAX_DELTA, roll + ROLL_MAX_DELTA) - roll_std = float(P[States.ROAD_ROLL].item()) - if learner.active and learner.speed > LOW_ACTIVE_SPEED: - # Account for the opposite signs of the yaw rates - # At low speeds, bumping into a curb can cause the yaw rate to be very high - sensors_valid = bool(abs(learner.speed * (x[States.YAW_RATE].item() + learner.yaw_rate)) < LATERAL_ACC_SENSOR_THRESHOLD) - else: - sensors_valid = True - avg_offset_valid = check_valid_with_hysteresis(avg_offset_valid, angle_offset_average, OFFSET_MAX, OFFSET_LOWERED_MAX) - total_offset_valid = check_valid_with_hysteresis(total_offset_valid, angle_offset, OFFSET_MAX, OFFSET_LOWERED_MAX) - roll_valid = check_valid_with_hysteresis(roll_valid, roll, ROLL_MAX, ROLL_LOWERED_MAX) - - msg = messaging.new_message('liveParameters') - - liveParameters = msg.liveParameters - liveParameters.posenetValid = True - liveParameters.sensorValid = sensors_valid - liveParameters.steerRatio = float(x[States.STEER_RATIO].item()) - liveParameters.steerRatioValid = min_sr <= liveParameters.steerRatio <= max_sr - liveParameters.stiffnessFactor = float(x[States.STIFFNESS].item()) - liveParameters.stiffnessFactorValid = 0.2 <= liveParameters.stiffnessFactor <= 5.0 - liveParameters.roll = float(roll) - liveParameters.angleOffsetAverageDeg = float(angle_offset_average) - liveParameters.angleOffsetAverageValid = bool(avg_offset_valid) - liveParameters.angleOffsetDeg = float(angle_offset) - liveParameters.angleOffsetValid = bool(total_offset_valid) - liveParameters.valid = all(( - liveParameters.angleOffsetAverageValid, - liveParameters.angleOffsetValid , - roll_valid, - roll_std < ROLL_STD_MAX, - liveParameters.stiffnessFactorValid, - liveParameters.steerRatioValid, - )) - liveParameters.steerRatioStd = float(P[States.STEER_RATIO].item()) - liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS].item()) - liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET].item()) - liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST].item()) - if DEBUG: - liveParameters.debugFilterState = log.LiveParametersData.FilterState.new_message() - liveParameters.debugFilterState.value = x.tolist() - liveParameters.debugFilterState.std = P.tolist() - - msg.valid = sm.all_checks() + if sm.updated['livePose']: + msg = learner.get_msg(sm.all_checks(), debug=DEBUG) + msg_dat = msg.to_bytes() if sm.frame % 1200 == 0: # once a minute - params = { - 'carFingerprint': CP.carFingerprint, - 'steerRatio': liveParameters.steerRatio, - 'stiffnessFactor': liveParameters.stiffnessFactor, - 'angleOffsetAverageDeg': liveParameters.angleOffsetAverageDeg, - } - params_reader.put_nonblocking("LiveParameters", json.dumps(params)) + params.put_nonblocking("LiveParametersV2", msg_dat) - pm.send('liveParameters', msg) + pm.send('liveParameters', msg_dat) if __name__ == "__main__": diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index d4b4e699..d86127de 100644 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os import numpy as np from collections import deque, defaultdict @@ -9,7 +10,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.swaglog import cloudlog -from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator +from openpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose HISTORY = 5 # secs POINTS_PER_BUCKET = 1500 @@ -32,7 +33,7 @@ MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100]) MIN_ENGAGE_BUFFER = 2 # secs VERSION = 1 # bump this to invalidate old parameter caches -ALLOWED_CARS = ['toyota', 'hyundai', 'rivian'] +ALLOWED_CARS = ['toyota', 'hyundai', 'rivian', 'honda'] def slope2rot(slope): @@ -77,6 +78,8 @@ class TorqueEstimator(ParameterEstimator): self.offline_friction = CP.lateralTuning.torque.friction self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor + self.calibrator = PoseCalibrator() + self.reset() initial_params = { @@ -171,12 +174,19 @@ class TorqueEstimator(ParameterEstimator): # TODO: check if high aEgo affects resulting lateral accel self.raw_points["vego"].append(msg.vEgo) self.raw_points["steer_override"].append(msg.steeringPressed) + elif which == "liveCalibration": + self.calibrator.feed_live_calib(msg) elif which == "liveDelay": self.lag = msg.lateralDelay - elif which == "liveLocationKalman": + # calculate lateral accel from past steering torque + elif which == "livePose": if len(self.raw_points['steer_torque']) == self.hist_len: - yaw_rate = msg.angularVelocityCalibrated.value[2] - roll = msg.orientationNED.value[0] + device_pose = Pose.from_live_pose(msg) + calibrated_pose = self.calibrator.build_calibrated_pose(device_pose) + angular_velocity_calibrated = calibrated_pose.angular_velocity + + yaw_rate = angular_velocity_calibrated.yaw + roll = device_pose.orientation.roll # check lat active up to now (without lag compensation) lat_active = np.interp(np.arange(t - MIN_ENGAGE_BUFFER, t + self.lag, DT_MDL), self.raw_points['carControl_t'], self.raw_points['lat_active']).astype(bool) @@ -234,7 +244,7 @@ def main(demo=False): config_realtime_process([0, 1, 2, 3], 5) pm = messaging.PubMaster(['liveTorqueParameters']) - sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveDelay', 'liveLocationKalman'], poll='liveLocationKalman') + sm = messaging.SubMaster(['carControl', 'carOutput', 'carState', 'liveCalibration', 'livePose', 'liveDelay'], poll='livePose') params = Params() estimator = TorqueEstimator(messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)) @@ -247,7 +257,7 @@ def main(demo=False): t = sm.logMonoTime[which] * 1e-9 estimator.handle_log(t, which, sm[which]) - # 4Hz driven by liveLocationKalman + # 4Hz driven by livePose if sm.frame % 5 == 0: pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks())) diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 0ff45c73..91866328 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -311,7 +311,7 @@ def main(demo=False): frame += 1 if frame % 100 == 0: custom_lat_delay = params.get_float("SteerActuatorDelay") * 0.01 - #lat_smooth_seconds = params.get_float("SteerSmoothSec") * 0.01 + lat_smooth_seconds = params.get_float("LatSmoothSec") * 0.01 long_delay = params.get_float("LongActuatorDelay")*0.01 vEgoStopping = params.get_float("VEgoStopping") * 0.01 @@ -357,7 +357,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - #lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS + #lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] @@ -425,6 +425,7 @@ def main(demo=False): modelv2_send.modelV2.meta.distanceToRoadEdgeRight = float(DH.distance_to_road_edge_right) modelv2_send.modelV2.meta.desire = DH.desire modelv2_send.modelV2.meta.laneChangeProb = DH.lane_change_ll_prob + modelv2_send.modelV2.meta.modelTurnSpeed = float(DH.model_turn_speed) fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen) pm.send('modelV2', modelv2_send) diff --git a/selfdrive/modeld/models/dmonitoring_model.onnx b/selfdrive/modeld/models/dmonitoring_model.onnx index b891902e..da60e9a9 100644 Binary files a/selfdrive/modeld/models/dmonitoring_model.onnx and b/selfdrive/modeld/models/dmonitoring_model.onnx differ diff --git a/selfdrive/modeld/models/driving_policy.onnx b/selfdrive/modeld/models/driving_policy.onnx index 5a7fa5b6..56fe9d10 100644 Binary files a/selfdrive/modeld/models/driving_policy.onnx and b/selfdrive/modeld/models/driving_policy.onnx differ diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 76e71eef..d2244c1a 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -16,6 +16,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.common.gps import get_gps_location_service from openpilot.selfdrive.car.car_specific import CarSpecificEvents +from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.selfdrived.events import Events, ET from openpilot.selfdrive.selfdrived.state import StateMachine from openpilot.selfdrive.selfdrived.alertmanager import AlertManager, set_offroad_alert @@ -57,6 +58,8 @@ class SelfdriveD: self.car_events = CarSpecificEvents(self.CP) self.disengage_on_accelerator = not (self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS) + self.pose_calibrator = PoseCalibrator() + self.calibrated_pose: Pose | None = None # Setup sockets self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents']) @@ -82,7 +85,7 @@ class SelfdriveD: # no vipc in replay will make them ignored anyways ignore += ['roadCameraState', 'wideRoadCameraState'] self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration', - 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'liveLocationKalman', # 'liveDelay', + 'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay', 'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters', 'carrotMan', 'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \ @@ -239,6 +242,12 @@ class SelfdriveD: if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture: self.events.add(EventName.ldw) + if self.sm.updated['liveCalibration']: + self.pose_calibrator.feed_live_calib(self.sm['liveCalibration']) + if self.sm.updated['livePose']: + device_pose = Pose.from_live_pose(self.sm['livePose']) + self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose) + if self.sm.alive['carrotMan']: atc_type = self.sm['carrotMan'].atcType if atc_type != self.atc_type_last: @@ -340,11 +349,9 @@ class SelfdriveD: self.logged_comm_issue = None if not self.CP.notCar: - if not self.sm['liveLocationKalman'].posenetOK: + if not self.sm['livePose'].posenetOK: self.events.add(EventName.posenetInvalid) - if not self.sm['liveLocationKalman'].deviceStable: - self.events.add(EventName.deviceFalling) - if not self.sm['liveLocationKalman'].inputsOK: + if not self.sm['livePose'].inputsOK: self.events.add(EventName.locationdTemporaryError) if not self.sm['liveParameters'].valid and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) @@ -387,7 +394,7 @@ class SelfdriveD: if not SIMULATION or REPLAY: # Not show in first 1.5 km to allow for driving out of garage. This event shows after 5 minutes gps_ok = self.sm.recv_frame[self.gps_location_service] > 0 and (self.sm.frame - self.sm.recv_frame[self.gps_location_service]) * DT_CTRL < 2.0 - if not gps_ok and self.sm['liveLocationKalman'].inputsOK and (self.distance_traveled > 1500): + if not gps_ok and self.sm['livePose'].inputsOK and (self.distance_traveled > 1500): if self.distance_traveled < 1600: self.events.add(EventName.noGps) #if gps_ok: diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index ad7337cb..b87c3e72 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -675,6 +675,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { cruiseToggles->addItem(new CValueControl("MyDrivingMode", tr("DRIVEMODE: Select"), tr("1:ECO,2:SAFE,3:NORMAL,4:HIGH"), 1, 4, 1)); cruiseToggles->addItem(new CValueControl("MyDrivingModeAuto", tr("DRIVEMODE: Auto"), tr("NORMAL mode only"), 0, 1, 1)); cruiseToggles->addItem(new CValueControl("TrafficLightDetectMode", tr("TrafficLight DetectMode"), tr("0:None, 1:Stopping only, 2: Stop & Go"), 0, 2, 1)); + cruiseToggles->addItem(new CValueControl("AChangeCostStarting", tr("AChangeCostStarting"), "", 0, 200, 10)); + cruiseToggles->addItem(new CValueControl("TrafficStopDistanceAdjust", tr("TrafficStopDistanceAdjust"), "", -600, 600, 50)); //cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", 5, 50, 1)); //cruiseToggles->addItem(new CValueControl("AutoResumeFromGas", "GAS CRUISE ON: Use", "Auto Cruise on when GAS pedal released, 60% Gas Cruise On automatically", 0, 3, 1)); //cruiseToggles->addItem(new CValueControl("AutoResumeFromGasSpeed", "GAS CRUISE ON: Speed(30)", "Driving speed exceeds the set value, Cruise ON", 20, 140, 5)); @@ -695,6 +697,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { latLongToggles->addItem(new CValueControl("SteerRatioRate", tr("LAT: SteerRatioRatex0.01(100)"), tr("SteerRatio apply rate"), 30, 170, 1)); latLongToggles->addItem(new CValueControl("PathOffset", tr("LAT: PathOffset"), tr("(-)left, (+)right"), -150, 150, 1)); latLongToggles->addItem(new CValueControl("SteerActuatorDelay", tr("LAT:SteerActuatorDelay(30)"), tr("x0.01, 0:LiveDelay"), 0, 100, 1)); + latLongToggles->addItem(new CValueControl("LatSmoothSec", tr("LAT:LatSmoothSec(13)"), tr("x0.01"), 1, 30, 1)); latLongToggles->addItem(new CValueControl("LateralTorqueCustom", tr("LAT: TorqueCustom(0)"), "", 0, 2, 1)); latLongToggles->addItem(new CValueControl("LateralTorqueAccelFactor", tr("LAT: TorqueAccelFactor(2500)"), "", 1000, 6000, 10)); latLongToggles->addItem(new CValueControl("LateralTorqueFriction", tr("LAT: TorqueFriction(100)"), "", 0, 1000, 10)); @@ -834,6 +837,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { startToggles->addItem(new CValueControl("HotspotOnBoot", tr("Hotspot enabled on boot"), "", 0, 1, 1)); startToggles->addItem(new CValueControl("SoftwareMenu", tr("Enable Software Menu"), "", 0, 1, 1)); startToggles->addItem(new CValueControl("IsLdwsCar", tr("IsLdwsCar"), "", 0, 1, 1)); + startToggles->addItem(new CValueControl("HardwareC3xLite", tr("Hardware is C3x Lite"), "", 0, 1, 1)); //startToggles->addItem(new CValueControl("CarrotCountDownSpeed", "NaviCountDown Speed(10)", "", 0, 200, 5)); //startToggles->addItem(new ParamControl("NoLogging", "Disable Logger", "", this)); @@ -855,6 +859,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) { speedToggles->addItem(new CValueControl("AutoNaviCountDownMode", tr("NaviCountDown mode(2)"), tr("0: off, 1:tbt+camera, 2:tbt+camera+bump"), 0, 2, 1)); speedToggles->addItem(new CValueControl("TurnSpeedControlMode", tr("Turn Speed control mode(1)"), tr("0: off, 1:vision, 2:vision+route, 3: route"), 0, 3, 1)); speedToggles->addItem(new CValueControl("MapTurnSpeedFactor", tr("Map TurnSpeed Factor(100)"), "", 50, 300, 5)); + speedToggles->addItem(new CValueControl("ModelTurnSpeedFactor", tr("Model TurnSpeed Factor(0)"), "", 0, 80, 10)); speedToggles->addItem(new CValueControl("AutoTurnControl", tr("ATC: Auto turn control(0)"), tr("0:None, 1: lane change, 2: lane change + speed, 3: speed"), 0, 3, 1)); speedToggles->addItem(new CValueControl("AutoTurnControlSpeedTurn", tr("ATC: Turn Speed (20)"), tr("0:None, turn speed"), 0, 100, 5)); speedToggles->addItem(new CValueControl("AutoTurnControlTurnEnd", tr("ATC: Turn CtrlDistTime (6)"), tr("dist=speed*time"), 0, 30, 1)); diff --git a/system/athena/registration.py b/system/athena/registration.py index ca11e001..d472ef8e 100755 --- a/system/athena/registration.py +++ b/system/athena/registration.py @@ -16,6 +16,10 @@ from openpilot.common.swaglog import cloudlog UNREGISTERED_DONGLE_ID = "UnregisteredDevice" +DUMMY_IMEI1 = '865420071781912' +DUMMY_IMEI2 = '865420071781904' + + def is_registered_device() -> bool: dongle = Params().get("DongleId", encoding='utf-8') return dongle not in (None, UNREGISTERED_DONGLE_ID) @@ -60,6 +64,7 @@ def register(show_spinner=False) -> str | None: start_time = time.monotonic() imei1: str | None = None imei2: str | None = None + while imei1 is None and imei2 is None: try: imei1, imei2 = HARDWARE.get_imei(0), HARDWARE.get_imei(1) @@ -67,8 +72,11 @@ def register(show_spinner=False) -> str | None: cloudlog.exception("Error getting imei, trying again...") time.sleep(1) - if time.monotonic() - start_time > 60 and show_spinner: + if time.monotonic() - start_time > 30 and show_spinner: spinner.update(f"registering device - serial: {serial}, IMEI: ({imei1}, {imei2})") + imei1 = DUMMY_IMEI1 + imei2 = DUMMY_IMEI2 + break backoff = 0 start_time = time.monotonic() diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 3a9692ef..ed5a2c5f 100755 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -172,7 +172,7 @@ def update_restart_condition(current_time, restart_triggered_ts, params, onroad_ if softRestartTriggered == 2: print("Parameter set to default") set_default_params() - + params.put_int("SoftRestartTriggered", 0) restart_triggered_ts = current_time return restart_triggered_ts @@ -337,20 +337,10 @@ def hardware_thread(end_event, hw_queue) -> None: set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", show_alert, extra_text=extra_text) # TODO: this should move to TICI.initialize_hardware, but we currently can't import params there - if TICI and HARDWARE.get_device_type() == "tici": + if False: #TICI and HARDWARE.get_device_type() == "tici": if not os.path.isfile("/persist/comma/living-in-the-moment"): if not Path("/data/media").is_mount(): set_offroad_alert_if_changed("Offroad_StorageMissing", True) - else: - # check for bad NVMe - try: - with open("/sys/block/nvme0n1/device/model") as f: - model = f.read().strip() - if not model.startswith("Samsung SSD 980") and params.get("Offroad_BadNvme") is None: - set_offroad_alert_if_changed("Offroad_BadNvme", True) - cloudlog.event("Unsupported NVMe", model=model, error=True) - except Exception: - pass # Handle offroad/onroad transition should_start = all(onroad_conditions.values()) diff --git a/system/manager/manager.py b/system/manager/manager.py index 39e3be2f..adf7d9ca 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -36,7 +36,7 @@ def get_default_params(): ("SearchInput", "0"), ("GMapKey", "0"), - ("MapboxStyle", "0"), + ("MapboxStyle", "0"), ("LongitudinalPersonalityMax", "3"), @@ -87,6 +87,7 @@ def get_default_params(): ("AutoNaviCountDownMode", "2"), ("TurnSpeedControlMode", "1"), ("MapTurnSpeedFactor", "90"), + ("ModelTurnSpeedFactor", "0"), ("StoppingAccel", "0"), ("StopDistanceCarrot", "550"), ("JLeadFactor3", "0"), @@ -134,6 +135,8 @@ def get_default_params(): ("TFollowGap3", "140"), ("TFollowGap4", "160"), ("DynamicTFollow", "0"), + ("AChangeCostStarting", "10"), + ("TrafficStopDistanceAdjust", "400"), ("DynamicTFollowLC", "100"), ("HapticFeedbackWhenSpeedCamera", "0"), ("UseLaneLineSpeed", "0"), @@ -164,6 +167,7 @@ def get_default_params(): ("CustomSteerDeltaDownLC", "0"), ("SpeedFromPCM", "2"), ("SteerActuatorDelay", "0"), + ("LatSmoothSec", "13"), ("MaxTimeOffroadMin", "60"), ("DisableDM", "0"), ("EnableConnect", "0"), @@ -298,6 +302,10 @@ def manager_thread() -> None: ignore.append("pandad") ignore += [x for x in os.getenv("BLOCK", "").split(",") if len(x) > 0] + if params.get("HardwareC3xLite"): + ignore += ["micd", "soundd", "loggerd"] + params.put("RecordAudio", "0") + sm = messaging.SubMaster(['deviceState', 'carParams'], poll='deviceState') pm = messaging.PubMaster(['managerState']) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index cf07e123..58e62744 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -75,6 +75,9 @@ def enable_dm(started, params, CP: car.CarParams) -> bool: def enable_connect(started, params, CP: car.CarParams) -> bool: return params.get_int("EnableConnect") > 0 +def c3x_lite(started: bool, params: Params, CP: car.CarParams) -> bool: + return started and params.get_bool("HardwareC3xLite") + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -100,8 +103,8 @@ procs = [ NativeProcess("sensord", "system/sensord", ["./sensord"], only_onroad, enabled=not PC), NativeProcess("ui", "selfdrive/ui", ["./ui"], always_run, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), - NativeProcess("locationd", "selfdrive/locationd", ["./locationd"], only_onroad), - #PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), + NativeProcess("locationd2", "selfdrive/locationd", ["./locationd"], only_onroad), + PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), @@ -136,6 +139,9 @@ procs = [ #PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", check_fleet, enabled=not PC), PythonProcess("fleet_manager", "selfdrive.frogpilot.fleetmanager.fleet_manager", check_fleet), PythonProcess("carrot_man", "selfdrive.carrot.carrot_man", always_run),#, enabled=not PC), + + # c3x lite + PythonProcess("beep", "selfdrive.controls.beep", c3x_lite, enabled=TICI), ] managed_processes = {p.name: p for p in procs}