Boardd should always send health, even with no panda (#1322)

* dont go offroad when health times out

* always send health packet

* Handle disconnect in thermal

* Handle unplug in power monitoring

* Small cleanup

* Remove copied code

* Add mutex

* Can just use infinite timeout. It will still return if not connected

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<cereal::Event>();
+  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<cereal::Event>();
-  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);

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

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