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

Extend UAVCAN light control

Browse Source 
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.
This commit is contained in:
Avionics Anonymous
2020-10-25 11:02:05 -04:00
committed by Lorenz Meier
parent 67a0e1993a
commit cc96468fad
3 changed files with 191 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/rgbled.cpp
+84 -1
View File
@@ -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;
}