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>

docs/assets/airframes/multicopter/amovlab_f410/amovlabf410_drone_v1.15.4.params

@@ -1056,9 +1056,6 @@
 1	1	UAVCAN_EC_REV	0	6
 1	1	UAVCAN_ENABLE	2	6
 1	1	UAVCAN_LGT_ANTCL	2	6
-1	1	UAVCAN_LGT_LAND	0	6
-1	1	UAVCAN_LGT_NAV	3	6
-1	1	UAVCAN_LGT_STROB	1	6
 1	1	UAVCAN_NODE_ID	1	6
 1	1	UAVCAN_PUB_ARM	0	6
 1	1	UAVCAN_PUB_MBD	0	6

docs/en/dronecan/index.md

@@ -281,6 +281,19 @@ PX4 DroneCAN parameters:
 Select the specific CAN interface(s) used for ESC data output using the [UAVCAN_ESC_IFACE](../advanced_config/parameter_reference.md#UAVCAN_ESC_IFACE) parameter (all that all interfaces are selected by default).
 Note that DroneCAN ESCs should be on their own dedicated CAN interface(s) because ESC messages can saturate the bus and starve other nodes of bandwidth.
 
+### Lights
+
+PX4 can control LEDs via DroneCAN [LightsCommand](https://dronecan.github.io/Specification/7._List_of_standard_data_types/#lightscommand) messages.
+
+Configuration:
+
+1. Set [UAVCAN_LGT_NUM](../advanced_config/parameter_reference.md#UAVCAN_LGT_NUM) to the number of lights (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:
+   - `UAVCAN_LGT_IDx`: The `light_id` matching your peripheral.
+   - `UAVCAN_LGT_FNx`: `Status` for system status colours, or `Anti-collision` for white beacon.
+3. For anti-collision lights, [UAVCAN_LGT_ANTCL](../advanced_config/parameter_reference.md#UAVCAN_LGT_ANTCL) controls when they illuminate (off, armed, prearmed, always on).
+4. Reboot for any changes to take effect.
+
 ## QGC CANNODE Parameter Configuration
 
 QGroundControl can inspect and modify parameters belonging to CAN devices attached to the flight controller, provided the device are connected to the flight controller before QGC is started.

docs/en/peripherals/vertiq.md

@@ -38,6 +38,19 @@ Instructions for integrating the motor/ESC using with DroneCAN can be found in [
 
 These instructions walk you through setting the correct parameters for enabling the flight controller's DroneCAN drivers, setting the correct configuration parameters for communication with Vertiq modules on the DroneCAN bus, ESC configuration, and testing that your flight controller can properly control your modules over DroneCAN.
 
+#### LED Configuration for Vertiq Modules
+
+::: info
+This configuration is only required if you have the optional [Vertiq LED module add-on](https://www.vertiq.co/add-ons).
+Standard Vertiq ESC modules do not include LEDs.
+:::
+
+Vertiq LED Add-on modules have two LEDs per ESC (RGB for status, White for anti-collision).
+See [DroneCAN Lights](../dronecan/index.md#lights) for configuration instructions.
+
+The `light_id` for each LED is calculated as: `esc_index × 3 + BASE_ID`, where `BASE_ID` is 1 for RGB and 2 for White.
+
+
 ### DShot/PWM Configuration
 
 Instructions for integrating the motor/ESC using PWM and DShot can be found in [PWM and DShot Control with a Flight Controller](https://iqmotion.readthedocs.io/en/latest/tutorials/pwm_control_flight_controller.html).

src/drivers/uavcan/module.yaml

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

src/drivers/uavcan/rgbled.cpp

@@ -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;
+	// Early return if disabled or no lights configured
-	uavcan::equipment::indication::LightsCommand cmds;
+	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) {
+	if (_led_controller.update(led_control_data) != 1) {
-		publish_lights = true;
+		return; // No update, nothing to do
+	}
 
-		// RGB color in the standard 5-6-5 16-bit palette.
+	// Compute status color from led_control_data
-		// Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31).
+	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 (_light_functions[i]) {
-
+		case LightFunction::Status:
-		switch (led_control_data.leds[0].color) {
+			cmd.color = status_color;
-		case led_control_s::COLOR_RED:
-			cmd.color.red = brightness >> 3;
-			cmd.color.green = 0;
-			cmd.color.blue = 0;
 			break;
 
-		case led_control_s::COLOR_GREEN:
+		case LightFunction::AntiCollision:
-			cmd.color.red = 0;
+			uint8_t brigtness = is_anticolision_on() ? 255 : 0;
-			cmd.color.green = brightness >> 2;
+			cmd.color = rgb888_to_rgb565(brigtness, brigtness, brigtness);
-			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;
 			break;
 		}
 
-		cmds.commands.push_back(cmd);
+		light_command.commands.push_back(cmd);
-
 	}
 
-	if (_armed_sub.updated()) {
+	_uavcan_pub_lights_cmd.broadcast(light_command);
-		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::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.
+	actuator_armed_s actuator_armed{};
-	// Monocolor lights should interpret this as brightness setpoint: from zero (0, 0, 0) to full brightness (31, 63, 31).
+	_actuator_armed_sub.copy(&actuator_armed);
-	uavcan::equipment::indication::RGB565 color;
 
-	color.red = (31.0f * (float)brightness / 255.0f);
+	switch (_param_uavcan_lgt_antcl.get()) {
-	color.green = (62.0f * (float)brightness / 255.0f);
+	case 3: // Always on
-	color.blue = (31.0f * (float)brightness / 255.0f);
+		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;
 }

src/drivers/uavcan/rgbled.hpp

@@ -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_NUM>) _param_lgt_num,
-		(ParamInt<px4::params::UAVCAN_LGT_STROB>) _param_mode_strobe,
+		(ParamInt<px4::params::UAVCAN_LGT_ANTCL>) _param_uavcan_lgt_antcl
-		(ParamInt<px4::params::UAVCAN_LGT_NAV>) _param_mode_nav,
-		(ParamInt<px4::params::UAVCAN_LGT_LAND>) _param_mode_land
 	)
 };

src/drivers/uavcan/uavcan_params.c

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

src/modules/commander/ModeManagement.cpp

@@ -107,7 +107,7 @@ uint8_t Modes::addExternalMode(const Modes::Mode &mode)
 	int matching_idx = -1;
 
 	for (int i = 0; i < MAX_NUM; ++i) {
-		char hash_param_name[20];
+		char hash_param_name[17];
 		snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", i);
 		const param_t handle = param_find(hash_param_name);
 		int32_t current_hash{};
@@ -164,7 +164,7 @@ uint8_t Modes::addExternalMode(const Modes::Mode &mode)
 
 	if (new_mode_idx != -1 && !_modes[new_mode_idx].valid) {
 		if (need_to_update_param) {
-			char hash_param_name[20];
+			char hash_param_name[17];
 			snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", new_mode_idx);
 			const param_t handle = param_find(hash_param_name);
 

src/modules/control_allocator/VehicleActuatorEffectiveness/ActuatorEffectivenessControlSurfaces.cpp

@@ -80,7 +80,7 @@ void ActuatorEffectivenessControlSurfaces::updateParams()
 
 	// Helper to check if a PWM center parameter is enabled, and clamp it to valid range
 	auto check_pwm_center = [](const char *prefix, int channel) -> bool {
-		char param_name[20];
+		char param_name[17];
 		snprintf(param_name, sizeof(param_name), "%s_CENT%d", prefix, channel);
 		param_t param = param_find(param_name);