fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan rgbled: overhaul color functions

Browse Source
This commit is contained in:
Matthias Grob 2026-02-18 20:32:49 +01:00
parent 637cece115
commit 51be1e3fb9
4 changed files with 106 additions and 111 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
docs/en/dronecan/index.md
View File

@ -287,16 +287,15 @@ PX4 can control LEDs via DroneCAN [LightsCommand](https://dronecan.github.io/Spe
Configuration:
1. Set [UAVCAN_LGT_NUM](../advanced_config/parameter_reference.md#UAVCAN_LGT_NUM) to the number of lights (0-2, 0 disables).
You might need to reopen the ground station to have parameters for new instances available.
2. For each light slot (0 to NUM-1), set:
1. Set [UAVCAN_LGT_NUM](../advanced_config/parameter_reference.md#UAVCAN_LGT_NUM) to the number of lights (0 disables, maximum 2). You need to reboot and reopen the ground station to have parameters for new instances available.
2. [UAVCAN_LGT_MODE](../advanced_config/parameter_reference.md#UAVCAN_LGT_MODE) controls when lights should be in active state (always off, when armed, when prearmed, always on).
3. For each light slot (0 to NUM-1), set:
- `UAVCAN_LGT_IDx`: The `light_id` matching your peripheral.
- `UAVCAN_LGT_FNx`: The light function. Available options:
- `Status`: System status colours from the LED controller.
- `Anti-collision` to `White Navigation`: Light functions controlled by `UAVCAN_LGT_MODE`.
- `Status / Anti-collision` to `Status / Off`: Hybrid modes that show status colours when `UAVCAN_LGT_MODE` is inactive, and switch to the second function when active.
3. [UAVCAN_LGT_MODE](../advanced_config/parameter_reference.md#UAVCAN_LGT_MODE) controls when navigation lights turn on (off, armed, prearmed, always on).
4. Reboot for any changes to take effect.
- System status light
- Static colors which light up when `UAVCAN_LGT_MODE` is active.
- Hybrid modes where the Status is shown when `UAVCAN_LGT_MODE` is inactive, and a static color when active.
4. Reboot for changes to take effect.
## QGC CANNODE Parameter Configuration

22
src/drivers/uavcan/module.yaml
View File

@ -55,8 +55,8 @@ parameters:
description:
short: Light ${i} function
long: |
Function for light ${i}. Navigation lights ("Anti-collision" to "White Navigation") follow UAVCAN_LGT_MODE.
Hybrid ("Status / Anti-collision" - "Status / White Navigation"): status when UAVCAN_LGT_MODE is inactive, second function to use when active.
Function for light ${i}.
UAVCAN_LGT_MODE determines when the first option or second option is active Off/On.
type: enum
num_instances: *max_num_uavcan_lights
instance_start: 0
@ -64,16 +64,14 @@ parameters:
max: 9
default: 0
values:
0: Status
1: Anti-collision
2: Red Navigation
3: Green Navigation
4: White Navigation
5: Status / Anti-collision
6: Status / Red Navigation
7: Status / Green Navigation
8: Status / White Navigation
9: Status / Off
0: Status/Status
1: Off/White
2: Off/Red
3: Off/Green
4: Status/White
5: Status/Red
6: Status/Green
7: Status/Off
actuator_output:
show_subgroups_if: 'UAVCAN_ENABLE>=3'
config_parameters:

145
src/drivers/uavcan/rgbled.cpp
View File

@ -46,7 +46,7 @@ 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);
_num_lights = math::min(static_cast<uint8_t>(_param_uavcan_lgt_num.get()), MAX_NUM_UAVCAN_LIGHTS);
if (_num_lights == 0) {
return 0; // Disabled, don't start timer
@ -98,109 +98,71 @@ void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
}
// 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;
case led_control_s::COLOR_OFF:
default:
break;
}
uavcan::equipment::indication::RGB565 status_color = color_to_rgb565(led_control_data.leds[0].color, led_control_data.leds[0].brightness);
// Build and send light commands for all configured lights
uavcan::equipment::indication::LightsCommand light_command;
const bool light_mode_active = check_light_state(static_cast<LightMode>(_param_lgt_mode.get()));
brightness = light_mode_active ? 255 : 0;
const bool light_on = is_light_on();
for (uint8_t i = 0; i < _num_lights; i++) {
uavcan::equipment::indication::SingleLightCommand cmd;
cmd.light_id = _light_ids[i];
uavcan::equipment::indication::RGB565 color_on, color_off;
color_on = color_off = color_to_rgb565(led_control_s::COLOR_OFF);
switch (_light_functions[i]) {
// Always show status
case LightFunction::Status:
cmd.color = status_color;
color_on = color_off = status_color;
break;
case LightFunction::AntiCollision:
cmd.color = rgb888_to_rgb565(brightness, brightness, brightness);
// Static color when UAVCAN_LGT_MODE active
case LightFunction::White:
color_on = color_to_rgb565(led_control_s::COLOR_WHITE);
break;
case LightFunction::RedNavigation:
cmd.color = rgb888_to_rgb565(brightness, 0, 0);
case LightFunction::Red:
color_on = color_to_rgb565(led_control_s::COLOR_RED);
break;
case LightFunction::GreenNavigation:
cmd.color = rgb888_to_rgb565(0, brightness, 0);
case LightFunction::Green:
color_on = color_to_rgb565(led_control_s::COLOR_GREEN);
break;
case LightFunction::WhiteNavigation:
cmd.color = rgb888_to_rgb565(brightness, brightness, brightness);
// Hybrid functions: show status when UAVCAN_LGT_MODE inactive, static color when active
case LightFunction::StatusOrWhite:
color_on = color_to_rgb565(led_control_s::COLOR_WHITE);
color_off = status_color;
break;
// Hybrid functions: show status when UAVCAN_LGT_MODE inactive, navigation light when active
case LightFunction::StatusOrAntiCollision:
cmd.color = light_mode_active ? rgb888_to_rgb565(brightness, brightness, brightness) : status_color;
case LightFunction::StatusOrRed:
color_on = color_to_rgb565(led_control_s::COLOR_RED);
color_off = status_color;
break;
case LightFunction::StatusOrRedNavigation:
cmd.color = light_mode_active ? rgb888_to_rgb565(brightness, 0, 0) : status_color;
break;
case LightFunction::StatusOrGreenNavigation:
cmd.color = light_mode_active ? rgb888_to_rgb565(0, brightness, 0) : status_color;
break;
case LightFunction::StatusOrWhiteNavigation:
cmd.color = light_mode_active ? rgb888_to_rgb565(brightness, brightness, brightness) : status_color;
case LightFunction::StatusOrGreen:
color_on = color_to_rgb565(led_control_s::COLOR_GREEN);
color_off = status_color;
break;
case LightFunction::StatusOrOff:
cmd.color = light_mode_active ? rgb888_to_rgb565(0, 0, 0) : status_color;
color_off = status_color;
break;
}
uavcan::equipment::indication::SingleLightCommand cmd;
cmd.light_id = _light_ids[i];
cmd.color = light_on ? color_on : color_off;
light_command.commands.push_back(cmd);
}
_uavcan_pub_lights_cmd.broadcast(light_command);
}
bool UavcanRGBController::check_light_state(LightMode mode)
bool UavcanRGBController::is_light_on()
{
actuator_armed_s actuator_armed{};
_actuator_armed_sub.copy(&actuator_armed);
switch (_param_lgt_mode.get()) {
switch (_param_uavcan_lgt_mode.get()) {
case 3: // Always on
return true;
@ -216,6 +178,53 @@ bool UavcanRGBController::check_light_state(LightMode mode)
}
}
uavcan::equipment::indication::RGB565 UavcanRGBController::color_to_rgb565(uint8_t color, uint8_t brightness)
{
uint8_t R = 0, G = 0, B = 0;
switch (color) {
case led_control_s::COLOR_RED:
R = brightness;
break;
case led_control_s::COLOR_GREEN:
G = brightness;
break;
case led_control_s::COLOR_BLUE:
B = brightness;
break;
case led_control_s::COLOR_AMBER: // make it the same as yellow
case led_control_s::COLOR_YELLOW:
R = brightness;
G = brightness;
break;
case led_control_s::COLOR_PURPLE:
R = brightness;
B = brightness;
break;
case led_control_s::COLOR_CYAN:
G = brightness;
B = brightness;
break;
case led_control_s::COLOR_WHITE:
R = brightness;
G = brightness;
B = brightness;
break;
default:
case led_control_s::COLOR_OFF:
break;
}
return rgb888_to_rgb565(R, G, B);
}
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)

35
src/drivers/uavcan/rgbled.hpp
View File

@ -59,32 +59,21 @@ private:
// Light function types - must match values in module.yaml UAVCAN_LGT_FN
enum class LightFunction : uint8_t {
Status = 0, // System status colors from led_control
AntiCollision = 1, // White beacon based on arm state
RedNavigation = 2, // Red navigation light
GreenNavigation = 3, // Green navigation light
WhiteNavigation = 4, // White navigation light
StatusOrAntiCollision = 5, // Status when LGT_MODE inactive, white beacon when active
StatusOrRedNavigation = 6, // Status when LGT_MODE inactive, red nav when active
StatusOrGreenNavigation = 7, // Status when LGT_MODE inactive, green nav when active
StatusOrWhiteNavigation = 8, // Status when LGT_MODE inactive, white nav when active
StatusOrOff = 9 // Status when LGT_MODE inactive, off when active
Status = 0,
White = 1,
Red = 2,
Green = 3,
StatusOrWhite = 4,
StatusOrRed = 5,
StatusOrGreen = 6,
StatusOrOff = 7
};
enum class LightMode : uint8_t {
Off = 0,
WhenArmed = 1,
WhenPrearmed = 2,
AlwaysOn = 3
};
// White light intensity levels
enum class Brightness { None, Full };
void periodic_update(const uavcan::TimerEvent &);
bool check_light_state(LightMode mode);
bool is_light_on();
uavcan::equipment::indication::RGB565 color_to_rgb565(uint8_t color, uint8_t brightness = 255);
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 &)>
@ -108,7 +97,7 @@ private:
param_t _light_fn_params[MAX_NUM_UAVCAN_LIGHTS] {};
DEFINE_PARAMETERS(
(ParamInt<px4::params::UAVCAN_LGT_NUM>) _param_lgt_num,
(ParamInt<px4::params::UAVCAN_LGT_MODE>) _param_lgt_mode
(ParamInt<px4::params::UAVCAN_LGT_NUM>) _param_uavcan_lgt_num,
(ParamInt<px4::params::UAVCAN_LGT_MODE>) _param_uavcan_lgt_mode
)
};