From a7f959e6f9abab61864119009b4f0e690db3f374 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 22 Jan 2016 12:03:07 -0800 Subject: [PATCH] AP_Notify: add ToneAlarm_PX4_Solo --- libraries/AP_Notify/AP_Notify.cpp | 10 +- libraries/AP_Notify/AP_Notify.h | 9 + libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp | 335 +++++++++++++++++++++ libraries/AP_Notify/ToneAlarm_PX4_Solo.h | 79 +++++ 4 files changed, 431 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp create mode 100644 libraries/AP_Notify/ToneAlarm_PX4_Solo.h diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 366294a0f1..ebbdde171f 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -25,6 +25,7 @@ #include "RCOutputRGBLed.h" #include "ToneAlarm_Linux.h" #include "ToneAlarm_PX4.h" +#include "ToneAlarm_PX4_Solo.h" #include "ToshibaLED.h" #include "ToshibaLED_I2C.h" #include "ToshibaLED_PX4.h" @@ -64,9 +65,14 @@ struct AP_Notify::notify_events_type AP_Notify::events; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 AP_BoardLED boardled; ToshibaLED_PX4 toshibaled; - ToneAlarm_PX4 tonealarm; -#ifdef OREOLED_ENABLED +#if AP_NOTIFY_SOLO_TONES == 1 + ToneAlarm_PX4_Solo tonealarm; +#else + ToneAlarm_PX4 tonealarm; +#endif + +#if AP_NOTIFY_OREOLED == 1 OreoLED_PX4 oreoled; NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &oreoled}; #else diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index c03d5d1a5d..8b67a42b72 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -22,6 +22,15 @@ #include "NotifyDevice.h" + +#ifndef AP_NOTIFY_OREOLED +#define AP_NOTIFY_OREOLED 0 +#endif + +#ifndef AP_NOTIFY_SOLO_TONES +#define AP_NOTIFY_SOLO_TONES 0 +#endif + // Device parameters values #define RGB_LED_OFF 0 #define RGB_LED_LOW 1 diff --git a/libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp b/libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp new file mode 100644 index 0000000000..842d40a2d7 --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp @@ -0,0 +1,335 @@ +/* + 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_Solo.h" +#include "AP_Notify.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const ToneAlarm_PX4_Solo::Tone ToneAlarm_PX4_Solo::_tones[] { + #define AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK 0 + { "MFMST100L64O2gg-feP64ee-dd-", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK 1 + { "MFMST100L64O2gg-feP64ee-dd-", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2 + { "MFMLT200L64O3f>cc", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK 3 + { "MFMLT200L64O3f>cc", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4 + { "MFMST200L32O2g>cef#gb", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK 5 + { "MFMST200L32O2g>cef#gb", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED 6 + { "MFMLT200L32O3cdef#gf#P32g>cc", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED 7 + { "MFMLT200L32O3cdef#gf#P32g>cc", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED 8 + { "MFT100L4>B#B#B#B#", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING 9 + { "MFMLT220L128O1cc#dd#eff#gg#aa#b>cc#dd#eff#gcc#dd#eff#gg#aa#b>cc#dd#eff#gcc#dd#eff#gcc#dd#eff#gg#aa#bP8L32cg>c", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_DISARMED 10 + { "MFMLT220L128O3cc#dd#eff#gcc#dd#eff#gP32cc#dd#eff#gcc#dd#eff#gcP8L20MS>>cC3", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_LAND_WARNING_CTS 12 + { "MBT200L2A-G-A-G-A-G-", true }, + #define AP_NOTIFY_PX4_TONE_LOUD_VEHICLE_LOST_CTS 13 + { "MBT200>B#1", true }, + #define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 14 + { "MBNT255>B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8", true }, + #define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 15 + { "MBMLT100O3L512eP4eP16bP16bP2", true }, + #define AP_NOTIFY_PX4_TONE_LOUD_GPS_DISCONNECTED 16 + { "MBMLT100O3L32dbaP16dP16", true }, + #define AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN 17 + { "MFMST200L32O3ceP32cdP32ceP32ccc>cprintf("ToneAlarm_PX4_Solo: Unable to open " TONEALARM0_DEVICE_PATH); + return false; + } + + // set initial boot states. This prevents us issuing a arming + // warning in plane and rover on every boot + flags.armed = AP_Notify::flags.armed; + flags.failsafe_battery = AP_Notify::flags.failsafe_battery; + flags.pre_arm_check = 1; + flags.gps_connected = 1; + _cont_tone_playing = -1; + _gps_disconnected_time = 0; + _init_time = AP_HAL::millis(); + return true; +} + +// play_tune - play one of the pre-defined tunes +void ToneAlarm_PX4_Solo::play_tone(const uint8_t tone_index) +{ + uint32_t tnow_ms = AP_HAL::millis(); + const Tone &tone_requested = _tones[tone_index]; + + if(tone_requested.continuous) { + _cont_tone_playing = tone_index; + } + + _tone_playing = tone_index; + _tone_beginning_ms = tnow_ms; + + play_string(tone_requested.str); +} + +void ToneAlarm_PX4_Solo::play_string(const char *str) { + write(_tonealarm_fd, str, strlen(str) + 1); +} + +void ToneAlarm_PX4_Solo::stop_cont_tone() { + if(_cont_tone_playing == _tone_playing) { + play_string(""); + _tone_playing = -1; + } + _cont_tone_playing = -1; +} + +void ToneAlarm_PX4_Solo::check_cont_tone() { + uint32_t tnow_ms = AP_HAL::millis(); + // if we are supposed to be playing a continuous tone, + // and it was interrupted, and the interrupting tone has timed out, + // resume the continuous tone + + if (_cont_tone_playing != -1 && _tone_playing != _cont_tone_playing && tnow_ms-_tone_beginning_ms > AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS) { + play_tone(_cont_tone_playing); + } +} + +// update - updates led according to timed_updated. Should be called at 50Hz +void ToneAlarm_PX4_Solo::update() +{ + // exit immediately if we haven't initialised successfully + if (_tonealarm_fd == -1) { + return; + } + + check_cont_tone(); + + if (AP_Notify::flags.powering_off) { + if (!flags.powering_off) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN); + } + flags.powering_off = AP_Notify::flags.powering_off; + return; + } + + if (AP_Notify::flags.compass_cal_running != flags.compass_cal_running) { + if(AP_Notify::flags.compass_cal_running) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS); + play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK); + } else { + if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS) { + stop_cont_tone(); + } + } + } + flags.compass_cal_running = AP_Notify::flags.compass_cal_running; + + /*if (!hal.util->get_test_mode()) { //don't notify for GPS disconnection when under Jig + //play tone if UBLOX gps not detected : Solo Specific + if(AP_Notify::flags.initialising || AP_HAL::millis()-_init_time < 10000) { + _gps_disconnected_time = AP_HAL::millis(); + } + if(!AP_Notify::flags.initialising && (AP_HAL::millis() - _gps_disconnected_time) > 10000){ + if (AP_Notify::flags.gps_connected != flags.gps_connected) { + if(!AP_Notify::flags.gps_connected) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_GPS_DISCONNECTED); + } else { + if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_LOUD_GPS_DISCONNECTED) { + stop_cont_tone(); + } + } + } + flags.gps_connected = AP_Notify::flags.gps_connected; + } + } else { + if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_LOUD_GPS_DISCONNECTED) { + stop_cont_tone(); + } + } + + if (flags.test_mode != hal.util->get_test_mode()) { + flags.test_mode = hal.util->get_test_mode(); + if (hal.util->get_test_mode()) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK); + } else { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + } + return; + }*/ + + if (AP_Notify::events.compass_cal_canceled) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + return; + } + + if (AP_Notify::events.initiated_compass_cal) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + return; + } + + if (AP_Notify::events.compass_cal_saved) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED); + return; + } + + if (AP_Notify::events.compass_cal_failed) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + return; + } + + // don't play other tones if compass cal is running + if (AP_Notify::flags.compass_cal_running) { + return; + } + + // notify the user when autotune or mission completes + if (AP_Notify::flags.armed && (AP_Notify::events.autotune_complete || AP_Notify::events.mission_complete)) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED); + } + + //notify the user when autotune fails + if (AP_Notify::flags.armed && (AP_Notify::events.autotune_failed)) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + } + + // notify the user when a waypoint completes + if (AP_Notify::events.waypoint_complete) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE); + } + + // notify the user when their mode change was successful + if (AP_Notify::events.user_mode_change) { + if (AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK); + } else { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + } + } + + // notify the user when their mode change failed + if (AP_Notify::events.user_mode_change_failed) { + if (AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + } else { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + } + } + + // failsafe initiated mode change + if(AP_Notify::events.failsafe_mode_change) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); + } + + // notify the user when arming fails + if (AP_Notify::events.arming_failed) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + } + + // notify the user when RC contact is lost + if (flags.failsafe_radio != AP_Notify::flags.failsafe_radio) { + flags.failsafe_radio = AP_Notify::flags.failsafe_radio; + if (flags.failsafe_radio) { + // armed case handled by events.failsafe_mode_change + if (!AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + } + } else { + if (AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK); + } else { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK); + } + } + } + + // notify the user when pre_arm checks are passing + if (flags.pre_arm_check != AP_Notify::flags.pre_arm_check) { + flags.pre_arm_check = AP_Notify::flags.pre_arm_check; + if (flags.pre_arm_check) { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED); + } + } + + // 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_tone(AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING); + }else{ + // disarming tune + play_tone(AP_NOTIFY_PX4_TONE_QUIET_DISARMED); + stop_cont_tone(); + } + } + + // 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 && !flags.armed) { + // battery warning tune + play_tone(AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS); + } + } + + // check parachute release + if (flags.parachute_release != AP_Notify::flags.parachute_release) { + flags.parachute_release = AP_Notify::flags.parachute_release; + if (flags.parachute_release) { + // parachute release warning tune + play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); + } + } + + // lost vehicle tone + if (flags.vehicle_lost != AP_Notify::flags.vehicle_lost) { + flags.vehicle_lost = AP_Notify::flags.vehicle_lost; + if (flags.vehicle_lost) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_VEHICLE_LOST_CTS); + } else { + stop_cont_tone(); + } + } + +} + +#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4 diff --git a/libraries/AP_Notify/ToneAlarm_PX4_Solo.h b/libraries/AP_Notify/ToneAlarm_PX4_Solo.h new file mode 100644 index 0000000000..e9fe5e345a --- /dev/null +++ b/libraries/AP_Notify/ToneAlarm_PX4_Solo.h @@ -0,0 +1,79 @@ +/* + 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_SOLO_H__ +#define __TONE_ALARM_PX4_SOLO_H__ + +#include "NotifyDevice.h" + +// wait 2 seconds before assuming a tone is done and continuing the continuous tone +#define AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS 2000 + +class ToneAlarm_PX4_Solo: public NotifyDevice +{ +public: + /// init - initialised the tone alarm + bool init(void); + + /// update - updates led according to timed_updated. Should be called at 50Hz + void update(); + +private: + /// play_tune - play one of the pre-defined tunes + void play_tone(const uint8_t tone_index); + + // play_string - play tone specified by the provided string of notes + void play_string(const char *str); + + // stop_cont_tone - stop playing the currently playing continuous tone + void stop_cont_tone(); + + // check_cont_tone - check if we should begin playing a continuous tone + void check_cont_tone(); + + int _tonealarm_fd; // file descriptor for the tone alarm + + uint32_t _gps_disconnected_time; + uint32_t _init_time; + + /// 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 + uint8_t parachute_release : 1; // 1 if parachute is being released + uint8_t pre_arm_check : 1; // 0 = failing checks, 1 = passed + uint8_t failsafe_radio : 1; // 1 if radio failsafe + uint8_t vehicle_lost : 1; // 1 if lost copter tone requested + uint8_t compass_cal_running : 1; // 1 if compass calibration is running + uint8_t gps_connected : 1; + uint8_t test_mode : 1; // 1 if in test_mode + uint8_t powering_off : 1; + } flags; + + int8_t _cont_tone_playing; + int8_t _tone_playing; + uint32_t _tone_beginning_ms; + + struct Tone { + const char *str; + const uint8_t continuous : 1; + }; + + const static Tone _tones[]; +}; + +#endif // __TONE_ALARM_PX4_SOLO_H__