diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 4f1d108d7f..48e7dfe3c1 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -654,7 +654,9 @@ void *hardware_control_thread(void *crap) { if (hw_type != cereal::HealthData::HwType::UNO) return NULL; + uint64_t last_front_frame_t = 0; uint16_t prev_fan_speed = 999; + uint16_t ir_pwr = 0; uint16_t prev_ir_pwr = 999; unsigned int cnt = 0; @@ -684,7 +686,7 @@ void *hardware_control_thread(void *crap) { } } else if (type == cereal::Event::FRONT_FRAME){ float cur_front_gain = event.getFrontFrame().getGainFrac(); - uint16_t ir_pwr; + last_front_frame_t = event.getLogMonoTime(); if (cur_front_gain <= CUTOFF_GAIN) { ir_pwr = 100.0 * MIN_IR_POWER; @@ -693,20 +695,22 @@ void *hardware_control_thread(void *crap) { } else { ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_front_gain - CUTOFF_GAIN) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_GAIN - CUTOFF_GAIN))); } + } + } - if (!ignition_last){ - ir_pwr = 0; - } - - if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){ - pthread_mutex_lock(&usb_lock); - libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); + // Disable ir_pwr on front frame timeout + uint64_t cur_t = nanos_since_boot(); + if (cur_t - last_front_frame_t > 1e9){ + ir_pwr = 0; + } - prev_ir_pwr = ir_pwr; - } - } + if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){ + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + prev_ir_pwr = ir_pwr; } + } delete poller;