Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-15 20:34:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: ExternalLED library

Browse Source
This commit is contained in:
Robert Lefebvre 2013-11-28 13:00:02 +09:00 committed by Randy Mackay
parent 49ac1a48a5
commit d1cf9f949f
4 changed files with 313 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_Notify/AP_Notify.cpp
View File

@ -23,10 +23,14 @@ struct AP_Notify::notify_type AP_Notify::flags;
void AP_Notify::init(void)
{
boardled.init();
toshibaled.init();
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
tonealarm.init();
#endif
toshibaled.init();
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.init();
#endif
}
// main update function, called at 50Hz
@ -38,4 +42,7 @@ void AP_Notify::update(void)
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
tonealarm.update();
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
externalled.update();
#endif
}

6
libraries/AP_Notify/AP_Notify.h
View File

@ -24,6 +24,7 @@
#include <ToshibaLED_I2C.h>
#include <ToshibaLED_PX4.h>
#include <ToneAlarm_PX4.h>
#include <ExternalLED.h>
class AP_Notify
{
@ -39,7 +40,7 @@ public:
uint16_t esc_calibration : 1; // 1 if calibrating escs
uint16_t failsafe_radio : 1; // 1 if radio failsafe
uint16_t failsafe_battery : 1; // 1 if battery failsafe
uint16_t failsafe_gps : 1; // 1 if gps failsafe
uint16_t failsafe_gps : 1; // 1 if gps failsafe
};
// the notify flags are static to allow direct class access
@ -58,6 +59,9 @@ private:
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
ToshibaLED_PX4 toshibaled;
ToneAlarm_PX4 tonealarm;
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
ToshibaLED_I2C toshibaled;
ExternalLED externalled;
#else
ToshibaLED_I2C toshibaled;
#endif

215
libraries/AP_Notify/ExternalLED.cpp Normal file
View File

@ -0,0 +1,215 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 <AP_Notify.h>
extern const AP_HAL::HAL& hal;
void ExternalLED::init(void)
{
// setup the main LEDs as outputs
hal.gpio->pinMode(EXTERNAL_LED_ARMED, GPIO_OUTPUT);
hal.gpio->pinMode(EXTERNAL_LED_GPS, GPIO_OUTPUT);
hal.gpio->pinMode(EXTERNAL_LED_MOTOR1, GPIO_OUTPUT);
hal.gpio->pinMode(EXTERNAL_LED_MOTOR2, GPIO_OUTPUT);
// turn leds off
hal.gpio->write(EXTERNAL_LED_ARMED, HAL_GPIO_LED_OFF);
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);
}
/*
main update function called at 50Hz
*/
void ExternalLED::update(void)
{
// reduce update rate from 50hz to 10hz
_counter++;
if (_counter < 5) {
return;
}
_counter = 0;
// internal counter used to control step of armed and gps led
_counter2++;
if (_counter2 >= 10) {
_counter2 = 0;
}
// arming led control
if (AP_Notify::flags.armed) {
armed_led(true);
}else{
// blink arming lede at 2hz
switch(_counter2) {
case 0:
case 1:
case 2:
case 5:
case 6:
case 7:
armed_led(false);
break;
case 3:
case 4:
case 8:
case 9:
armed_led(true);
break;
}
}
// GPS led control
switch (AP_Notify::flags.gps_status) {
case 0:
// no GPS attached
gps_led(false);
break;
case 1:
case 2:
// GPS attached but no lock, blink at 4Hz
switch(_counter2) { // Pattern: 3(off), 2(on), 3(off), 2(on), repeat
case 0:
case 1:
case 2:
case 5:
case 6:
case 7:
gps_led(false);
break;
case 3:
case 4:
case 8:
case 9:
gps_led(true);
break;
}
break;
case 3:
// solid blue on gps lock
gps_led(true);
break;
}
// motor led control
// if we are displaying a pattern complete it
if (_pattern != NONE) {
_pattern_counter++;
switch(_pattern) {
case NONE:
// do nothing
break;
case FAST_FLASH:
switch(_pattern_counter) {
case 1:
case 3:
case 5:
case 7:
case 9:
motor_led1(true);
motor_led2(true);
break;
case 2:
case 4:
case 6:
case 8:
motor_led1(false);
motor_led2(false);
break;
case 10:
motor_led1(false);
motor_led2(false);
set_pattern(NONE);
break;
}
break;
case OSCILLATE:
switch(_pattern_counter) {
case 1:
motor_led1(true);
motor_led2(false);
break;
case 4:
motor_led1(false);
motor_led2(true);
break;
case 6:
set_pattern(NONE);
break;
}
break;
}
}else{
if (AP_Notify::flags.failsafe_battery || AP_Notify::flags.failsafe_radio) {
// radio or battery failsafe indicated by fast flashing
set_pattern(FAST_FLASH);
}else if(AP_Notify::flags.failsafe_gps || AP_Notify::flags.gps_glitching)
// gps failsafe indicated by oscillating
set_pattern(OSCILLATE);
else{
// otherwise do whatever the armed led is doing
motor_led1(_flags.armedled_on);
motor_led2(_flags.armedled_on);
}
}
}
// set_pattern - sets pattern for motor leds
void ExternalLED::set_pattern(LEDPattern pattern_id)
{
_pattern = pattern_id;
_pattern_counter = 0;
}
// armed_led - set armed light on or off
void ExternalLED::armed_led(bool on_off)
{
if (_flags.armedled_on != on_off) {
_flags.armedled_on = on_off;
hal.gpio->write(EXTERNAL_LED_ARMED, _flags.armedled_on);
}
}
// gps_led - set gps light on or off
void ExternalLED::gps_led(bool on_off)
{
if (_flags.gpsled_on != on_off) {
_flags.gpsled_on = on_off;
hal.gpio->write(EXTERNAL_LED_GPS, _flags.gpsled_on);
}
}
// motor_led - set motor light on or off
void ExternalLED::motor_led1(bool on_off)
{
if (_flags.motorled1_on != on_off) {
_flags.motorled1_on = on_off;
hal.gpio->write(EXTERNAL_LED_MOTOR1, _flags.motorled1_on);
}
}
// motor_led - set motor light on or off
void ExternalLED::motor_led2(bool on_off)
{
if (_flags.motorled2_on != on_off) {
_flags.motorled2_on = on_off;
hal.gpio->write(EXTERNAL_LED_MOTOR2, _flags.motorled2_on);
}
}

