|
|
|
@ -292,28 +292,6 @@ void OreoLED_PX4::set_macro(uint8_t instance, oreoled_macro macro)
@@ -292,28 +292,6 @@ void OreoLED_PX4::set_macro(uint8_t instance, oreoled_macro macro)
|
|
|
|
|
_state_desired_semaphore = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// send_bytes - send bytes to one or all LEDs
|
|
|
|
|
void OreoLED_PX4::send_bytes(uint8_t instance, uint8_t num_bytes, uint8_t bytes[OREOLED_CMD_LENGTH_MAX]) |
|
|
|
|
{ |
|
|
|
|
// return immediately if no healthy leds
|
|
|
|
|
if (!_overall_health) { |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// set semaphore
|
|
|
|
|
_state_desired_semaphore = true; |
|
|
|
|
|
|
|
|
|
// send bytes to some or all leds
|
|
|
|
|
oreoled_cmd_t new_cmd; |
|
|
|
|
new_cmd.led_num = instance; |
|
|
|
|
new_cmd.num_bytes = num_bytes; |
|
|
|
|
memcpy(new_cmd.buff, bytes, OREOLED_CMD_LENGTH_MAX); |
|
|
|
|
ioctl(_oreoled_fd, OREOLED_SEND_BYTES, (unsigned long)&new_cmd); |
|
|
|
|
|
|
|
|
|
// release semaphore
|
|
|
|
|
_state_desired_semaphore = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// update_timer - called by scheduler and updates PX4 driver with commands
|
|
|
|
|
void OreoLED_PX4::update_timer(void) |
|
|
|
|
{ |
|
|
|
@ -381,14 +359,8 @@ void OreoLED_PX4::handle_led_control(mavlink_message_t *msg)
@@ -381,14 +359,8 @@ void OreoLED_PX4::handle_led_control(mavlink_message_t *msg)
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// custom pattern
|
|
|
|
|
// custom patterns not implemented
|
|
|
|
|
if (packet.pattern == LED_CONTROL_PATTERN_CUSTOM) { |
|
|
|
|
// sanity check length
|
|
|
|
|
if (packet.custom_len <= OREOLED_CMD_LENGTH_MAX) { |
|
|
|
|
// send bytes
|
|
|
|
|
send_bytes(packet.instance, packet.custom_len, packet.custom_bytes); |
|
|
|
|
_pattern_override = packet.pattern; |
|
|
|
|
} |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|