uavcan rgbled: overhaul color functions

docs/en/dronecan/index.md

@@ -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).
+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.
-   You might need to 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).
-2. For each light slot (0 to NUM-1), set:
+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.
+     - System status light
-     - `Anti-collision` to `White Navigation`: Light functions controlled by `UAVCAN_LGT_MODE`.
+     - Static colors which light up when `UAVCAN_LGT_MODE` is active.
-     - `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.
+     - Hybrid modes where the Status is shown when `UAVCAN_LGT_MODE` is inactive, and a static color 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 changes to take effect.
-4. Reboot for any changes to take effect.
 
 ## QGC CANNODE Parameter Configuration
 

src/drivers/uavcan/module.yaml

@@ -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.
+          Function for light ${i}.
-          Hybrid ("Status / Anti-collision" - "Status / White Navigation"): status when UAVCAN_LGT_MODE is inactive, second function to use when active.
+          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
+        0: Status/Status
-        1: Anti-collision
+        1: Off/White
-        2: Red Navigation
+        2: Off/Red
-        3: Green Navigation
+        3: Off/Green
-        4: White Navigation
+        4: Status/White
-        5: Status / Anti-collision
+        5: Status/Red
-        6: Status / Red Navigation
+        6: Status/Green
-        7: Status / Green Navigation
+        7: Status/Off
-        8: Status / White Navigation
-        9: Status / Off
 actuator_output:
   show_subgroups_if: 'UAVCAN_ENABLE>=3'
   config_parameters:

src/drivers/uavcan/rgbled.cpp

@@ -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{};
+	uavcan::equipment::indication::RGB565 status_color = color_to_rgb565(led_control_data.leds[0].color, led_control_data.leds[0].brightness);
-	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;
-	}
 
 	// Build and send light commands for all configured lights
 	uavcan::equipment::indication::LightsCommand light_command;
-
+	const bool light_on = is_light_on();
-	const bool light_mode_active = check_light_state(static_cast<LightMode>(_param_lgt_mode.get()));
-	brightness = light_mode_active ? 255 : 0;
 
 	for (uint8_t i = 0; i < _num_lights; i++) {
-		uavcan::equipment::indication::SingleLightCommand cmd;
+		uavcan::equipment::indication::RGB565 color_on, color_off;
-		cmd.light_id = _light_ids[i];
+		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:
+		// Static color when UAVCAN_LGT_MODE active
-			cmd.color = rgb888_to_rgb565(brightness, brightness, brightness);
+		case LightFunction::White:
+			color_on = color_to_rgb565(led_control_s::COLOR_WHITE);
 			break;
 
-		case LightFunction::RedNavigation:
+		case LightFunction::Red:
-			cmd.color = rgb888_to_rgb565(brightness, 0, 0);
+			color_on = color_to_rgb565(led_control_s::COLOR_RED);
 			break;
 
-		case LightFunction::GreenNavigation:
+		case LightFunction::Green:
-			cmd.color = rgb888_to_rgb565(0, brightness, 0);
+			color_on = color_to_rgb565(led_control_s::COLOR_GREEN);
 			break;
 
-		case LightFunction::WhiteNavigation:
+		// Hybrid functions: show status when UAVCAN_LGT_MODE inactive, static color when active
-			cmd.color = rgb888_to_rgb565(brightness, brightness, brightness);
+		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::StatusOrRed:
-		case LightFunction::StatusOrAntiCollision:
+			color_on = color_to_rgb565(led_control_s::COLOR_RED);
-			cmd.color = light_mode_active ? rgb888_to_rgb565(brightness, brightness, brightness) : status_color;
+			color_off = status_color;
 			break;
 
-		case LightFunction::StatusOrRedNavigation:
+		case LightFunction::StatusOrGreen:
-			cmd.color = light_mode_active ? rgb888_to_rgb565(brightness, 0, 0) : status_color;
+			color_on = color_to_rgb565(led_control_s::COLOR_GREEN);
-			break;
+			color_off = status_color;
-
-		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;
 			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)

src/drivers/uavcan/rgbled.hpp

@@ -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
+		Status = 0,
-		AntiCollision = 1,             // White beacon based on arm state
+		White = 1,
-		RedNavigation = 2,             // Red navigation light
+		Red = 2,
-		GreenNavigation = 3,           // Green navigation light
+		Green = 3,
-		WhiteNavigation = 4,           // White navigation light
+		StatusOrWhite = 4,
-		StatusOrAntiCollision = 5,     // Status when LGT_MODE inactive, white beacon when active
+		StatusOrRed = 5,
-		StatusOrRedNavigation = 6,     // Status when LGT_MODE inactive, red nav when active
+		StatusOrGreen = 6,
-		StatusOrGreenNavigation = 7,   // Status when LGT_MODE inactive, green nav when active
+		StatusOrOff = 7
-		StatusOrWhiteNavigation = 8,   // Status when LGT_MODE inactive, white nav when active
-		StatusOrOff = 9                // Status when LGT_MODE inactive, off when active
 	};
 
-	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_NUM>) _param_uavcan_lgt_num,
-		(ParamInt<px4::params::UAVCAN_LGT_MODE>) _param_lgt_mode
+		(ParamInt<px4::params::UAVCAN_LGT_MODE>) _param_uavcan_lgt_mode
 	)
 };