Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: added abstract Led and RGBLed classes

This commit is contained in:
Staroselskii Georgii 2014-11-14 17:14:40 +03:00 committed by Andrew Tridgell
parent 955753f3f0
commit d40011acf3
22 changed files with 697 additions and 384 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Notify/AP_BoardLED.cpp
View File

@ -19,7 +19,7 @@
extern const AP_HAL::HAL& hal;
void AP_BoardLED::init(void)
bool AP_BoardLED::init(void)
{
// setup the main LEDs as outputs
hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT);
@ -30,6 +30,7 @@ void AP_BoardLED::init(void)
hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
hal.gpio->write(HAL_GPIO_C_LED_PIN, HAL_GPIO_LED_OFF);
return true;
}
/*

5
libraries/AP_Notify/AP_BoardLED.h
View File

@ -20,6 +20,7 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#include "Led.h"
#define HIGH 1
#define LOW 0
@ -70,11 +71,11 @@
#error "Unknown board type in AP_Notify"
#endif
class AP_BoardLED
class AP_BoardLED: public Led
{
public:
// initialise the LED driver
void init(void);
bool init(void);
// should be called at 50Hz
void update(void);

77
libraries/AP_Notify/AP_Notify.cpp
View File

@ -19,52 +19,53 @@
// static flags, to allow for direct class update from device drivers
struct AP_Notify::notify_type AP_Notify::flags;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
AP_BoardLED boardled;
ToshibaLED_PX4 toshibaled;
ToneAlarm_PX4 tonealarm;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &tonealarm};
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
AP_BoardLED boardled;
ExternalLED externalled;
Buzzer buzzer;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &externalled, &buzzer};
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
Buzzer buzzer;
AP_BoardLED boardled;
ToshibaLED_I2C toshibaled;
ExternalLED externalled;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &externalled, &buzzer};
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
AP_BoardLED boardled;
NavioLED_I2C navioled;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &navioled};
#else
AP_BoardLED boardled;
ToshibaLED_I2C toshibaled;
ToneAlarm_Linux tonealarm;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled, &tonealarm};
#endif
#else
AP_BoardLED boardled;
ToshibaLED_I2C toshibaled;
NotifyDevice *AP_Notify::_devices[CONFIG_NOTIFY_DEVICES_COUNT] = {&boardled, &toshibaled};
#endif
// initialisation
void AP_Notify::init(bool enable_external_leds)
{
AP_Notify::flags.external_leds = enable_external_leds;
boardled.init();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
toshibaled.init();
tonealarm.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
toshibaled.init();
tonealarm.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.init();
buzzer.init();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
toshibaled.init();
externalled.init();
buzzer.init();
#endif
for (int i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
_devices[i]->init();
}
}
// main update function, called at 50Hz
void AP_Notify::update(void)
{
boardled.update();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
toshibaled.update();
tonealarm.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
toshibaled.update();
tonealarm.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.update();
buzzer.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
toshibaled.update();
externalled.update();
buzzer.update();
#endif
for (int i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) {
_devices[i]->update();
}
}

36
libraries/AP_Notify/AP_Notify.h
View File

@ -25,9 +25,26 @@
#include <ToshibaLED_PX4.h>
#include <ToneAlarm_PX4.h>
#include <ToneAlarm_Linux.h>
#include <NavioLED_I2C.h>
#include <ExternalLED.h>
#include <Buzzer.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#define CONFIG_NOTIFY_DEVICES_COUNT 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
#define CONFIG_NOTIFY_DEVICES_COUNT 3
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#define CONFIG_NOTIFY_DEVICES_COUNT 4
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#define CONFIG_NOTIFY_DEVICES_COUNT 2
#else
#define CONFIG_NOTIFY_DEVICES_COUNT 3
#endif
#else
#define CONFIG_NOTIFY_DEVICES_COUNT 2
#endif
class AP_Notify
{
public:
@ -63,24 +80,7 @@ public:
void update(void);
private:
// individual drivers
AP_BoardLED boardled;
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
ToshibaLED_PX4 toshibaled;
ToneAlarm_PX4 tonealarm;
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
ToshibaLED_I2C toshibaled;
ToneAlarm_Linux tonealarm;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
ExternalLED externalled;
Buzzer buzzer;
#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
ToshibaLED_I2C toshibaled;
ExternalLED externalled;
Buzzer buzzer;
#else
ToshibaLED_I2C toshibaled;
#endif
static NotifyDevice* _devices[CONFIG_NOTIFY_DEVICES_COUNT];
};
#endif // __AP_NOTIFY_H__

5
libraries/AP_Notify/Buzzer.cpp
View File

@ -23,11 +23,11 @@
extern const AP_HAL::HAL& hal;
void Buzzer::init()
bool Buzzer::init()
{
// return immediately if disabled
if (!AP_Notify::flags.external_leds) {
return;
return false;
}
// setup the pin and ensure it's off
@ -38,6 +38,7 @@ void Buzzer::init()
// warning in plane and rover on every boot
_flags.armed = AP_Notify::flags.armed;
_flags.failsafe_battery = AP_Notify::flags.failsafe_battery;
return true;
}
// update - updates led according to timed_updated. Should be called at 50Hz

6
libraries/AP_Notify/Buzzer.h
View File

@ -30,7 +30,9 @@
#define BUZZER_ARMING_BUZZ_MS 3000 // arming buzz length in milliseconds (i.e. 3 seconds)
class Buzzer
#include "NotifyDevice.h"
class Buzzer: public NotifyDevice
{
public:
/// Constructor
@ -42,7 +44,7 @@ public:
{}
/// init - initialise the buzzer
void init(void);
bool init(void);
/// update - updates buzzer according to timed_updated. Should be called at 50Hz
void update();

5
libraries/AP_Notify/ExternalLED.cpp
View File

@ -20,11 +20,11 @@
extern const AP_HAL::HAL& hal;
void ExternalLED::init(void)
bool ExternalLED::init(void)
{
// return immediately if disabled
if (!AP_Notify::flags.external_leds) {
return;
return false;
}
// setup the main LEDs as outputs
@ -38,6 +38,7 @@ void ExternalLED::init(void)
hal.gpio->write(EXTERNAL_LED_GPS, HAL_GPIO_LED_OFF);
hal.gpio->write(EXTERNAL_LED_MOTOR1, HAL_GPIO_LED_OFF);
hal.gpio->write(EXTERNAL_LED_MOTOR2, HAL_GPIO_LED_OFF);
return true;
}
/*

5
libraries/AP_Notify/ExternalLED.h
View File

@ -21,6 +21,7 @@
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#include "Led.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#define EXTERNAL_LED_ARMED 61 // Armed LED - AN7
@ -44,14 +45,14 @@
#define EXTERNAL_LED_MOTOR2 0
#endif
class ExternalLED
class ExternalLED: public Led
{
public:
// constructor
ExternalLED() : _counter(0), _counter2(0), _pattern(NONE), _pattern_counter(0) {}
// initialise the LED driver
void init(void);
bool init(void);
// should be called at 50Hz
void update(void);

0
libraries/AP_Notify/Led.cpp Normal file
View File

37
libraries/AP_Notify/Led.h Normal file
View File

@ -0,0 +1,37 @@
/*
* AP_Notify Library.
* based upon a prototype library by David "Buzz" Bussenschutt.
*/
/*
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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __LED_H__
#define __LED_H__
#include <AP_HAL.h>
#include "NotifyDevice.h"
class Led: public NotifyDevice {
public:
virtual ~Led() {}
// init - initialised the LED
virtual bool init(void) = 0;
// update - updates led according to timed_updated. Should be
// called at 50Hz
virtual void update() = 0;
};
#endif //__LED_H__

29
libraries/AP_Notify/NavioLED.cpp Normal file
View File

@ -0,0 +1,29 @@
/*
NavioLED 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 "NavioLED.h"
#define NAVIO_LED_BRIGHT 0x0 // full brightness
#define NAVIO_LED_MEDIUM 0x7F // medium brightness
#define NAVIO_LED_DIM 0x4F // dim brightness
#define NAVIO_LED_OFF 0xFF // off
NavioLED::NavioLED() :
RGBLed(NAVIO_LED_OFF, NAVIO_LED_BRIGHT, NAVIO_LED_MEDIUM, NAVIO_LED_DIM)
{
}

31
libraries/AP_Notify/NavioLED.h Normal file
View File

@ -0,0 +1,31 @@
/*
* AP_Notify Library.
* based upon a prototype library by David "Buzz" Bussenschutt.
*/
/*
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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __NAVIO_LED_H__
#define __NAVIO_LED_H__
#include "RGBLed.h"
class NavioLED: public RGBLed {
public:
NavioLED();
};
#endif

88
libraries/AP_Notify/NavioLED_I2C.cpp Normal file
View File

@ -0,0 +1,88 @@
/*
NavioLED I2C 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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL.h>
#include "NavioLED_I2C.h"
#define PCA9685_ADDRESS 0x40
#define PCA9685_PWM 0x6
extern const AP_HAL::HAL& hal;
bool NavioLED_I2C::hw_init()
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
return false;
}
// disable recording of i2c lockup errors
hal.i2c->ignore_errors(true);
// enable the led
bool ret = true;
// re-enable recording of i2c lockup errors
hal.i2c->ignore_errors(false);
// give back i2c semaphore
i2c_sem->give();
return ret;
}
// set_rgb - set color as a combination of red, green and blue values
bool NavioLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// exit immediately if we can't take the semaphore
if (i2c_sem == NULL || !i2c_sem->take(5)) {
return false;
}
uint16_t red_adjusted = red * 0x10;
uint16_t green_adjusted = green * 0x10;
uint16_t blue_adjusted = blue * 0x10;
uint8_t blue_channel_lsb = blue_adjusted & 0xFF;
uint8_t blue_channel_msb = blue_adjusted >> 8;
uint8_t green_channel_lsb = green_adjusted & 0xFF;
uint8_t green_channel_msb = green_adjusted >> 8;
uint8_t red_channel_lsb = red_adjusted & 0xFF;
uint8_t red_channel_msb = red_adjusted >> 8;
uint8_t transaction[] = {0x00, 0x00, blue_channel_lsb, blue_channel_msb,
0x00, 0x00, green_channel_lsb, green_channel_msb,
0x00, 0x00, red_channel_lsb, red_channel_msb
};
bool success = (hal.i2c->writeRegisters(PCA9685_ADDRESS, PCA9685_PWM, sizeof(transaction), transaction) == 0);
// give back i2c semaphore
i2c_sem->give();
return success;
}

29
libraries/AP_Notify/NavioLED_I2C.h Normal file
View File

@ -0,0 +1,29 @@
/*
NavioLED I2C 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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __NAVIO_LED_I2C_H__
#define __NAVIO_LED_I2C_H__
#include "NavioLED.h"
class NavioLED_I2C : public NavioLED
{
protected:
virtual bool hw_init(void);
virtual bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);
};
#endif // __TOSHIBA_LED_I2C_H__

14
libraries/AP_Notify/NotifyDevice.h Normal file
View File

@ -0,0 +1,14 @@
#ifndef __NOTIFYDEVICE_H__
#define __NOTIFYDEVICE_H__
class NotifyDevice {
public:
virtual ~NotifyDevice() {}
// init - initialised the device
virtual bool init(void) = 0;
// update - updates device according to timed_updated. Should be
// called at 50Hz
virtual void update() = 0;
};
#endif

315
libraries/AP_Notify/RGBLed.cpp Normal file
View File

@ -0,0 +1,315 @@
/*
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.h>
#include <AP_GPS.h>
#include "Led.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()
{
_healthy = hw_init();
return _healthy;
}
// 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)
{
// return immediately if not enabled
if (!_healthy) {
return;
}
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;
}
}
}
// _scheduled_update - updates _red, _green, _blue according to notify flags
void RGBLed::update_colours(void)
{
uint8_t brightness = _led_bright;
// slow rate from 50Hz to 10hz
counter++;
if (counter < 5) {
return;
}
// reset counter
counter = 0;
// move forward one step
step++;
if (step >= 10) {
step = 0;
}
// use dim light when connected through USB
if (hal.gpio->usb_connected()) {
brightness = _led_dim;
}
// initialising pattern
if (AP_Notify::flags.initialising) {
if (step & 1) {
// odd steps display red light
_red_des = brightness;
_blue_des = _led_off;
_green_des = _led_off;
} else {
// even display blue light
_red_des = _led_off;
_blue_des = brightness;
_green_des = _led_off;
}
// exit so no other status modify this pattern
return;
}
// save trim and esc calibration pattern
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
switch(step) {
case 0:
case 3:
case 6:
// red on
_red_des = brightness;
_blue_des = _led_off;
_green_des = _led_off;
break;
case 1:
case 4:
case 7:
// blue on
_red_des = _led_off;
_blue_des = brightness;
_green_des = _led_off;
break;
case 2:
case 5:
case 8:
// green on
_red_des = _led_off;
_blue_des = _led_off;
_green_des = brightness;
break;
case 9:
// all off
_red_des = _led_off;
_blue_des = _led_off;
_green_des = _led_off;
break;
}
// exit so no other status modify this pattern
return;
}
// radio and battery failsafe patter: flash yellow
// gps failsafe pattern : flashing yellow and blue
// baro glitching pattern : flashing yellow and purple
// ekf_bad pattern : flashing yellow and red
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching ||
AP_Notify::flags.baro_glitching ||
AP_Notify::flags.ekf_bad) {
switch(step) {
case 0:
case 1:
case 2:
case 3:
case 4:
// yellow on
_red_des = brightness;
_blue_des = _led_off;
_green_des = brightness;
break;
case 5:
case 6:
case 7:
case 8:
case 9:
if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
// blue on for gps failsafe or glitching
_red_des = _led_off;
_blue_des = brightness;
_green_des = _led_off;
} else if (AP_Notify::flags.baro_glitching) {
// purple on if baro glitching
_red_des = brightness;
_blue_des = brightness;
_green_des = _led_off;
} else if (AP_Notify::flags.ekf_bad) {
// red on if ekf bad
_red_des = brightness;
_blue_des = _led_off;
_green_des = _led_off;
}else{
// all off for radio or battery failsafe
_red_des = _led_off;
_blue_des = _led_off;
_green_des = _led_off;
}
break;
}
// exit so no other status modify this pattern
return;
}
// solid green or flashing green 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) {
_red_des = _led_off;
_blue_des = _led_off;
_green_des = brightness;
}else{
// solid blue if armed with no GPS lock
_red_des = _led_off;
_blue_des = brightness;
_green_des = _led_off;
}
return;
}else{
// double flash yellow if failing pre-arm checks
if (!AP_Notify::flags.pre_arm_check) {
switch(step) {
case 0:
case 1:
case 4:
case 5:
// yellow on
_red_des = brightness;
_blue_des = _led_off;
_green_des = brightness;
break;
case 2:
case 3:
case 6:
case 7:
case 8:
case 9:
// all off
_red_des = _led_off;
_blue_des = _led_off;
_green_des = _led_off;
break;
}
}else{
// flashing green if disarmed with GPS 3d lock
// flashing blue if disarmed with no gps lock
switch(step) {
case 0:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 1:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = _led_off;
}
break;
case 2:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 3:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = _led_off;
}
break;
case 4:
_red_des = _led_off;
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
// flashing green if disarmed with GPS 3d lock
_blue_des = _led_off;
_green_des = brightness;
}else{
// flashing blue if disarmed with no gps lock
_blue_des = brightness;
_green_des = _led_off;
}
break;
case 5:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = _led_off;
}
break;
case 6:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 7:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = _led_off;
}
break;
case 8:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 9:
// all off
_red_des = _led_off;
_blue_des = _led_off;
_green_des = _led_off;
break;
}
}
}
}
// update - updates led according to timed_updated. Should be called
// at 50Hz
void RGBLed::update()
{
// return immediately if not enabled
if (!_healthy) {
return;
}
update_colours();
set_rgb(_red_des, _green_des, _blue_des);
}

63
libraries/AP_Notify/RGBLed.h Normal file
View File

@ -0,0 +1,63 @@
/*
* AP_Notify Library.
* based upon a prototype library by David "Buzz" Bussenschutt.
*/
/*
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.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __RGBLED_H__
#define __RGBLED_H__
#include <AP_HAL.h>
#include "Led.h"
class RGBLed: public Led {
public:
RGBLed(uint8_t led_off, uint8_t led_bright, uint8_t led_medium, uint8_t led_dim);
// init - initialised the LED
virtual bool init(void);
// healthy - returns true if the LED is operating properly
virtual bool healthy() { return _healthy; }
// set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15
virtual void set_rgb(uint8_t red, uint8_t green, uint8_t blue);
// update - updates led according to timed_updated. Should be
// called at 50Hz
virtual void update();
protected:
// methods implemented in hardware specific classes
virtual bool hw_init(void) = 0;
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0;
// meta-data common to all hw devices
uint8_t counter;
uint8_t step;
bool _healthy; // true if the LED is operating properly
uint8_t _red_des, _green_des, _blue_des; // color requested by timed update
uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led
uint8_t _led_off;
uint8_t _led_bright;
uint8_t _led_medium;
uint8_t _led_dim;
private:
virtual void update_colours();
};
#endif //__RGBLED_H__

4
libraries/AP_Notify/ToneAlarm_Linux.h
View File

@ -20,7 +20,9 @@
#include "ToneAlarm_Linux.h"
class ToneAlarm_Linux
#include "NotifyDevice.h"
class ToneAlarm_Linux: public NotifyDevice
{
public:
ToneAlarm_Linux():

4
libraries/AP_Notify/ToneAlarm_PX4.h
View File

@ -18,9 +18,9 @@
#ifndef __TONE_ALARM_PX4_H__
#define __TONE_ALARM_PX4_H__
#include "ToneAlarm_PX4.h"
#include "NotifyDevice.h"
class ToneAlarm_PX4
class ToneAlarm_PX4: public NotifyDevice
{
public:
/// init - initialised the tone alarm

287
libraries/AP_Notify/ToshibaLED.cpp
View File

@ -17,288 +17,15 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL.h>
#include <AP_GPS.h>
#include "ToshibaLED.h"
#include "AP_Notify.h"
extern const AP_HAL::HAL& hal;
#define TOSHIBA_LED_BRIGHT 0xFF // full brightness
#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness
#define TOSHIBA_LED_DIM 0x11 // dim
#define TOSHIBA_LED_OFF 0x00 // off
void ToshibaLED::init()
ToshibaLED::ToshibaLED():
RGBLed(TOSHIBA_LED_OFF, TOSHIBA_LED_BRIGHT, TOSHIBA_LED_MEDIUM, TOSHIBA_LED_DIM)
{
_healthy = hw_init();
}
// set_rgb - set color as a combination of red, green and blue values
void ToshibaLED::set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
// return immediately if not enabled
if (!_healthy) {
return;
}
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;
}
}
}
// _scheduled_update - updates _red, _green, _blue according to notify flags
void ToshibaLED::update_colours(void)
{
uint8_t brightness = TOSHIBA_LED_BRIGHT;
// slow rate from 50Hz to 10hz
counter++;
if (counter < 5) {
return;
}
// reset counter
counter = 0;
// move forward one step
step++;
if (step >= 10) {
step = 0;
}
// use dim light when connected through USB
if (hal.gpio->usb_connected()) {
brightness = TOSHIBA_LED_DIM;
}
// initialising pattern
if (AP_Notify::flags.initialising) {
if (step & 1) {
// odd steps display red light
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
} else {
// even display blue light
_red_des = TOSHIBA_LED_OFF;
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
}
// exit so no other status modify this pattern
return;
}
// save trim and esc calibration pattern
if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
switch(step) {
case 0:
case 3:
case 6:
// red on
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
break;
case 1:
case 4:
case 7:
// blue on
_red_des = TOSHIBA_LED_OFF;
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
break;
case 2:
case 5:
case 8:
// green on
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = brightness;
break;
case 9:
// all off
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
break;
}
// exit so no other status modify this pattern
return;
}
// radio and battery failsafe patter: flash yellow
// gps failsafe pattern : flashing yellow and blue
// baro glitching pattern : flashing yellow and purple
// ekf_bad pattern : flashing yellow and red
if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_battery ||
AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching ||
AP_Notify::flags.baro_glitching ||
AP_Notify::flags.ekf_bad) {
switch(step) {
case 0:
case 1:
case 2:
case 3:
case 4:
// yellow on
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF;
_green_des = brightness;
break;
case 5:
case 6:
case 7:
case 8:
case 9:
if (AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching) {
// blue on for gps failsafe or glitching
_red_des = TOSHIBA_LED_OFF;
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
} else if (AP_Notify::flags.baro_glitching) {
// purple on if baro glitching
_red_des = brightness;
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
} else if (AP_Notify::flags.ekf_bad) {
// red on if ekf bad
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
}else{
// all off for radio or battery failsafe
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
}
break;
}
// exit so no other status modify this pattern
return;
}
// solid green or flashing green 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) {
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = brightness;
}else{
// solid blue if armed with no GPS lock
_red_des = TOSHIBA_LED_OFF;
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
}
return;
}else{
// double flash yellow if failing pre-arm checks
if (!AP_Notify::flags.pre_arm_check) {
switch(step) {
case 0:
case 1:
case 4:
case 5:
// yellow on
_red_des = brightness;
_blue_des = TOSHIBA_LED_OFF;
_green_des = brightness;
break;
case 2:
case 3:
case 6:
case 7:
case 8:
case 9:
// all off
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
break;
}
}else{
// flashing green if disarmed with GPS 3d lock
// flashing blue if disarmed with no gps lock
switch(step) {
case 0:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 1:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = TOSHIBA_LED_OFF;
}
break;
case 2:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 3:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = TOSHIBA_LED_OFF;
}
break;
case 4:
_red_des = TOSHIBA_LED_OFF;
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D) {
// flashing green if disarmed with GPS 3d lock
_blue_des = TOSHIBA_LED_OFF;
_green_des = brightness;
}else{
// flashing blue if disarmed with no gps lock
_blue_des = brightness;
_green_des = TOSHIBA_LED_OFF;
}
break;
case 5:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = TOSHIBA_LED_OFF;
}
break;
case 6:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 7:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = TOSHIBA_LED_OFF;
}
break;
case 8:
if (AP_Notify::flags.gps_status >= AP_GPS::GPS_OK_FIX_3D_DGPS) {
_green_des = brightness;
}
break;
case 9:
// all off
_red_des = TOSHIBA_LED_OFF;
_blue_des = TOSHIBA_LED_OFF;
_green_des = TOSHIBA_LED_OFF;
break;
}
}
}
}
// update - updates led according to timed_updated. Should be called
// at 50Hz
void ToshibaLED::update()
{
// return immediately if not enabled
if (!_healthy) {
return;
}
update_colours();
set_rgb(_red_des, _green_des, _blue_des);
}

36
libraries/AP_Notify/ToshibaLED.h
View File

@ -21,41 +21,11 @@
#ifndef __TOSHIBA_LED_H__
#define __TOSHIBA_LED_H__
#include <AP_HAL.h>
#include "RGBLed.h"
#define TOSHIBA_LED_BRIGHT 0xFF // full brightness
#define TOSHIBA_LED_MEDIUM 0x80 // medium brightness
#define TOSHIBA_LED_DIM 0x11 // dim
#define TOSHIBA_LED_OFF 0x00 // off
class ToshibaLED {
class ToshibaLED: public RGBLed {
public:
// init - initialised the LED
void init(void);
// healthy - returns true if the LED is operating properly
bool healthy() { return _healthy; }
// set_rgb - set color as a combination of red, green and blue levels from 0 ~ 15
void set_rgb(uint8_t red, uint8_t green, uint8_t blue);
// update - updates led according to timed_updated. Should be
// called at 50Hz
void update();
protected:
// methods implemented in hardware specific classes
virtual bool hw_init(void) = 0;
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) = 0;
// meta-data common to all hw devices
uint8_t counter;
uint8_t step;
bool _healthy; // true if the LED is operating properly
uint8_t _red_des, _green_des, _blue_des; // color requested by timed update
uint8_t _red_curr, _green_curr, _blue_curr; // current colours displayed by the led
void update_colours();
ToshibaLED();
};
#endif // __TOSHIBA_LED_H__

2
libraries/AP_Notify/ToshibaLED_I2C.cpp
View File

@ -45,7 +45,7 @@ bool ToshibaLED_I2C::hw_init()
bool ret = (hal.i2c->writeRegister(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_ENABLE, 0x03) == 0);
// update the red, green and blue values to zero
uint8_t val[3] = { TOSHIBA_LED_OFF, TOSHIBA_LED_OFF, TOSHIBA_LED_OFF };
uint8_t val[3] = { _led_off, _led_off, _led_off };
ret &= (hal.i2c->writeRegisters(TOSHIBA_LED_ADDRESS, TOSHIBA_LED_PWM0, 3, val) == 0);
// re-enable recording of i2c lockup errors