mirror of https://github.com/ArduPilot/ardupilot
257 lines
6.9 KiB
C++
257 lines
6.9 KiB
C++
/*
|
|
Generic RGBLed driver
|
|
*/
|
|
|
|
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
*/
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include "RGBLed.h"
|
|
#include "AP_Notify.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
RGBLed::RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim):
|
|
_led_off(led_off),
|
|
_led_bright(led_bright),
|
|
_led_medium(led_medium),
|
|
_led_dim(led_dim)
|
|
{
|
|
|
|
}
|
|
|
|
bool RGBLed::init()
|
|
{
|
|
return hw_init();
|
|
}
|
|
|
|
// set_rgb - set color as a combination of red, green and blue values
|
|
void RGBLed::_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
|
|
{
|
|
if (red != _red_curr ||
|
|
green != _green_curr ||
|
|
blue != _blue_curr) {
|
|
// call the hardware update routine
|
|
if (hw_set_rgb(red, green, blue)) {
|
|
_red_curr = red;
|
|
_green_curr = green;
|
|
_blue_curr = blue;
|
|
}
|
|
}
|
|
}
|
|
|
|
RGBLed::rgb_source_t RGBLed::rgb_source() const
|
|
{
|
|
return rgb_source_t(pNotify->_rgb_led_override.get());
|
|
}
|
|
|
|
// set_rgb - set color as a combination of red, green and blue values
|
|
void RGBLed::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
|
|
{
|
|
if (rgb_source() == mavlink) {
|
|
// don't set if in override mode
|
|
return;
|
|
}
|
|
_set_rgb(red, green, blue);
|
|
}
|
|
|
|
uint8_t RGBLed::get_brightness(void) const
|
|
{
|
|
uint8_t brightness = _led_bright;
|
|
|
|
switch (pNotify->_rgb_led_brightness) {
|
|
case RGB_LED_OFF:
|
|
brightness = _led_off;
|
|
break;
|
|
case RGB_LED_LOW:
|
|
brightness = _led_dim;
|
|
break;
|
|
case RGB_LED_MEDIUM:
|
|
brightness = _led_medium;
|
|
break;
|
|
case RGB_LED_HIGH:
|
|
brightness = _led_bright;
|
|
break;
|
|
}
|
|
|
|
// use dim light when connected through USB
|
|
if (hal.gpio->usb_connected() && brightness > _led_dim) {
|
|
brightness = _led_dim;
|
|
}
|
|
return brightness;
|
|
}
|
|
|
|
uint32_t RGBLed::get_colour_sequence_obc(void) const
|
|
{
|
|
if (AP_Notify::flags.armed) {
|
|
return DEFINE_COLOUR_SEQUENCE_SOLID(RED);
|
|
}
|
|
return DEFINE_COLOUR_SEQUENCE_SOLID(GREEN);
|
|
}
|
|
|
|
// _scheduled_update - updates _red, _green, _blue according to notify flags
|
|
uint32_t RGBLed::get_colour_sequence(void) const
|
|
{
|
|
// initialising pattern
|
|
if (AP_Notify::flags.initialising) {
|
|
return sequence_initialising;
|
|
}
|
|
|
|
// save trim and esc calibration pattern
|
|
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
|
|
return sequence_trim_or_esc;
|
|
}
|
|
|
|
// radio and battery failsafe patter: flash yellow
|
|
// gps failsafe pattern : flashing yellow and blue
|
|
// ekf_bad pattern : flashing yellow and red
|
|
if (AP_Notify::flags.failsafe_radio ||
|
|
AP_Notify::flags.failsafe_battery ||
|
|
AP_Notify::flags.ekf_bad ||
|
|
AP_Notify::flags.gps_glitching ||
|
|
AP_Notify::flags.leak_detected) {
|
|
|
|
if (AP_Notify::flags.leak_detected) {
|
|
// purple if leak detected
|
|
return sequence_failsafe_leak;
|
|
} else if (AP_Notify::flags.ekf_bad) {
|
|
// red on if ekf bad
|
|
return sequence_failsafe_ekf;
|
|
} else if (AP_Notify::flags.gps_glitching) {
|
|
// blue on gps glitch
|
|
return sequence_failsafe_gps_glitching;
|
|
}
|
|
// all off for radio or battery failsafe
|
|
return sequence_failsafe_radio_or_battery;
|
|
}
|
|
|
|
// solid green or blue if armed
|
|
if (AP_Notify::flags.armed) {
|
|
// solid green if armed with GPS 3d lock
|
|
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
|
|
return sequence_armed;
|
|
}
|
|
// solid blue if armed with no GPS lock
|
|
return sequence_armed_nogps;
|
|
}
|
|
|
|
// double flash yellow if failing pre-arm checks
|
|
if (!AP_Notify::flags.pre_arm_check) {
|
|
return sequence_prearm_failing;
|
|
}
|
|
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS && AP_Notify::flags.pre_arm_gps_check) {
|
|
return sequence_disarmed_good_dgps;
|
|
}
|
|
|
|
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D && AP_Notify::flags.pre_arm_gps_check) {
|
|
return sequence_disarmed_good_gps;
|
|
}
|
|
|
|
return sequence_disarmed_bad_gps;
|
|
}
|
|
|
|
// update - updates led according to timed_updated. Should be called
|
|
// at 50Hz
|
|
void RGBLed::update()
|
|
{
|
|
uint32_t current_colour_sequence = 0;
|
|
|
|
switch (rgb_source()) {
|
|
case mavlink:
|
|
update_override();
|
|
return; // note this is a return not a break!
|
|
case standard:
|
|
current_colour_sequence = get_colour_sequence();
|
|
break;
|
|
case obc:
|
|
current_colour_sequence = get_colour_sequence_obc();
|
|
break;
|
|
}
|
|
|
|
const uint8_t brightness = get_brightness();
|
|
|
|
uint8_t step = (AP_HAL::millis()/100) % 10;
|
|
|
|
// ensure we can't skip a step even with awful timing
|
|
if (step != last_step) {
|
|
step = (last_step+1) % 10;
|
|
last_step = step;
|
|
}
|
|
|
|
const uint8_t colour = (current_colour_sequence >> (step*3)) & 7;
|
|
|
|
_red_des = (colour & RED) ? brightness : 0;
|
|
_green_des = (colour & GREEN) ? brightness : 0;
|
|
_blue_des = (colour & BLUE) ? brightness : 0;
|
|
|
|
set_rgb(_red_des, _green_des, _blue_des);
|
|
}
|
|
|
|
/*
|
|
handle LED control, only used when LED_OVERRIDE=1
|
|
*/
|
|
void RGBLed::handle_led_control(mavlink_message_t *msg)
|
|
{
|
|
if (rgb_source() != mavlink) {
|
|
// ignore LED_CONTROL commands if not in LED_OVERRIDE mode
|
|
return;
|
|
}
|
|
|
|
// decode mavlink message
|
|
mavlink_led_control_t packet;
|
|
mavlink_msg_led_control_decode(msg, &packet);
|
|
|
|
_led_override.start_ms = AP_HAL::millis();
|
|
|
|
switch (packet.custom_len) {
|
|
case 3:
|
|
_led_override.rate_hz = 0;
|
|
_led_override.r = packet.custom_bytes[0];
|
|
_led_override.g = packet.custom_bytes[1];
|
|
_led_override.b = packet.custom_bytes[2];
|
|
break;
|
|
case 4:
|
|
_led_override.rate_hz = packet.custom_bytes[3];
|
|
_led_override.r = packet.custom_bytes[0];
|
|
_led_override.g = packet.custom_bytes[1];
|
|
_led_override.b = packet.custom_bytes[2];
|
|
break;
|
|
default:
|
|
// not understood
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*
|
|
update LED when in override mode
|
|
*/
|
|
void RGBLed::update_override(void)
|
|
{
|
|
if (_led_override.rate_hz == 0) {
|
|
// solid colour
|
|
_set_rgb(_led_override.r, _led_override.g, _led_override.b);
|
|
return;
|
|
}
|
|
// blinking
|
|
uint32_t ms_per_cycle = 1000 / _led_override.rate_hz;
|
|
uint32_t cycle = (AP_HAL::millis() - _led_override.start_ms) % ms_per_cycle;
|
|
if (cycle > ms_per_cycle / 2) {
|
|
// on
|
|
_set_rgb(_led_override.r, _led_override.g, _led_override.b);
|
|
} else {
|
|
_set_rgb(0, 0, 0);
|
|
}
|
|
}
|