spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

use proper timeout on frontFrame to turn off IR leds

pull/1026/head
Comma Device 2020-01-30 15:52:27 -08:00
parent a75891f099
commit 58262bac9d
1 changed files with 17 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
selfdrive/boardd/boardd.cc
View File

@ -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);
prev_ir_pwr = ir_pwr;
}
}
}
// 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;
}
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;