From d16587340d3bb06ab7bcce36010c85cedfcc3a25 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 15 Sep 2013 17:24:00 +0900 Subject: [PATCH] AP_Notify: add PX4's tone_alarm Sounds added for arming, disarming and low battery events --- libraries/AP_Notify/AP_Notify.cpp | 7 +++ libraries/AP_Notify/AP_Notify.h | 2 + libraries/AP_Notify/ToneAlarm_PX4.cpp | 85 +++++++++++++++++++++++++++ libraries/AP_Notify/ToneAlarm_PX4.h | 45 ++++++++++++++ 4 files changed, 139 insertions(+) create mode 100644 libraries/AP_Notify/ToneAlarm_PX4.cpp create mode 100644 libraries/AP_Notify/ToneAlarm_PX4.h diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index af24d593e9..b0d6de4fa3 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -26,6 +26,9 @@ void AP_Notify::init(void) #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 toshibaled.init(); #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + tonealarm.init(); +#endif } // main update function, called at 50Hz @@ -36,4 +39,8 @@ void AP_Notify::update(void) #if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 toshibaled.update(); #endif + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + tonealarm.update(); +#endif } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 9ad17d096c..09ade53282 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -23,6 +23,7 @@ #include #include #include +#include class AP_Notify { @@ -56,6 +57,7 @@ private: ToshibaLED_I2C toshibaled; #elif CONFIG_HAL_BOARD == HAL_BOARD_PX4 ToshibaLED_PX4 toshibaled; + ToneAlarm_PX4 tonealarm; #endif }; diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp new file mode 100644 index 0000000000..838ec7b8bd --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -0,0 +1,85 @@ +/* + ToneAlarm PX4 driver +*/ +/* + 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 + +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "ToneAlarm_PX4.h" +#include "AP_Notify.h" + +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +bool ToneAlarm_PX4::init() +{ + // open the tone alarm device + _tonealarm_fd = open(TONEALARM_DEVICE_PATH, 0); + if (_tonealarm_fd == -1) { + hal.console->printf("Unable to open " TONEALARM_DEVICE_PATH); + return false; + } + return true; +} + +// play_tune - play one of the pre-defined tunes +bool ToneAlarm_PX4::play_tune(const uint8_t tune_number) +{ + int ret = ioctl(_tonealarm_fd, TONE_SET_ALARM, tune_number); + return (ret == 0); +} + + +// update - updates led according to timed_updated. Should be called at 50Hz +void ToneAlarm_PX4::update() +{ + // exit immediately if we haven't initialised successfully + if (_tonealarm_fd <= 0 ) { + return; + } + + // check if arming status has changed + if (flags.armed != AP_Notify::flags.armed) { + flags.armed = AP_Notify::flags.armed; + if (flags.armed) { + // arming tune + play_tune(TONE_ARMING_WARNING_TUNE); + }else{ + // disarming tune + play_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } + } + + // check if battery status has changed + if (flags.failsafe_battery != AP_Notify::flags.failsafe_battery) { + flags.failsafe_battery = AP_Notify::flags.failsafe_battery; + if (flags.failsafe_battery) { + // low battery warning tune + play_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } + } +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm_PX4.h new file mode 100644 index 0000000000..7a9a70b505 --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_PX4.h @@ -0,0 +1,45 @@ +/* + ToneAlarm PX4 driver +*/ +/* + 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 __TONE_ALARM_PX4_H__ +#define __TONE_ALARM_PX4_H__ + +#include "ToneAlarm_PX4.h" + +class ToneAlarm_PX4 +{ +public: + /// init - initialised the tone alarm + bool init(void); + + /// play_tune - play one of the pre-defined tunes + bool play_tune(const uint8_t tune_number); + + /// update - updates led according to timed_updated. Should be called at 50Hz + void update(); + +private: + int _tonealarm_fd; // file descriptor for the tone alarm + + /// tonealarm_type - bitmask of states we track + struct tonealarm_type { + uint8_t armed : 1; // 0 = disarmed, 1 = armed + uint8_t failsafe_battery : 1; // 1 if battery failsafe + } flags; +}; + +#endif // __TONE_ALARM_PX4_H__