mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:58:53 +08:00
UAVCAN: Configurable LED Light Control with Flexible Addressing (#26253)
* feat: implement UAVCAN LED control for individual light control and assignment * uavcan led: nit-picks from review * uavcan led: reduce maximum number of lights to avoid unused parameters * uavcan led: simplify anticolision on check * uavcan led: correctly map 8-bit RGB to rgb565 * Trim param name character arrays to 17 16 characters + \0 termination * uavcan led: final nit-picks --------- Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Claudio Chies
co-authored by
Matthias Grob
Matthias Grob
parent
32c94bd3b1
commit
e52ce5c43b
@@ -1,3 +1,5 @@
|
||||
__max_num_uavcan_lights: &max_num_uavcan_lights 3 # Needs to be equal to MAX_NUM_UAVCAN_LIGHTS constant
|
||||
|
||||
module_name: UAVCAN
|
||||
parameters:
|
||||
- group: UAVCAN
|
||||
@@ -24,6 +26,46 @@ parameters:
|
||||
min: 1
|
||||
max: 255
|
||||
reboot_required: true
|
||||
UAVCAN_LGT_NUM:
|
||||
description:
|
||||
short: Number of UAVCAN lights to configure
|
||||
long: |
|
||||
Number of lights to control via UAVCAN LightsCommand messages.
|
||||
Set to 0 to disable UAVCAN light control.
|
||||
Each light uses two parameters: LGT_IDx for the light_id and LGT_FNx for the function.
|
||||
type: int32
|
||||
min: 0
|
||||
max: *max_num_uavcan_lights
|
||||
default: 1
|
||||
reboot_required: true
|
||||
UAVCAN_LGT_ID${i}:
|
||||
description:
|
||||
short: Light ${i} ID
|
||||
long: |
|
||||
specifies the light_id value for light ${i} in UAVCAN LightsCommand messages.
|
||||
This determines which physical LED responds to commands for this light slot.
|
||||
type: int32
|
||||
num_instances: *max_num_uavcan_lights
|
||||
instance_start: 0
|
||||
min: 0
|
||||
max: 255
|
||||
default: 0
|
||||
UAVCAN_LGT_FN${i}:
|
||||
description:
|
||||
short: Light ${i} function
|
||||
long: |
|
||||
Function assigned to light ${i}.
|
||||
0: Status - displays system status colors
|
||||
1: Anti-collision - white beacon controlled by LGT_ANTCL parameter
|
||||
type: enum
|
||||
num_instances: *max_num_uavcan_lights
|
||||
instance_start: 0
|
||||
min: 0
|
||||
max: 1
|
||||
default: 0
|
||||
values:
|
||||
0: Status Light
|
||||
1: Anti-collision Light
|
||||
actuator_output:
|
||||
show_subgroups_if: 'UAVCAN_ENABLE>=3'
|
||||
config_parameters:
|
||||
|
||||
+119
-120
@@ -32,6 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
#include "rgbled.hpp"
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
UavcanRGBController::UavcanRGBController(uavcan::INode &node) :
|
||||
ModuleParams(nullptr),
|
||||
@@ -44,6 +45,38 @@ UavcanRGBController::UavcanRGBController(uavcan::INode &node) :
|
||||
|
||||
int UavcanRGBController::init()
|
||||
{
|
||||
// Cache number of lights (0 disables the feature)
|
||||
_num_lights = math::min(static_cast<uint8_t>(_param_lgt_num.get()), MAX_NUM_UAVCAN_LIGHTS);
|
||||
|
||||
if (_num_lights == 0) {
|
||||
return 0; // Disabled, don't start timer
|
||||
}
|
||||
|
||||
// Cache parameter handles and values for each light
|
||||
for (uint8_t i = 0; i < _num_lights; i++) {
|
||||
char param_name[17];
|
||||
|
||||
// Light ID parameter
|
||||
snprintf(param_name, sizeof(param_name), "UAVCAN_LGT_ID%u", i);
|
||||
_light_id_params[i] = param_find(param_name);
|
||||
|
||||
if (_light_id_params[i] != PARAM_INVALID) {
|
||||
int32_t light_id = 0;
|
||||
param_get(_light_id_params[i], &light_id);
|
||||
_light_ids[i] = static_cast<uint8_t>(light_id);
|
||||
}
|
||||
|
||||
// Light function parameter
|
||||
snprintf(param_name, sizeof(param_name), "UAVCAN_LGT_FN%u", i);
|
||||
_light_fn_params[i] = param_find(param_name);
|
||||
|
||||
if (_light_fn_params[i] != PARAM_INVALID) {
|
||||
int32_t light_fn = 0;
|
||||
param_get(_light_fn_params[i], &light_fn);
|
||||
_light_functions[i] = static_cast<LightFunction>(light_fn);
|
||||
}
|
||||
}
|
||||
|
||||
// Setup timer and call back function for periodic updates
|
||||
_timer.setCallback(TimerCbBinder(this, &UavcanRGBController::periodic_update));
|
||||
_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
|
||||
@@ -52,142 +85,108 @@ int UavcanRGBController::init()
|
||||
|
||||
void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
|
||||
{
|
||||
bool publish_lights = false;
|
||||
uavcan::equipment::indication::LightsCommand cmds;
|
||||
// Early return if disabled or no lights configured
|
||||
if (_num_lights == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for status color updates from led_controller
|
||||
LedControlData led_control_data;
|
||||
|
||||
if (_led_controller.update(led_control_data) == 1) {
|
||||
publish_lights = true;
|
||||
if (_led_controller.update(led_control_data) != 1) {
|
||||
return; // No update, nothing to do
|
||||
}
|
||||
|
||||
// RGB color in the standard 5-6-5 16-bit palette.
|
||||
// Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31).
|
||||
// Compute status color from led_control_data
|
||||
uavcan::equipment::indication::RGB565 status_color{};
|
||||
uint8_t brightness = led_control_data.leds[0].brightness;
|
||||
|
||||
switch (led_control_data.leds[0].color) {
|
||||
case led_control_s::COLOR_RED:
|
||||
status_color = rgb888_to_rgb565(brightness, 0, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_GREEN:
|
||||
status_color = rgb888_to_rgb565(0, brightness, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_BLUE:
|
||||
status_color = rgb888_to_rgb565(0, 0, brightness);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_AMBER: // make it the same as yellow
|
||||
case led_control_s::COLOR_YELLOW:
|
||||
status_color = rgb888_to_rgb565(brightness, brightness, 0);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_PURPLE:
|
||||
status_color = rgb888_to_rgb565(brightness, 0, brightness);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_CYAN:
|
||||
status_color = rgb888_to_rgb565(0, brightness, brightness);
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_WHITE:
|
||||
status_color = rgb888_to_rgb565(brightness, brightness, brightness);
|
||||
break;
|
||||
|
||||
default:
|
||||
case led_control_s::COLOR_OFF:
|
||||
break;
|
||||
}
|
||||
|
||||
// Build and send light commands for all configured lights
|
||||
uavcan::equipment::indication::LightsCommand light_command;
|
||||
|
||||
for (uint8_t i = 0; i < _num_lights; i++) {
|
||||
uavcan::equipment::indication::SingleLightCommand cmd;
|
||||
cmd.light_id = _light_ids[i];
|
||||
|
||||
uint8_t brightness = led_control_data.leds[0].brightness;
|
||||
|
||||
switch (led_control_data.leds[0].color) {
|
||||
case led_control_s::COLOR_RED:
|
||||
cmd.color.red = brightness >> 3;
|
||||
cmd.color.green = 0;
|
||||
cmd.color.blue = 0;
|
||||
switch (_light_functions[i]) {
|
||||
case LightFunction::Status:
|
||||
cmd.color = status_color;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_GREEN:
|
||||
cmd.color.red = 0;
|
||||
cmd.color.green = brightness >> 2;
|
||||
cmd.color.blue = 0;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_BLUE:
|
||||
cmd.color.red = 0;
|
||||
cmd.color.green = 0;
|
||||
cmd.color.blue = brightness >> 3;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_AMBER: // make it the same as yellow
|
||||
|
||||
// FALLTHROUGH
|
||||
case led_control_s::COLOR_YELLOW:
|
||||
cmd.color.red = (brightness / 2) >> 3;
|
||||
cmd.color.green = (brightness / 2) >> 2;
|
||||
cmd.color.blue = 0;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_PURPLE:
|
||||
cmd.color.red = (brightness / 2) >> 3;
|
||||
cmd.color.green = 0;
|
||||
cmd.color.blue = (brightness / 2) >> 3;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_CYAN:
|
||||
cmd.color.red = 0;
|
||||
cmd.color.green = (brightness / 2) >> 2;
|
||||
cmd.color.blue = (brightness / 2) >> 3;
|
||||
break;
|
||||
|
||||
case led_control_s::COLOR_WHITE:
|
||||
cmd.color.red = (brightness / 3) >> 3;
|
||||
cmd.color.green = (brightness / 3) >> 2;
|
||||
cmd.color.blue = (brightness / 3) >> 3;
|
||||
break;
|
||||
|
||||
default: // led_control_s::COLOR_OFF
|
||||
cmd.color.red = 0;
|
||||
cmd.color.green = 0;
|
||||
cmd.color.blue = 0;
|
||||
case LightFunction::AntiCollision:
|
||||
uint8_t brigtness = is_anticolision_on() ? 255 : 0;
|
||||
cmd.color = rgb888_to_rgb565(brigtness, brigtness, brigtness);
|
||||
break;
|
||||
}
|
||||
|
||||
cmds.commands.push_back(cmd);
|
||||
|
||||
light_command.commands.push_back(cmd);
|
||||
}
|
||||
|
||||
if (_armed_sub.updated()) {
|
||||
publish_lights = true;
|
||||
|
||||
actuator_armed_s armed;
|
||||
|
||||
if (_armed_sub.copy(&armed)) {
|
||||
|
||||
/* Determine the current control mode
|
||||
* If a light's control mode config >= current control mode, the light will be enabled
|
||||
* Logic must match UAVCAN_LGT_* param values.
|
||||
* @value 0 Always off
|
||||
* @value 1 When autopilot is armed
|
||||
* @value 2 When autopilot is prearmed
|
||||
* @value 3 Always on
|
||||
*/
|
||||
uint8_t control_mode = 0;
|
||||
|
||||
if (armed.armed) {
|
||||
control_mode = 1;
|
||||
|
||||
} else if (armed.prearmed) {
|
||||
control_mode = 2;
|
||||
|
||||
} else {
|
||||
control_mode = 3;
|
||||
}
|
||||
|
||||
uavcan::equipment::indication::SingleLightCommand cmd;
|
||||
|
||||
// Beacons
|
||||
cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_ANTI_COLLISION;
|
||||
cmd.color = brightness_to_rgb565(_param_mode_anti_col.get() >= control_mode ? 255 : 0);
|
||||
cmds.commands.push_back(cmd);
|
||||
|
||||
// Strobes
|
||||
cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_STROBE;
|
||||
cmd.color = brightness_to_rgb565(_param_mode_strobe.get() >= control_mode ? 255 : 0);
|
||||
cmds.commands.push_back(cmd);
|
||||
|
||||
// Nav lights
|
||||
cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_RIGHT_OF_WAY;
|
||||
cmd.color = brightness_to_rgb565(_param_mode_nav.get() >= control_mode ? 255 : 0);
|
||||
cmds.commands.push_back(cmd);
|
||||
|
||||
// Landing lights
|
||||
cmd.light_id = uavcan::equipment::indication::SingleLightCommand::LIGHT_ID_LANDING;
|
||||
cmd.color = brightness_to_rgb565(_param_mode_land.get() >= control_mode ? 255 : 0);
|
||||
cmds.commands.push_back(cmd);
|
||||
}
|
||||
}
|
||||
|
||||
if (publish_lights) {
|
||||
_uavcan_pub_lights_cmd.broadcast(cmds);
|
||||
}
|
||||
_uavcan_pub_lights_cmd.broadcast(light_command);
|
||||
}
|
||||
|
||||
uavcan::equipment::indication::RGB565 UavcanRGBController::brightness_to_rgb565(uint8_t brightness)
|
||||
bool UavcanRGBController::is_anticolision_on()
|
||||
{
|
||||
// RGB color in the standard 5-6-5 16-bit palette.
|
||||
// Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31).
|
||||
uavcan::equipment::indication::RGB565 color;
|
||||
actuator_armed_s actuator_armed{};
|
||||
_actuator_armed_sub.copy(&actuator_armed);
|
||||
|
||||
color.red = (31.0f * (float)brightness / 255.0f);
|
||||
color.green = (62.0f * (float)brightness / 255.0f);
|
||||
color.blue = (31.0f * (float)brightness / 255.0f);
|
||||
switch (_param_uavcan_lgt_antcl.get()) {
|
||||
case 3: // Always on
|
||||
return true;
|
||||
|
||||
return color;
|
||||
case 2: // When autopilot is prearmed
|
||||
return actuator_armed.armed || actuator_armed.prearmed;
|
||||
|
||||
case 1: // When autopilot is armed
|
||||
return actuator_armed.armed;
|
||||
|
||||
case 0: // Always off
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
uavcan::equipment::indication::RGB565 UavcanRGBController::rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue)
|
||||
{
|
||||
// RGB565: Full brightness is (31, 63, 31), off is (0, 0, 0)
|
||||
uavcan::equipment::indication::RGB565 rgb565{};
|
||||
rgb565.red = (red * 31 + 127) / 255;
|
||||
rgb565.green = (green * 63 + 127) / 255;
|
||||
rgb565.blue = (blue * 31 + 127) / 255;
|
||||
return rgb565;
|
||||
}
|
||||
|
||||
@@ -54,9 +54,20 @@ private:
|
||||
// Max update rate to avoid excessive bus traffic
|
||||
static constexpr unsigned MAX_RATE_HZ = 20;
|
||||
|
||||
// Maximum number of configurable lights
|
||||
static constexpr uint8_t MAX_NUM_UAVCAN_LIGHTS = 3;
|
||||
|
||||
// Light function types
|
||||
enum class LightFunction : uint8_t {
|
||||
Status = 0, // System status colors from led_control
|
||||
AntiCollision = 1 // White beacon based on arm state
|
||||
};
|
||||
|
||||
void periodic_update(const uavcan::TimerEvent &);
|
||||
|
||||
uavcan::equipment::indication::RGB565 brightness_to_rgb565(uint8_t brightness);
|
||||
bool is_anticolision_on(); ///< Evaluates current on state of collision lights accordingt to UAVCAN_LGT_ANTCL
|
||||
|
||||
uavcan::equipment::indication::RGB565 rgb888_to_rgb565(uint8_t red, uint8_t green, uint8_t blue);
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanRGBController *, void (UavcanRGBController::*)(const uavcan::TimerEvent &)>
|
||||
TimerCbBinder;
|
||||
@@ -65,14 +76,21 @@ private:
|
||||
uavcan::Publisher<uavcan::equipment::indication::LightsCommand> _uavcan_pub_lights_cmd;
|
||||
uavcan::TimerEventForwarder<TimerCbBinder> _timer;
|
||||
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
|
||||
|
||||
LedController _led_controller;
|
||||
|
||||
// Cached configuration (set during init, requires reboot to change)
|
||||
uint8_t _num_lights{0};
|
||||
uint8_t _light_ids[MAX_NUM_UAVCAN_LIGHTS] {};
|
||||
LightFunction _light_functions[MAX_NUM_UAVCAN_LIGHTS] {};
|
||||
|
||||
// Cached parameter handles
|
||||
param_t _light_id_params[MAX_NUM_UAVCAN_LIGHTS] {};
|
||||
param_t _light_fn_params[MAX_NUM_UAVCAN_LIGHTS] {};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::UAVCAN_LGT_ANTCL>) _param_mode_anti_col,
|
||||
(ParamInt<px4::params::UAVCAN_LGT_STROB>) _param_mode_strobe,
|
||||
(ParamInt<px4::params::UAVCAN_LGT_NAV>) _param_mode_nav,
|
||||
(ParamInt<px4::params::UAVCAN_LGT_LAND>) _param_mode_land
|
||||
(ParamInt<px4::params::UAVCAN_LGT_NUM>) _param_lgt_num,
|
||||
(ParamInt<px4::params::UAVCAN_LGT_ANTCL>) _param_uavcan_lgt_antcl
|
||||
)
|
||||
};
|
||||
|
||||
@@ -135,7 +135,7 @@ PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1);
|
||||
* UAVCAN ANTI_COLLISION light operating mode
|
||||
*
|
||||
* This parameter defines the minimum condition under which the system will command
|
||||
* the ANTI_COLLISION lights on
|
||||
* lights with anti-collision function to turn on (white).
|
||||
*
|
||||
* 0 - Always off
|
||||
* 1 - When autopilot is armed
|
||||
@@ -153,72 +153,6 @@ PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1);
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_LGT_ANTCL, 2);
|
||||
|
||||
/**
|
||||
* UAVCAN STROBE light operating mode
|
||||
*
|
||||
* This parameter defines the minimum condition under which the system will command
|
||||
* the STROBE lights on
|
||||
*
|
||||
* 0 - Always off
|
||||
* 1 - When autopilot is armed
|
||||
* 2 - When autopilot is prearmed
|
||||
* 3 - Always on
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @value 0 Always off
|
||||
* @value 1 When autopilot is armed
|
||||
* @value 2 When autopilot is prearmed
|
||||
* @value 3 Always on
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_LGT_STROB, 1);
|
||||
|
||||
/**
|
||||
* UAVCAN RIGHT_OF_WAY light operating mode
|
||||
*
|
||||
* This parameter defines the minimum condition under which the system will command
|
||||
* the RIGHT_OF_WAY lights on
|
||||
*
|
||||
* 0 - Always off
|
||||
* 1 - When autopilot is armed
|
||||
* 2 - When autopilot is prearmed
|
||||
* 3 - Always on
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @value 0 Always off
|
||||
* @value 1 When autopilot is armed
|
||||
* @value 2 When autopilot is prearmed
|
||||
* @value 3 Always on
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_LGT_NAV, 3);
|
||||
|
||||
/**
|
||||
* UAVCAN LIGHT_ID_LANDING light operating mode
|
||||
*
|
||||
* This parameter defines the minimum condition under which the system will command
|
||||
* the LIGHT_ID_LANDING lights on
|
||||
*
|
||||
* 0 - Always off
|
||||
* 1 - When autopilot is armed
|
||||
* 2 - When autopilot is prearmed
|
||||
* 3 - Always on
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @value 0 Always off
|
||||
* @value 1 When autopilot is armed
|
||||
* @value 2 When autopilot is prearmed
|
||||
* @value 3 Always on
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_LGT_LAND, 0);
|
||||
|
||||
/**
|
||||
* publish Arming Status stream
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user