diff --git a/src/drivers/uavcan/rgbled.cpp b/src/drivers/uavcan/rgbled.cpp index 40816aff41..095c1697c5 100644 --- a/src/drivers/uavcan/rgbled.cpp +++ b/src/drivers/uavcan/rgbled.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "rgbled.hpp" +#include 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; +} diff --git a/src/drivers/uavcan/rgbled.hpp b/src/drivers/uavcan/rgbled.hpp index abde33990f..966bde1498 100644 --- a/src/drivers/uavcan/rgbled.hpp +++ b/src/drivers/uavcan/rgbled.hpp @@ -33,6 +33,8 @@ #pragma once +#include + #include #include @@ -53,6 +55,8 @@ private: void periodic_update(const uavcan::TimerEvent &); + uavcan::equipment::indication::RGB565 brightness_to_rgb565(uint8_t brightness); + typedef uavcan::MethodBinder TimerCbBinder; @@ -60,5 +64,20 @@ private: uavcan::Publisher _uavcan_pub_lights_cmd; uavcan::TimerEventForwarder _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}; }; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 885c45a4f4..a005906ef3 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/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);