diff --git a/libraries/AP_Notify/AP_BoardLED2.cpp b/libraries/AP_Notify/AP_BoardLED2.cpp
new file mode 100644
index 0000000000..79fcfcc1db
--- /dev/null
+++ b/libraries/AP_Notify/AP_BoardLED2.cpp
@@ -0,0 +1,265 @@
+/*
+ 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 "AP_BoardLED2.h"
+
+#include "AP_Notify.h"
+
+
+// show all status on only 2 leds
+
+#if defined(HAL_GPIO_A_LED_PIN) && defined(HAL_GPIO_B_LED_PIN)
+
+extern const AP_HAL::HAL& hal;
+
+bool AP_BoardLED2::init(void)
+{
+ // setup the main LEDs as outputs
+ hal.gpio->pinMode(HAL_GPIO_A_LED_PIN, HAL_GPIO_OUTPUT);
+ hal.gpio->pinMode(HAL_GPIO_B_LED_PIN, HAL_GPIO_OUTPUT);
+
+ // turn all lights off
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+
+ _sat_cnt=0;
+ save_trim_counter = 0;
+ arm_counter = 0;
+ return true;
+}
+
+/*
+ main update function called at 50Hz
+ */
+void AP_BoardLED2::update(void)
+{
+ _counter++;
+
+ // we never want to update LEDs at a higher than 16Hz rate
+ if (_counter % 3 != 0) {
+ return;
+ }
+
+ // counter2 used to drop frequency down to 16hz
+ uint8_t counter2 = _counter / 3;
+
+ // initialising
+ if (AP_Notify::flags.initialising) {
+ // blink LEDs A at 8Hz (full cycle) during initialisation
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, (counter2 & 1) ? HAL_GPIO_LED_ON : HAL_GPIO_LED_OFF);
+ return;
+ }
+
+ if ((counter2 & 0x2) == 0) {
+ save_trim_counter++;
+ }
+
+ bool led_a_used=false;
+
+ // save trim and ESC calibration
+ if (AP_Notify::flags.save_trim || AP_Notify::flags.esc_calibration) {
+ switch(save_trim_counter) {
+ case 0:
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+
+ case 1:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+
+ default:
+ save_trim_counter = -1;
+ break;
+ }
+ return;
+ }
+
+ if(AP_Notify::flags.compass_cal_running){ // compass calibration
+ switch(save_trim_counter) {
+ case 0:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short blinks by both LEDs
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+ case 1:
+ case 2:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+ case 3:
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ break;
+ default:
+ // reset counter to restart the sequence
+ save_trim_counter = -1;
+ break;
+ }
+ return;
+
+ }
+
+ if(AP_Notify::events.autotune_complete){
+ switch(save_trim_counter) {
+ case 0:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short darkening
+ break;
+ case 1:
+ case 2:
+ case 3:
+ case 4:
+ case 5:
+ case 6:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+ case 7:
+ break;
+ default:
+ // reset counter to restart the sequence
+ save_trim_counter = -1;
+ break;
+ }
+ led_a_used=true;
+ }
+
+ if(AP_Notify::events.autotune_failed){
+ switch(save_trim_counter) {
+ case 0:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON); // short double darkening
+ break;
+ case 1:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+ case 2:
+ case 3:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+ default:
+ // reset counter to restart the sequence
+ save_trim_counter = -1;
+ break;
+ }
+ led_a_used=true;
+ }
+
+ // arming light
+ if(!led_a_used) {
+ if (AP_Notify::flags.armed) {
+ if(AP_Notify::flags.failsafe_battery){// blink slowly (around 2Hz)
+ if ((counter2 & 0x7) == 0) {
+ hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
+ }
+ }else if(AP_Notify::flags.failsafe_radio){// blink fast (around 4Hz)
+ if ((counter2 & 0x3) == 0) {
+ hal.gpio->toggle(HAL_GPIO_A_LED_PIN);
+ }
+ } else {
+ // ARM led solid
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ }
+ }else{
+ if ((counter2 & 0x2) == 0) {
+ arm_counter++;
+ }
+
+ if (AP_Notify::flags.pre_arm_check) {
+ // passed pre-arm checks so slower single flash
+ switch(arm_counter) {
+ case 0:
+ case 1:
+ case 2:
+ case 3:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+ default:
+ // reset counter to restart the sequence
+ arm_counter = -1;
+ break;
+ }
+ }else{
+ // failed pre-arm checks so double flash
+ switch(arm_counter) {
+ case 0:
+ case 1:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+
+ case 2:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+
+
+ case 3:
+ case 4:
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_ON);
+ break;
+
+ case 5:
+ case 6:
+ case 7: // add one tick to do period be a multiple of the second
+ hal.gpio->write(HAL_GPIO_A_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+
+ default:
+ arm_counter = -1;
+ break;
+ }
+ }
+ }
+ }
+ // gps light
+ switch (AP_Notify::flags.gps_status) {
+ case 0:
+ case 1:
+ // no GPS attached or no lock - be dark
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+ break;
+
+ case 2: // 2d lock
+ case 3:// 3d lock
+ default: // show number of sats
+ if ((counter2 & 0x2) == 0) {
+ _sat_cnt++;
+ }
+ uint16_t sats = AP_Notify::flags.gps_num_sats;
+
+ if(_sat_cnt<8) { // pause between pulses
+ hal.gpio->write(HAL_GPIO_B_LED_PIN, HAL_GPIO_LED_OFF);
+ } else if(_sat_cnt< (8 + (sats-6)*2) ) {
+ hal.gpio->toggle(HAL_GPIO_B_LED_PIN); // 2Hz
+ } else {
+ _sat_cnt=-1;
+ }
+ break;
+ }
+}
+#else
+bool AP_BoardLED2::init(void) {return true;}
+void AP_BoardLED2::update(void) {return;}
+#endif
diff --git a/libraries/AP_Notify/AP_BoardLED2.h b/libraries/AP_Notify/AP_BoardLED2.h
new file mode 100644
index 0000000000..8b0ab50c7a
--- /dev/null
+++ b/libraries/AP_Notify/AP_BoardLED2.h
@@ -0,0 +1,42 @@
+/*
+ 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 .
+ */
+#pragma once
+
+
+
+#include
+#include
+
+#include "NotifyDevice.h"
+
+#define HIGH 1
+#define LOW 0
+
+class AP_BoardLED2: public NotifyDevice
+{
+public:
+ // initialise the LED driver
+ bool init(void);
+
+ // should be called at 50Hz
+ void update(void);
+
+private:
+ // counter incremented at 50Hz
+ uint8_t _counter;
+ uint16_t _sat_cnt;
+ uint8_t save_trim_counter;
+ uint8_t arm_counter = 0;
+};
diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 32da266ee2..8230fe8bdb 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -34,6 +34,7 @@
#include "Led_Sysfs.h"
#include "UAVCAN_RGB_LED.h"
#include
+#include "AP_BoardLED2.h"
extern const AP_HAL::HAL& hal;
@@ -61,6 +62,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
// @User: Advanced
AP_GROUPINFO("BUZZ_ENABLE", 1, AP_Notify, _buzzer_enable, BUZZER_ON),
+
// @Param: LED_OVERRIDE
// @DisplayName: Setup for MAVLink LED override
// @Description: This sets up the board RGB LED for override by MAVLink. Normal notify LED control is disabled
@@ -82,6 +84,15 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
// @User: Advanced
AP_GROUPINFO("OREO_THEME", 4, AP_Notify, _oreo_theme, 0),
+#if !defined(BUZZER_PIN)
+ // @Param: BUZZ_PIN
+ // @DisplayName: Buzzer pin
+ // @Description: Enables to connect active buzzer to arbitrary pin. Requires 3-pin buzzer or additional MOSFET!
+ // @Values: 0:Disabled, or pin number
+ // @User: Advanced
+ AP_GROUPINFO("BUZZ_PIN", 5, AP_Notify, _buzzer_pin, 0),
+#endif
+
AP_GROUPEND
};
@@ -235,7 +246,7 @@ void AP_Notify::add_backends(void)
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
ADD_BACKEND(new Display());
ADD_BACKEND(new Buzzer());
-// ADD_BACKEND(new AP_BoardLED2()); // needs AP_BoardLED2 in master
+ ADD_BACKEND(new AP_BoardLED2()); // needs AP_BoardLED2 in master
#else
ADD_BACKEND(new AP_BoardLED());
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 2e0fe3d3c6..5741e36a59 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -143,8 +143,10 @@ public:
const char* get_text() const { return _send_text; }
static const struct AP_Param::GroupInfo var_info[];
+ uint8_t get_buzz_pin() const { return _buzzer_pin; }
private:
+
static AP_Notify *_instance;
// parameters
@@ -153,6 +155,7 @@ private:
AP_Int8 _buzzer_enable;
AP_Int8 _display_type;
AP_Int8 _oreo_theme;
+ AP_Int8 _buzzer_pin;
char _send_text[NOTIFY_TEXT_BUFFER_SIZE];
uint32_t _send_text_updated_millis; // last time text changed
diff --git a/libraries/AP_Notify/Buzzer.cpp b/libraries/AP_Notify/Buzzer.cpp
index 89f35fb896..f0b71ee829 100644
--- a/libraries/AP_Notify/Buzzer.cpp
+++ b/libraries/AP_Notify/Buzzer.cpp
@@ -21,17 +21,32 @@
#include "AP_Notify.h"
+#ifndef HAL_BUZZER_ON
+ #define HAL_BUZZER_ON 1
+ #define HAL_BUZZER_OFF 0
+#endif
+
+
+
extern const AP_HAL::HAL& hal;
+
bool Buzzer::init()
{
- // return immediately if disabled
- if (!AP_Notify::flags.external_leds) {
- return false;
- }
+ // this is for leds, not for buzzer!
+// if (!AP_Notify::flags.external_leds) {
+// return false;
+// }
+
+#if defined(BUZZER_PIN)
+ _pin = BUZZER_PIN;
+#else
+ _pin = pNotify->get_buzz_pin();
+#endif
+ if(!_pin) return false;
// setup the pin and ensure it's off
- hal.gpio->pinMode(BUZZER_PIN, HAL_GPIO_OUTPUT);
+ hal.gpio->pinMode(_pin, HAL_GPIO_OUTPUT);
on(false);
// set initial boot states. This prevents us issuing a arming
@@ -208,7 +223,7 @@ void Buzzer::on(bool turn_on)
_flags.on = turn_on;
// pull pin high or low
- hal.gpio->write(BUZZER_PIN, _flags.on);
+ hal.gpio->write(_pin, _flags.on? HAL_BUZZER_ON : HAL_BUZZER_OFF);
}
/// play_pattern - plays the defined buzzer pattern
@@ -217,3 +232,4 @@ void Buzzer::play_pattern(BuzzerPattern pattern_id)
_pattern = pattern_id;
_pattern_counter = 0;
}
+
diff --git a/libraries/AP_Notify/Buzzer.h b/libraries/AP_Notify/Buzzer.h
index 0d25f5cfcf..d807b8da00 100644
--- a/libraries/AP_Notify/Buzzer.h
+++ b/libraries/AP_Notify/Buzzer.h
@@ -19,14 +19,17 @@
#include
-#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#ifndef BUZZER_PIN
+// better to define it in hal
+ #if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
# define BUZZER_PIN 32
-#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
+ #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
# define BUZZER_PIN 11 // GPIO P8_32
-#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
+ #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
# define BUZZER_PIN 28 // GPIO P2_8
-#else
- # define BUZZER_PIN 0 // pin undefined on other boards
+// #else
+// # define BUZZER_PIN -1 // pin undefined on other boards
+ #endif
#endif
#define BUZZER_ARMING_BUZZ_MS 3000 // arming buzz length in milliseconds (i.e. 3 seconds)
@@ -82,4 +85,5 @@ private:
BuzzerPattern _pattern; // current pattern
uint8_t _pattern_counter; // used to time on/off of current patter
uint32_t _arming_buzz_start_ms; // arming_buzz start time in milliseconds
+ uint8_t _pin;
};