From 545e417348fc9c56c5d5db2d917802c6f308d740 Mon Sep 17 00:00:00 2001 From: Matt Date: Mon, 29 May 2017 08:17:02 -0400 Subject: [PATCH] AP_Notify: Remove solo specific tones, align with px4 tones Evaluated solo specific tones file vs standard px4 tones files. The only thing the Solo had that standard ArduPilot does not have is the GPS unplugged tone and the power off tone. * Some tones have a different tune, which is fine. We want the Solo to sound like ArduPilot if it is running ArduPilot. * GPS unplugged tone abandoned. Determined to be unnecessary. * Power off tone merged into standard px4 tones file. Smart battery signalling a power off will make use of this tone. Has application for any smart battery equipped vehicle, not just Solo. * Removed all references and dependencies to `ToneAlarm_PX4_Solo.cpp` and `ToneAlarm_PX4_Solo.h` * Deleted `ToneAlarm_PX4_Solo.cpp` and `ToneAlarm_PX4_Solo.h` since they're no longer needed. --- libraries/AP_Notify/ToneAlarm_PX4.cpp | 10 + libraries/AP_Notify/ToneAlarm_PX4.h | 1 + libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp | 335 --------------------- libraries/AP_Notify/ToneAlarm_PX4_Solo.h | 79 ----- 4 files changed, 11 insertions(+), 414 deletions(-) delete mode 100644 libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp delete mode 100644 libraries/AP_Notify/ToneAlarm_PX4_Solo.h diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp index 8a533be4ce..09fae9b162 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -89,6 +89,8 @@ const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] { { "MFT100L10>BBBBBBBB", false}, #define AP_NOTIFY_PX4_TONE_LEAK_DETECTED 26 { "MBT255L8>A+AA-", true}, + #define AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN 27 + { "MFMST200L32O3ceP32cdP32ceP32ccc>c. - */ - -#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 deleted file mode 100644 index e9fe5e345a..0000000000 --- a/libraries/AP_Notify/ToneAlarm_PX4_Solo.h +++ /dev/null @@ -1,79 +0,0 @@ -/* - 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__