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

full system status on 2 LEDs + buzzer pin as parameter

This commit is contained in:
night-ghost 2018-01-22 14:11:14 +05:00 committed by Andrew Tridgell
parent 0a08bba437
commit 64c2e691f8
6 changed files with 353 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

265
libraries/AP_Notify/AP_BoardLED2.cpp Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

42
libraries/AP_Notify/AP_BoardLED2.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#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;
};

13
libraries/AP_Notify/AP_Notify.cpp
View File

@ -34,6 +34,7 @@
#include "Led_Sysfs.h"
#include "UAVCAN_RGB_LED.h"
#include <stdio.h>
#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));

3
libraries/AP_Notify/AP_Notify.h
View File

@ -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

28
libraries/AP_Notify/Buzzer.cpp
View File

@ -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;
}

14
libraries/AP_Notify/Buzzer.h
View File

@ -19,14 +19,17 @@
#include <AP_HAL/AP_HAL.h>
#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;
};