diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 02c020def0..ddbde85c1e 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -29,6 +29,9 @@ #include #include +#include +#include +#include #include @@ -337,6 +340,7 @@ static void (*battery_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure* act_out_array[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* esc_raw[MAX_NUMBER_OF_CAN_DRIVERS]; +static uavcan::Publisher* 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(*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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 34286275f0..9f175226e0 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -18,6 +18,7 @@ #include #include +#include #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) {