Extend UAVCAN light control

Adds automatic control of more light types via UAVCAN. Publishes
commands for beacon, strobe, nav, and landings lights. Each is
automatically controlled based on arming state and this behavior is
configurable per light type via params. For example, nav lights can be
set to be always on while beacons turn on when the system is prearmed
and strobes turn on only when armed.

src/drivers/uavcan/rgbled.cpp

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include "rgbled.hpp"
+#include <parameters/param.h>
 
 UavcanRGBController::UavcanRGBController(uavcan::INode &node) :
 	_node(node),
@@ -43,6 +44,18 @@ UavcanRGBController::UavcanRGBController(uavcan::INode &node) :
 
 int UavcanRGBController::init()
 {
+	// Get parameters
+	int32_t i = 0;
+	param_get(param_find("UAVCAN_LGT_ANTCL"), &i);
+	_mode_anti_col = (light_control_mode)i;
+	param_get(param_find("UAVCAN_LGT_STROB"), &i);
+	_mode_strobe = (light_control_mode)i;
+	param_get(param_find("UAVCAN_LGT_NAV"), &i);
+	_mode_nav = (light_control_mode)i;
+	param_get(param_find("UAVCAN_LGT_LAND"), &i);
+	_mode_land = (light_control_mode)i;
+
+
 	// 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));
@@ -51,9 +64,14 @@ int UavcanRGBController::init()
 
 void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
 {
+	bool publish_lights = false;
+	uavcan::equipment::indication::LightsCommand cmds;
+
 	LedControlData led_control_data;
 
 	if (_led_controller.update(led_control_data) == 1) {
+		publish_lights = true;
+
 		// 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::SingleLightCommand cmd;
@@ -113,9 +131,74 @@ void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
 			break;
 		}
 
-		uavcan::equipment::indication::LightsCommand cmds;
 		cmds.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
+			* @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(_mode_anti_col >= 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(_mode_strobe >= 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(_mode_nav >= 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(_mode_land >= 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)
+{
+	// 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;
+
+	color.red = (31.0f * (float)brightness / 255.0f);
+	color.green = (62.0f * (float)brightness / 255.0f);
+	color.blue = (31.0f * (float)brightness / 255.0f);
+
+	return color;
+}

src/drivers/uavcan/rgbled.hpp

@@ -33,6 +33,8 @@
 
 #pragma once
 
+#include <uORB/topics/actuator_armed.h>
+
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/indication/LightsCommand.hpp>
 
@@ -53,6 +55,8 @@ private:
 
 	void periodic_update(const uavcan::TimerEvent &);
 
+	uavcan::equipment::indication::RGB565 brightness_to_rgb565(uint8_t brightness);
+
 	typedef uavcan::MethodBinder<UavcanRGBController *, void (UavcanRGBController::*)(const uavcan::TimerEvent &)>
 	TimerCbBinder;
 
@@ -60,5 +64,20 @@ private:
 	uavcan::Publisher<uavcan::equipment::indication::LightsCommand> _uavcan_pub_lights_cmd;
 	uavcan::TimerEventForwarder<TimerCbBinder> _timer;
 
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+
 	LedController _led_controller;
+
+	// Enum defining light activation condition. Must match UAVCAN_LGT_* param values.
+	enum light_control_mode {
+		ALWAYS_OFF = 0,
+		PREARMED,
+		ARMED,
+		ALWAYS_ON
+	};
+
+	light_control_mode _mode_anti_col{ALWAYS_OFF};
+	light_control_mode _mode_strobe{ALWAYS_OFF};
+	light_control_mode _mode_nav{ALWAYS_OFF};
+	light_control_mode _mode_land{ALWAYS_OFF};
 };

src/drivers/uavcan/uavcan_params.c

@@ -105,3 +105,91 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f);
  * @group UAVCAN
  */
 PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f);
+
+/**
+ * UAVCAN ANTI_COLLISION light operating mode
+ *
+ * This parameter defines the minimum condition under which the system will command
+ * the ANTI_COLLISION 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_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);