Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Nikita Tomilov 2018-01-17 10:18:00 +03:00 committed by Tom Pittenger
parent 8ffc2aa6e6
commit a1017fb815
2 changed files with 108 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
libraries/AP_UAVCAN/AP_UAVCAN.cpp
View File

@ -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

24
libraries/AP_UAVCAN/AP_UAVCAN.h
View File

@ -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)
{