diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 35299434a9..78645a46a9 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -56,7 +56,7 @@ struct __attribute__((packed)) timestamp_t { }; libusb_context *ctx = NULL; -libusb_device_handle *dev_handle; +libusb_device_handle *dev_handle = NULL; pthread_mutex_t usb_lock; bool spoofing_started = false; @@ -260,6 +260,7 @@ fail: return false; } +// must be called before threads or with mutex void usb_retry_connect() { LOG("attempting to connect"); while (!usb_connect()) { usleep(100*1000); } @@ -358,15 +359,32 @@ void can_health(PubSocket *publisher) { uint8_t power_save_enabled; } health; + // create message + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto healthData = event.initHealth(); + + bool received = false; + // recv from board - pthread_mutex_lock(&usb_lock); - do { + if (dev_handle != NULL) { + pthread_mutex_lock(&usb_lock); cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT); - if (cnt != sizeof(health)) { - handle_usb_issue(cnt, __func__); - } - } while(cnt != sizeof(health)); - pthread_mutex_unlock(&usb_lock); + pthread_mutex_unlock(&usb_lock); + + received = (cnt == sizeof(health)); + } + + // No panda connected, send empty health packet + if (!received){ + healthData.setHwType(cereal::HealthData::HwType::UNKNOWN); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + publisher->send((char*)bytes.begin(), bytes.size()); + return; + } if (spoofing_started) { health.ignition_line = 1; @@ -476,12 +494,6 @@ void can_health(PubSocket *publisher) { ignition_last = ignition; - // create message - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto healthData = event.initHealth(); - // set fields healthData.setUptime(health.uptime); healthData.setVoltage(health.voltage); @@ -507,11 +519,9 @@ void can_health(PubSocket *publisher) { auto bytes = words.asBytes(); publisher->send((char*)bytes.begin(), bytes.size()); - pthread_mutex_lock(&usb_lock); - // send heartbeat back to panda + pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); } @@ -796,7 +806,7 @@ void pigeon_init() { usleep(100*1000); // init from ubloxd - // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py + // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F"); pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2"); pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35"); @@ -905,16 +915,17 @@ int main() { assert(err == 0); libusb_set_debug(ctx, 3); - // connect to the board - usb_retry_connect(); - - - // create threads pthread_t can_health_thread_handle; err = pthread_create(&can_health_thread_handle, NULL, can_health_thread, NULL); assert(err == 0); + // connect to the board + pthread_mutex_lock(&usb_lock); + usb_retry_connect(); + pthread_mutex_unlock(&usb_lock); + + // create threads pthread_t can_send_thread_handle; err = pthread_create(&can_send_thread_handle, NULL, can_send_thread, NULL); diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index 376ec0704d..468e767f65 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -76,7 +76,8 @@ class PowerMonitoring: # Only integrate when there is no ignition # If health is None, we're probably not in a car, so we don't care - if health is None or (health.health.ignitionLine or health.health.ignitionCan): + if health is None or (health.health.ignitionLine or health.health.ignitionCan) or \ + health.health.hwType == log.HealthData.HwType.unknown: with self.integration_lock: self.last_measurement_time = None self.next_pulsed_measurement_time = None diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 3f5490237a..be60ae41a1 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -197,13 +197,16 @@ def thermald_thread(): location = location.gpsLocation if location else None msg = read_thermal() - # clear car params when panda gets disconnected - if health is None and health_prev is not None: - params.panda_disconnect() - health_prev = health - if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client + ignition = health.health.ignitionLine or health.health.ignitionCan + + # Handle disconnect + if health_prev is not None: + if health.health.hwType == log.HealthData.HwType.unknown and \ + health_prev.health.hwType != log.HealthData.HwType.unknown: + params.panda_disconnect() + health_prev = health # get_network_type is an expensive call. update every 10s if (count % int(10. / DT_TRML)) == 0: @@ -305,9 +308,6 @@ def thermald_thread(): params.delete("Offroad_ConnectivityNeeded") params.delete("Offroad_ConnectivityNeededPrompt") - # start constellation of processes when the car starts - ignition = health is not None and (health.health.ignitionLine or health.health.ignitionCan) - do_uninstall = params.get("DoUninstall") == b"1" accepted_terms = params.get("HasAcceptedTerms") == terms_version completed_training = params.get("CompletedTrainingVersion") == training_version