diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index c019bba0a7..5acc7312eb 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -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 } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index b3ca1af17b..60d0cb61eb 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -24,6 +24,7 @@ #include #include #include +#include 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 diff --git a/libraries/AP_Notify/ExternalLED.cpp b/libraries/AP_Notify/ExternalLED.cpp new file mode 100644 index 0000000000..0e777a85e5 --- /dev/null +++ b/libraries/AP_Notify/ExternalLED.cpp @@ -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 . + */ + +#include +#include + +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); + } +} diff --git a/libraries/AP_Notify/ExternalLED.h b/libraries/AP_Notify/ExternalLED.h new file mode 100644 index 0000000000..8d6e77bc00 --- /dev/null +++ b/libraries/AP_Notify/ExternalLED.h @@ -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 . + */ + +#ifndef __EXTERNALLED_H__ +#define __EXTERNALLED_H__ + +#include +#include +#include + +#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__