dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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.
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/drivers/uavcan/rgbled.cpp
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;
}

19
src/drivers/uavcan/rgbled.hpp
View File

@ -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};
};

88
src/drivers/uavcan/uavcan_params.c
View File

@ -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);