aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_UAVCAN: rearranged some logic to be more efficient

Browse Source 
rearranged some logic to be more efficient. Check array bounds and semiphore take status before doing any work that would otherwise been thrown away
This commit is contained in:
Tom Pittenger 2018-01-19 14:32:02 -08:00
parent 228058e089
commit f457dee568
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -1308,13 +1308,21 @@ void AP_UAVCAN::update_bi_state(uint8_t id)
}
bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue) {
if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) {
return false;
}
if (!led_out_sem_take()) {
return false;
}
uavcan::equipment::indication::RGB565 color;
color.red = (red >> 3);
color.green = (green >> 2);
color.blue = (blue >> 3);
if (!led_out_sem_take()) return false;
// check if device exists. if so, update here and return
for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
if (!_led_conf.devices[i].enabled || (_led_conf.devices[i].led_index == led_index)) {
_led_conf.devices[i].led_index = led_index;
@ -1326,11 +1334,7 @@ bool AP_UAVCAN::led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t
}
}
if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) {
led_out_sem_give();
return false;
}
// no existing device found, add it
_led_conf.devices[_led_conf.devices_count].led_index = led_index;
_led_conf.devices[_led_conf.devices_count].rgb565_color = color;
_led_conf.devices[_led_conf.devices_count].enabled = true;