AP_UAVCAN: utilizing equipment.indication.LightsCommand

This can be used to command multiple devices on the UAVCAN bus to
update their LEDs. This will come in handy for status outputs etc.

This utilizes equipment.indication.LightsCommand message.
This message is not so important and therefore we limit publishing
it to avoid bus overflow. The priority of the message is also low.

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -29,6 +29,9 @@
 #include <uavcan/equipment/actuator/Status.hpp>
 
 #include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/indication/LightsCommand.hpp>
+#include <uavcan/equipment/indication/SingleLightCommand.hpp>
+#include <uavcan/equipment/indication/RGB565.hpp>
 
 #include <uavcan/equipment/power/BatteryInfo.hpp>
 
@@ -337,6 +340,7 @@ static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uav
 // publisher interfaces
 static uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<uavcan::equipment::esc::RawCommand>* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS];
+static uavcan::Publisher<uavcan::equipment::indication::LightsCommand>* rgb_led[MAX_NUMBER_OF_CAN_DRIVERS];
 
 AP_UAVCAN::AP_UAVCAN() :
     _node_allocator(
@@ -384,6 +388,7 @@ AP_UAVCAN::AP_UAVCAN() :
     }
 
     _rc_out_sem = hal.util->new_semaphore();
+    _led_out_sem = hal.util->new_semaphore();
 
     debug_uavcan(2, "AP_UAVCAN constructed\n\r");
 }
@@ -503,6 +508,12 @@ bool AP_UAVCAN::try_init(void)
                     esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
                     esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
 
+                    rgb_led[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*node);
+                    rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
+                    rgb_led[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
+                    _led_conf.devices_count = 0;
+
                     /*
                      * Informing other nodes that we're ready to work.
                      * Default mode is INITIALIZING.
@@ -669,6 +680,46 @@ void AP_UAVCAN::do_cyclic(void)
 
 		rc_out_sem_give();
 	}
+
+    if (led_out_sem_take()) {
+
+    	led_out_send();
+    	led_out_sem_give();
+    }
+
+}
+
+bool AP_UAVCAN::led_out_sem_take()
+{
+    bool sem_ret = _led_out_sem->take(10);
+    if (!sem_ret) {
+        debug_uavcan(1, "AP_UAVCAN LEDOut semaphore fail\n\r");
+    }
+    return sem_ret;
+}
+
+void AP_UAVCAN::led_out_sem_give()
+{
+    _led_out_sem->give();
+}
+
+void AP_UAVCAN::led_out_send()
+{
+    if (_led_conf.broadcast_enabled && ((AP_HAL::micros64() - _led_conf.last_update) > (AP_UAVCAN_LED_DELAY_MILLISECONDS * 1000))) {
+        uavcan::equipment::indication::LightsCommand msg;
+        uavcan::equipment::indication::SingleLightCommand cmd;
+
+        for (uint8_t i = 0; i < _led_conf.devices_count; i++) {
+            if (_led_conf.devices[i].enabled) {
+                cmd.light_id =_led_conf.devices[i].led_index;
+                cmd.color = _led_conf.devices[i].rgb565_color;
+                msg.commands.push_back(cmd);
+            }
+        }
+
+        rgb_led[_uavcan_i]->broadcast(msg);
+        _led_conf.last_update = AP_HAL::micros64();
+    }
 }
 
 uavcan::ISystemClock & AP_UAVCAN::get_system_clock()
@@ -1256,4 +1307,37 @@ 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) {
+    uavcan::equipment::indication::RGB565 color;
+    color.red = (red >> 3);
+    color.green = (green >> 2);
+    color.blue = (blue >> 3);
+    
+    if (!led_out_sem_take()) return false;
+    
+    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;
+            _led_conf.devices[i].rgb565_color = color;
+            _led_conf.devices[i].enabled = true;
+            _led_conf.broadcast_enabled = true;
+            led_out_sem_give();
+            return true;
+        }
+    }
+    
+    if (_led_conf.devices_count >= AP_UAVCAN_MAX_LED_DEVICES) {
+        led_out_sem_give();
+        return false;
+    }
+    
+    _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;
+    _led_conf.devices_count++;
+    _led_conf.broadcast_enabled = true;
+    led_out_sem_give();
+    return true;
+}
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -18,6 +18,7 @@
 #include <AP_BattMonitor/AP_BattMonitor_Backend.h>
 
 #include <uavcan/helpers/heap_based_pool_allocator.hpp>
+#include <uavcan/equipment/indication/RGB565.hpp>
 
 #ifndef UAVCAN_NODE_POOL_SIZE
 #define UAVCAN_NODE_POOL_SIZE 8192
@@ -43,6 +44,9 @@
 #define AP_UAVCAN_HW_VERS_MAJOR 1
 #define AP_UAVCAN_HW_VERS_MINOR 0
 
+#define AP_UAVCAN_MAX_LED_DEVICES 4
+#define AP_UAVCAN_LED_DELAY_MILLISECONDS 50
+
 class AP_UAVCAN {
 public:
     AP_UAVCAN();
@@ -114,6 +118,11 @@ public:
     // synchronization for RC output
     bool rc_out_sem_take();
     void rc_out_sem_give();
+    
+    // synchronization for LED output
+    bool led_out_sem_take();
+    void led_out_sem_give();
+    void led_out_send();
 
     // output from do_cyclic
     void rc_out_send_servos();
@@ -167,7 +176,21 @@ private:
     uint8_t _rco_armed;
     uint8_t _rco_safety;
 
+    typedef struct {
+        bool enabled;
+        uint8_t led_index;
+        uavcan::equipment::indication::RGB565 rgb565_color;
+    } _led_device;
+
+    struct {
+        bool broadcast_enabled;
+        _led_device devices[AP_UAVCAN_MAX_LED_DEVICES];
+        uint8_t devices_count;
+        uint64_t last_update;
+    } _led_conf;
+
     AP_HAL::Semaphore *_rc_out_sem;
+    AP_HAL::Semaphore *_led_out_sem;
 
     class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
         SystemClock()
@@ -243,6 +266,7 @@ public:
     void rco_force_safety_off(void);
     void rco_arm_actuators(bool arm);
     void rco_write(uint16_t pulse_len, uint8_t ch);
+    bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);
 
     void set_parent_can_mgr(AP_HAL::CANManager* parent_can_mgr)
     {