AP_Notify: disable buzzer along with external leds

This commit is contained in:
Randy Mackay 2013-11-30 23:27:58 +09:00
parent f8b4187148
commit def5ddb747
2 changed files with 14 additions and 3 deletions

View File

@ -23,8 +23,13 @@
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
bool Buzzer::init() void Buzzer::init()
{ {
// return immediately if disabled
if (!AP_Notify::flags.external_leds) {
return;
}
// setup the pin and ensure it's off // setup the pin and ensure it's off
hal.gpio->pinMode(BUZZER_PIN, GPIO_OUTPUT); hal.gpio->pinMode(BUZZER_PIN, GPIO_OUTPUT);
on(false); on(false);
@ -33,12 +38,16 @@ bool Buzzer::init()
// warning in plane and rover on every boot // warning in plane and rover on every boot
_flags.armed = AP_Notify::flags.armed; _flags.armed = AP_Notify::flags.armed;
_flags.failsafe_battery = AP_Notify::flags.failsafe_battery; _flags.failsafe_battery = AP_Notify::flags.failsafe_battery;
return true;
} }
// update - updates led according to timed_updated. Should be called at 50Hz // update - updates led according to timed_updated. Should be called at 50Hz
void Buzzer::update() void Buzzer::update()
{ {
// return immediately if disabled
if (!AP_Notify::flags.external_leds) {
return;
}
// reduce 50hz call down to 10hz // reduce 50hz call down to 10hz
_counter++; _counter++;
if (_counter < 5) { if (_counter < 5) {

View File

@ -22,6 +22,8 @@
# define BUZZER_PIN 63 // pin 63 on APM1 # define BUZZER_PIN 63 // pin 63 on APM1
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2 #elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define BUZZER_PIN 59 // pin 59 on APM2 # define BUZZER_PIN 59 // pin 59 on APM2
#else
# define BUZZER_PIN 0 // pin undefined on other boards
#endif #endif
class Buzzer class Buzzer
@ -31,7 +33,7 @@ public:
Buzzer() : _counter(0), _pattern(NONE), _pattern_counter(0) {} Buzzer() : _counter(0), _pattern(NONE), _pattern_counter(0) {}
/// init - initialise the buzzer /// init - initialise the buzzer
bool init(void); void init(void);
/// update - updates buzzer according to timed_updated. Should be called at 50Hz /// update - updates buzzer according to timed_updated. Should be called at 50Hz
void update(); void update();