diff --git a/cereal b/cereal index ce5ef7a1e5..2881d47cea 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit ce5ef7a1e5d16b3f6c91f4ac6ca20cba9f4a255c +Subproject commit 2881d47cea8b9ee0ba972f1515b13c13af0c0c57 diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 7487109832..912096f105 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -40,6 +40,7 @@ std::atomic ignition(false); ExitHandler do_exit; + std::string get_time_str(const struct tm &time) { char s[30] = {'\0'}; std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time); @@ -143,7 +144,7 @@ Panda *usb_connect() { // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton #ifndef __x86_64__ static std::once_flag connected_once; - std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PandaState::UsbPowerMode::CDP); + std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PeripheralState::UsbPowerMode::CDP); #endif if (panda->has_rtc) { @@ -234,6 +235,13 @@ void can_recv_thread(Panda *panda) { } } +void send_empty_peripheral_state(PubMaster *pm) { + MessageBuilder msg; + auto peripheralState = msg.initEvent().initPeripheralState(); + peripheralState.setPandaType(cereal::PandaState::PandaType::UNKNOWN); + pm->send("peripheralState", msg); +} + void send_empty_panda_state(PubMaster *pm) { MessageBuilder msg; auto pandaState = msg.initEvent().initPandaState(); @@ -241,44 +249,113 @@ void send_empty_panda_state(PubMaster *pm) { pm->send("pandaState", msg); } -void panda_state_thread(PubMaster *pm, Panda *panda, bool spoofing_started) { - LOGD("start panda state thread"); - uint32_t no_ignition_cnt = 0; - bool ignition_last = false; - Params params = Params(); +bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) { + health_t pandaState = panda->get_state(); - // run at 2hz - while (!do_exit && panda->connected) { - health_t pandaState = panda->get_state(); + if (spoofing_started) { + pandaState.ignition_line = 1; + } - if (spoofing_started) { - pandaState.ignition_line = 1; - } + // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node + if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } - // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node - if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } + bool ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); - ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); +#ifndef __x86_64__ + bool power_save_desired = !ignition; + if (pandaState.power_save_enabled != power_save_desired) { + panda->set_power_saving(power_save_desired); + } - if (ignition) { - no_ignition_cnt = 0; - } else { - no_ignition_cnt += 1; - } + // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect + if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } +#endif -#ifndef __x86_64__ - bool power_save_desired = !ignition; - if (pandaState.power_save_enabled != power_save_desired) { - panda->set_power_saving(power_save_desired); + // build msg + MessageBuilder msg; + auto evt = msg.initEvent(); + evt.setValid(panda->comms_healthy); + + auto ps = evt.initPandaState(); + ps.setUptime(pandaState.uptime); + ps.setIgnitionLine(pandaState.ignition_line); + ps.setIgnitionCan(pandaState.ignition_can); + ps.setControlsAllowed(pandaState.controls_allowed); + ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); + ps.setCanRxErrs(pandaState.can_rx_errs); + ps.setCanSendErrs(pandaState.can_send_errs); + ps.setCanFwdErrs(pandaState.can_fwd_errs); + ps.setGmlanSendErrs(pandaState.gmlan_send_errs); + ps.setPandaType(panda->hw_type); + ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); + ps.setSafetyParam(pandaState.safety_param); + ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); + ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); + ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); + + // Convert faults bitset to capnp list + std::bitset fault_bits(pandaState.faults); + auto faults = ps.initFaults(fault_bits.count()); + + size_t i = 0; + for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); + f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) { + if (fault_bits.test(f)) { + faults.set(i, cereal::PandaState::FaultType(f)); + i++; } + } + pm->send("pandaState", msg); + + return ignition; +} - // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect - if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); +void send_peripheral_state(PubMaster *pm, Panda *panda) { + health_t pandaState = panda->get_state(); + + // build msg + MessageBuilder msg; + auto evt = msg.initEvent(); + evt.setValid(panda->comms_healthy); + + auto ps = evt.initPeripheralState(); + ps.setPandaType(panda->hw_type); + + if (Hardware::TICI()) { + double read_time = millis_since_boot(); + ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str())); + ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str())); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); } -#endif + } else { + ps.setVoltage(pandaState.voltage); + ps.setCurrent(pandaState.current); + } + + uint16_t fan_speed_rpm = panda->get_fan_speed(); + ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode)); + ps.setFanSpeedRpm(fan_speed_rpm); + + pm->send("peripheralState", msg); +} + +void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, bool spoofing_started) { + Params params; + bool ignition_last = false; + + LOGD("start panda state thread"); + + // run at 2hz + while (!do_exit && panda->connected) { + send_peripheral_state(pm, peripheral_panda); + ignition = send_panda_state(pm, panda, spoofing_started); // clear VIN, CarParams, and set new safety on car start if (ignition && !ignition_last) { @@ -294,87 +371,16 @@ void panda_state_thread(PubMaster *pm, Panda *panda, bool spoofing_started) { params.clearAll(CLEAR_ON_IGNITION_OFF); } - // Write to rtc once per minute when no ignition present - if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)) { - // Write time to RTC if it looks reasonable - setenv("TZ","UTC",1); - struct tm sys_time = util::get_time(); - - if (util::time_valid(sys_time)) { - struct tm rtc_time = panda->get_rtc(); - double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); - - if (std::abs(seconds) > 1.1) { - panda->set_rtc(sys_time); - LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s", - seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); - } - } - } - ignition_last = ignition; - uint16_t fan_speed_rpm = panda->get_fan_speed(); - - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy); - - auto ps = evt.initPandaState(); - ps.setUptime(pandaState.uptime); - - if (Hardware::TICI()) { - double read_time = millis_since_boot(); - ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str())); - ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str())); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } - } else { - ps.setVoltage(pandaState.voltage); - ps.setCurrent(pandaState.current); - } - ps.setIgnitionLine(pandaState.ignition_line); - ps.setIgnitionCan(pandaState.ignition_can); - ps.setControlsAllowed(pandaState.controls_allowed); - ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); - ps.setHasGps(true); - ps.setCanRxErrs(pandaState.can_rx_errs); - ps.setCanSendErrs(pandaState.can_send_errs); - ps.setCanFwdErrs(pandaState.can_fwd_errs); - ps.setGmlanSendErrs(pandaState.gmlan_send_errs); - ps.setPandaType(panda->hw_type); - ps.setUsbPowerMode(cereal::PandaState::UsbPowerMode(pandaState.usb_power_mode)); - ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); - ps.setSafetyParam(pandaState.safety_param); - ps.setFanSpeedRpm(fan_speed_rpm); - ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); - ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); - ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); - - // Convert faults bitset to capnp list - std::bitset fault_bits(pandaState.faults); - auto faults = ps.initFaults(fault_bits.count()); - - size_t i = 0; - for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) { - if (fault_bits.test(f)) { - faults.set(i, cereal::PandaState::FaultType(f)); - i++; - } - } - pm->send("pandaState", msg); panda->send_heartbeat(); util::sleep_for(500); } } -void hardware_control_thread(Panda *panda) { - LOGD("start hardware control thread"); + +void peripheral_control_thread(Panda *panda) { + LOGD("start peripheral control thread"); SubMaster sm({"deviceState", "driverCameraState"}); uint64_t last_front_frame_t = 0; @@ -395,10 +401,10 @@ void hardware_control_thread(Panda *panda) { bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled(); if (charging_disabled != prev_charging_disabled) { if (charging_disabled) { - panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CLIENT); + panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CLIENT); LOGW("TURN OFF CHARGING!\n"); } else { - panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP); + panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CDP); LOGW("TURN ON CHARGING!\n"); } prev_charging_disabled = charging_disabled; @@ -444,6 +450,23 @@ void hardware_control_thread(Panda *panda) { prev_ir_pwr = ir_pwr; } + // Write to rtc once per minute when no ignition present + if ((panda->has_rtc) && !ignition && (cnt % 120 == 1)) { + // Write time to RTC if it looks reasonable + setenv("TZ","UTC",1); + struct tm sys_time = util::get_time(); + + if (util::time_valid(sys_time)) { + struct tm rtc_time = panda->get_rtc(); + double seconds = difftime(mktime(&rtc_time), mktime(&sys_time)); + + if (std::abs(seconds) > 1.1) { + panda->set_rtc(sys_time); + LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s", + seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str()); + } + } + } } } @@ -538,14 +561,16 @@ int main() { LOG("set affinity returns %d", err); LOGW("attempting to connect"); - PubMaster pm({"pandaState"}); + PubMaster pm({"pandaState", "peripheralState"}); while (!do_exit) { Panda *panda = usb_connect(); + Panda *peripheral_panda = panda; - // Send empty pandaState and try again - if (panda == nullptr) { + // Send empty pandaState & peripheralState and try again + if (panda == nullptr || peripheral_panda == nullptr) { send_empty_panda_state(&pm); + send_empty_peripheral_state(&pm); util::sleep_for(500); continue; } @@ -553,11 +578,12 @@ int main() { LOGW("connected to board"); std::vector threads; - threads.emplace_back(panda_state_thread, &pm, panda, getenv("STARTED") != nullptr); + threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, getenv("STARTED") != nullptr); + threads.emplace_back(peripheral_control_thread, peripheral_panda); + threads.emplace_back(pigeon_thread, peripheral_panda); + threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr); threads.emplace_back(can_recv_thread, panda); - threads.emplace_back(hardware_control_thread, panda); - threads.emplace_back(pigeon_thread, panda); for (auto &t : threads) t.join(); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 7c78c87685..1bd8d56f32 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -333,7 +333,7 @@ void Panda::set_power_saving(bool power_saving) { usb_write(0xe7, power_saving, 0); } -void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode) { +void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) { usb_write(0xe6, (uint16_t)power_mode, 0); } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index dd7600789d..971ec8b422 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -81,7 +81,7 @@ class Panda { std::optional> get_firmware_version(); std::optional get_serial(); void set_power_saving(bool power_saving); - void set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode); + void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode); void send_heartbeat(); void can_send(capnp::List::Reader can_data_list); int can_receive(kj::Array& out_buf); diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4df2f91926..273a94732e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -72,7 +72,7 @@ class Controls: self.sm = sm if self.sm is None: ignore = ['driverCameraState', 'managerState'] if SIMULATION else None - self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', + self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'peripheralState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman', 'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet, ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan']) @@ -207,8 +207,8 @@ class Controls: self.events.add(EventName.highCpuUsage) # Alert if fan isn't spinning for 5 seconds - if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]: - if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: + if self.sm['peripheralState'].pandaType in [PandaType.uno, PandaType.dos]: + if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50: if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0: self.events.add(EventName.fanMalfunction) else: diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 09847a4efd..1c72c0bcf4 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -37,10 +37,6 @@ "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.", "severity": 0 }, - "Offroad_HardwareUnsupported": { - "text": "White and grey panda are unsupported. Upgrade to comma two or black panda.", - "severity": 0 - }, "Offroad_UnofficialHardware": { "text": "Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, contact support@comma.ai.", "severity": 1 diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 8dd0a0c880..84c69d0cbe 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -204,7 +204,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert: - gps_integrated = sm['pandaState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] + gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos] return Alert( "Poor GPS reception", "If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement", diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index df68d77383..8ca799c3c2 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -240,7 +240,7 @@ CONFIGS = [ proc_name="controlsd", pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], - "deviceState": [], "pandaState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], + "deviceState": [], "pandaState": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], "modelV2": [], "driverCameraState": [], "roadCameraState": [], "ubloxRaw": [], "managerState": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index ccb7efde6e..c73efa406a 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -66,6 +66,7 @@ TIMINGS = { # rtols: max/min, rsd "can": [2.5, 0.35], "pandaState": [2.5, 0.35], + "peripheralState": [2.5, 0.35], "sendcan": [2.5, 0.35], "carState": [2.5, 0.35], "carControl": [2.5, 0.35], diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 70ee9fc0af..9c07363917 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -28,8 +28,8 @@ class PowerMonitoring: self.last_save_time = 0 # Used for saving current value in a param self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad self.next_pulsed_measurement_time = None - self.car_voltage_mV = 12e3 # Low-passed version of pandaState voltage - self.car_voltage_instant_mV = 12e3 # Last value of pandaState voltage + self.car_voltage_mV = 12e3 # Low-passed version of peripheralState voltage + self.car_voltage_instant_mV = 12e3 # Last value of peripheralState voltage self.integration_lock = threading.Lock() car_battery_capacity_uWh = self.params.get("CarBatteryCapacity") @@ -39,14 +39,13 @@ class PowerMonitoring: # Reset capacity if it's low self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh)) - # Calculation tick - def calculate(self, pandaState): + def calculate(self, peripheralState, ignition): try: now = sec_since_boot() - # If pandaState is None, we're probably not in a car, so we don't care - if pandaState is None or pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: + # If peripheralState is None, we're probably not in a car, so we don't care + if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None @@ -54,8 +53,8 @@ class PowerMonitoring: return # Low-pass battery voltage - self.car_voltage_instant_mV = pandaState.pandaState.voltage - self.car_voltage_mV = ((pandaState.pandaState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) + self.car_voltage_instant_mV = peripheralState.voltage + self.car_voltage_mV = ((peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K))) # Cap the car battery power and save it in a param every 10-ish seconds self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0) @@ -70,7 +69,7 @@ class PowerMonitoring: self.last_measurement_time = now return - if (pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan): + if ignition: # If there is ignition, we integrate the charging rate of the car with self.integration_lock: self.power_used_uWh = 0 @@ -81,7 +80,7 @@ class PowerMonitoring: self.last_measurement_time = now else: # No ignition, we integrate the offroad power used by the device - is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno + is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno # Get current power draw somehow current_power = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none if current_power is not None: @@ -158,8 +157,8 @@ class PowerMonitoring: return int(self.car_battery_capacity_uWh) # See if we need to disable charging - def should_disable_charging(self, pandaState, offroad_timestamp): - if pandaState is None or offroad_timestamp is None: + def should_disable_charging(self, ignition, in_car, offroad_timestamp): + if offroad_timestamp is None: return False now = sec_since_boot() @@ -167,24 +166,24 @@ class PowerMonitoring: disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3)) disable_charging |= (self.car_battery_capacity_uWh <= 0) - disable_charging &= (not pandaState.pandaState.ignitionLine and not pandaState.pandaState.ignitionCan) + disable_charging &= not ignition disable_charging &= (not self.params.get_bool("DisablePowerDown")) - disable_charging &= (pandaState.pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected) + disable_charging &= in_car disable_charging |= self.params.get_bool("ForcePowerDown") return disable_charging # See if we need to shutdown - def should_shutdown(self, pandaState, offroad_timestamp, started_seen): - if pandaState is None or offroad_timestamp is None: + def should_shutdown(self, peripheralState, ignition, in_car, offroad_timestamp, started_seen): + if offroad_timestamp is None: return False now = sec_since_boot() - panda_charging = (pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client) + panda_charging = (peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client) BATT_PERC_OFF = 10 should_shutdown = False # Wait until we have shut down charging before powering down - should_shutdown |= (not panda_charging and self.should_disable_charging(pandaState, offroad_timestamp)) + should_shutdown |= (not panda_charging and self.should_disable_charging(ignition, in_car, offroad_timestamp)) should_shutdown |= ((HARDWARE.get_battery_capacity() < BATT_PERC_OFF) and (not HARDWARE.get_battery_charging()) and ((now - offroad_timestamp) > 60)) should_shutdown &= started_seen or (now > MIN_ON_TIME_S) return should_shutdown diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py index 5b4989871b..0086715fc6 100755 --- a/selfdrive/thermald/tests/test_power_monitoring.py +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -37,20 +37,17 @@ class TestPowerMonitoring(unittest.TestCase): params.delete("CarBatteryCapacity") params.delete("DisablePowerDown") - def mock_pandaState(self, ignition, hw_type, car_voltage=12, harness_status=log.PandaState.HarnessStatus.normal): - pandaState = messaging.new_message('pandaState') - pandaState.pandaState.pandaType = hw_type - pandaState.pandaState.voltage = car_voltage * 1e3 - pandaState.pandaState.ignitionLine = ignition - pandaState.pandaState.ignitionCan = False - pandaState.pandaState.harnessStatus = harness_status - return pandaState + def mock_peripheralState(self, hw_type, car_voltage=12): + ps = messaging.new_message('peripheralState').peripheralState + ps.pandaType = hw_type + ps.voltage = car_voltage * 1e3 + return ps # Test to see that it doesn't do anything when pandaState is None def test_pandaState_present(self): pm = PowerMonitoring() for _ in range(10): - pm.calculate(None) + pm.calculate(None, None) self.assertEqual(pm.get_power_used(), 0) self.assertEqual(pm.get_car_battery_capacity(), (CAR_BATTERY_CAPACITY_uWh / 10)) @@ -59,7 +56,7 @@ class TestPowerMonitoring(unittest.TestCase): def test_offroad_ignition(self, hw_type): pm = PowerMonitoring() for _ in range(10): - pm.calculate(self.mock_pandaState(True, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), True) self.assertEqual(pm.get_power_used(), 0) # Test to see that it integrates with discharging battery @@ -71,7 +68,7 @@ class TestPowerMonitoring(unittest.TestCase): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm = PowerMonitoring() for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_pandaState(False, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), False) expected_power_usage = ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10) @@ -85,7 +82,7 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = 0 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_pandaState(True, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), True) expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) @@ -99,7 +96,7 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_pandaState(True, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), True) estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) @@ -113,7 +110,7 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_pandaState(False, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), False) expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10) @@ -127,7 +124,7 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = 1000 for _ in range(TEST_DURATION_S + 1): - pm.calculate(self.mock_pandaState(False, hw_type)) + pm.calculate(self.mock_peripheralState(hw_type), False) estimated_capacity = 0 - ((1/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6) self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10) @@ -143,12 +140,13 @@ class TestPowerMonitoring(unittest.TestCase): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh start_time = ssb - pandaState = self.mock_pandaState(False, hw_type) + ignition = False + peripheralState = self.mock_peripheralState(hw_type) while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME: - pm.calculate(pandaState) + pm.calculate(peripheralState, ignition) if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME: - self.assertFalse(pm.should_disable_charging(pandaState, start_time)) - self.assertTrue(pm.should_disable_charging(pandaState, start_time)) + self.assertFalse(pm.should_disable_charging(ignition, True, start_time)) + self.assertTrue(pm.should_disable_charging(ignition, True, start_time)) # Test to check policy of stopping charging when the car voltage is too low @parameterized.expand(ALL_PANDA_TYPES) @@ -160,12 +158,13 @@ class TestPowerMonitoring(unittest.TestCase): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - pandaState = self.mock_pandaState(False, hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + ignition = False + peripheralState = self.mock_peripheralState(hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(pandaState) + pm.calculate(peripheralState, ignition) if i % 10 == 0: - self.assertEqual(pm.should_disable_charging(pandaState, ssb), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3)) - self.assertTrue(pm.should_disable_charging(pandaState, ssb)) + self.assertEqual(pm.should_disable_charging(ignition, True, ssb), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3)) + self.assertTrue(pm.should_disable_charging(ignition, True, ssb)) # Test to check policy of not stopping charging when DisablePowerDown is set def test_disable_power_down(self): @@ -177,12 +176,13 @@ class TestPowerMonitoring(unittest.TestCase): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - pandaState = self.mock_pandaState(False, log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + ignition = False + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(pandaState) + pm.calculate(peripheralState, ignition) if i % 10 == 0: - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) + self.assertFalse(pm.should_disable_charging(ignition, True, ssb)) + self.assertFalse(pm.should_disable_charging(ignition, True, ssb)) # Test to check policy of not stopping charging when ignition def test_ignition(self): @@ -193,12 +193,13 @@ class TestPowerMonitoring(unittest.TestCase): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - pandaState = self.mock_pandaState(True, log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) + ignition = True + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(pandaState) + pm.calculate(peripheralState, ignition) if i % 10 == 0: - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) + self.assertFalse(pm.should_disable_charging(ignition, True, ssb)) + self.assertFalse(pm.should_disable_charging(ignition, True, ssb)) # Test to check policy of not stopping charging when harness is not connected def test_harness_connection(self): @@ -209,13 +210,14 @@ class TestPowerMonitoring(unittest.TestCase): pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None): pm = PowerMonitoring() pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - pandaState = self.mock_pandaState(False, log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1), - harness_status=log.PandaState.HarnessStatus.notConnected) + + ignition = False + peripheralState = self.mock_peripheralState(log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1)) for i in range(TEST_TIME): - pm.calculate(pandaState) + pm.calculate(peripheralState, ignition) if i % 10 == 0: - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) - self.assertFalse(pm.should_disable_charging(pandaState, ssb)) + self.assertFalse(pm.should_disable_charging(ignition, False,ssb)) + self.assertFalse(pm.should_disable_charging(ignition, False, ssb)) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 4a801d5f95..a70773a993 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -155,8 +155,7 @@ def thermald_thread(): pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout) - location_sock = messaging.sub_sock('gpsLocationExternal') - managerState_sock = messaging.sub_sock('managerState', conflate=True) + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState"]) fan_speed = 0 count = 0 @@ -184,6 +183,7 @@ def thermald_thread(): temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) pandaState_prev = None should_start_prev = False + in_car = False handle_fan = None is_uno = False ui_running_prev = False @@ -202,15 +202,19 @@ def thermald_thread(): if params.get_bool("IsOnroad"): params.put_bool("BootedOnroad", True) - while 1: + while True: pandaState = messaging.recv_sock(pandaState_sock, wait=True) + + sm.update(0) + peripheralState = sm['peripheralState'] + msg = read_thermal(thermal_config) if pandaState is not None: - usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client + pandaState = pandaState.pandaState # If we lose connection to the panda, wait 5 seconds before going offroad - if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown: + if pandaState.pandaType == log.PandaState.PandaType.unknown: no_panda_cnt += 1 if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML: if startup_conditions["ignition"]: @@ -218,15 +222,14 @@ def thermald_thread(): startup_conditions["ignition"] = False else: no_panda_cnt = 0 - startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan + startup_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan - startup_conditions["hardware_supported"] = pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda, - log.PandaState.PandaType.greyPanda] - set_offroad_alert_if_changed("Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"]) + in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected + usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client # Setup fan handler on first connect to panda - if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown: - is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno + if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown: + is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno if TICI: cloudlog.info("Setting up TICI fan handler") @@ -241,8 +244,8 @@ def thermald_thread(): # Handle disconnect if pandaState_prev is not None: - if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \ - pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown: + if pandaState.pandaType == log.PandaState.PandaType.unknown and \ + pandaState_prev.pandaType != log.PandaState.PandaType.unknown: params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT) pandaState_prev = pandaState @@ -399,27 +402,25 @@ def thermald_thread(): off_ts = sec_since_boot() # Offroad power monitoring - power_monitor.calculate(pandaState) + power_monitor.calculate(peripheralState, startup_conditions["ignition"]) msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used() msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity()) # Check if we need to disable charging (handled by boardd) - msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts) + msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts) # Check if we need to shut down - if power_monitor.should_shutdown(pandaState, off_ts, started_seen): + if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, off_ts, started_seen): cloudlog.info(f"shutting device down, offroad since {off_ts}") # TODO: add function for blocking cloudlog instead of sleep time.sleep(10) HARDWARE.shutdown() # If UI has crashed, set the brightness to reasonable non-zero value - manager_state = messaging.recv_one_or_none(managerState_sock) - if manager_state is not None: - ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running) - if ui_running_prev and not ui_running: - HARDWARE.set_screen_brightness(20) - ui_running_prev = ui_running + ui_running = "ui" in (p.name for p in sm["managerState"].processes if p.running) + if ui_running_prev and not ui_running: + HARDWARE.set_screen_brightness(20) + ui_running_prev = ui_running msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged msg.deviceState.started = started_ts is not None @@ -443,11 +444,11 @@ def thermald_thread(): if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40: cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent) - location = messaging.recv_sock(location_sock) cloudlog.event("STATUS_PACKET", count=count, pandaState=(strip_deprecated_keys(pandaState.to_dict()) if pandaState else None), - location=(strip_deprecated_keys(location.gpsLocationExternal.to_dict()) if location else None), + peripheralState=strip_deprecated_keys(peripheralState.to_dict()), + location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) count += 1