85
libraries/AP_Notify/ExternalLED.h Normal file
View File

@ -0,0 +1,85 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
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 __EXTERNALLED_H__
#define __EXTERNALLED_H__
#include <AP_Common.h>
#include <AP_HAL.h>
#include <AP_Param.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
#define EXTERNAL_LED_ARMED 61 // Armed LED - AN7
#define EXTERNAL_LED_GPS 60 // GPS LED - AN6
#define EXTERNAL_LED_MOTOR1 58 // Motor1 LED - AN4
#define EXTERNAL_LED_MOTOR2 62 // Motor2 LED - AN8
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM1
#define EXTERNAL_LED_GPS 64 // GPS LED - AN10
#define EXTERNAL_LED_ARMED 65 // Armed LED - AN11
#define EXTERNAL_LED_MOTOR1 62 // Motor1 LED - AN8
#define EXTERNAL_LED_MOTOR2 66 // Motor2 LED - AN12
#endif
class ExternalLED
{
public:
// constructor
ExternalLED() : _counter(0), _counter2(0), _pattern(NONE), _pattern_counter(0) {}
// initialise the LED driver
void init(void);
// should be called at 50Hz
void update(void);
private:
enum LEDPattern {
NONE = 0,
FAST_FLASH = 1,
OSCILLATE = 2
};
/// buzzer_flag_type - bitmask of current state and ap_notify states we track
struct copterleds_flag_type {
// individual led status
uint8_t armedled_on : 1; // 1 if the armed led is currently on
uint8_t gpsled_on : 1; // 1 if the gps led is currently on
uint8_t motorled1_on : 1; // 1 if motor led #1 is currently on
uint8_t motorled2_on : 1; // 1 if motor led #2 is currently on
} _flags;
uint8_t _counter; // reduces 50hz calls to 10hz
uint8_t _counter2; // used to control steps of arming and gps leds
LEDPattern _pattern; // current pattern for motor leds
uint8_t _pattern_counter; // used to time on/off of current patter
// armed_led - set armed light on or off
void armed_led(bool on_off);
// gps_led - set gps light on or off
void gps_led(bool on_off);
// set_pattern - sets pattern for motor leds
void set_pattern(LEDPattern pattern_id);
// motor_led1, motor_led2 - set motor lights on or off
void motor_led1(bool on_off);
void motor_led2(bool on_off);
};
#endif // __EXTERNALLED_H__