From 3dfcdde6be73f5c91763f9e7709004c267087763 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 23 Oct 2014 05:04:34 -0700 Subject: [PATCH] AP_Notify: Rework ToneAlarm_PX4 to play tones from local array --- libraries/AP_Notify/AP_Notify.cpp | 4 + libraries/AP_Notify/AP_Notify.h | 14 +- libraries/AP_Notify/ToneAlarm_PX4.cpp | 198 +++++++++++++++++++------- libraries/AP_Notify/ToneAlarm_PX4.h | 37 +++-- 4 files changed, 194 insertions(+), 59 deletions(-) diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index 85cbcfe8fa..938c466b9e 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -18,6 +18,7 @@ // static flags, to allow for direct class update from device drivers struct AP_Notify::notify_type AP_Notify::flags; +struct AP_Notify::notify_events_type AP_Notify::events; #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 AP_BoardLED boardled; @@ -68,4 +69,7 @@ void AP_Notify::update(void) for (int i = 0; i < CONFIG_NOTIFY_DEVICES_COUNT; i++) { _devices[i]->update(); } + + //reset the events + memset(&AP_Notify::events, 0, sizeof(AP_Notify::events)); } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index b6696f9a24..65ce25feaa 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -56,12 +56,12 @@ public: uint16_t baro_glitching : 1; // 1 if baro altitude is not good uint16_t armed : 1; // 0 = disarmed, 1 = armed uint16_t pre_arm_check : 1; // 0 = failing checks, 1 = passed + uint16_t arming_failed : 1; uint16_t save_trim : 1; // 1 if gathering trim data uint16_t esc_calibration : 1; // 1 if calibrating escs uint16_t failsafe_radio : 1; // 1 if radio failsafe uint16_t failsafe_battery : 1; // 1 if battery failsafe uint16_t failsafe_gps : 1; // 1 if gps failsafe - uint16_t arming_failed : 1; // 1 if copter failed to arm after user input uint16_t parachute_release : 1; // 1 if parachute is being released uint16_t ekf_bad : 1; // 1 if ekf is reporting problems @@ -69,9 +69,21 @@ public: uint16_t external_leds : 1; // 1 if external LEDs are enabled (normally only used for copter) }; + struct notify_events_type { + uint8_t user_mode_change : 1; + uint8_t autotune_complete : 1; + uint8_t autotune_failed : 1; + uint8_t autotune_next_axis : 1; + uint8_t mission_complete : 1; + uint8_t waypoint_complete : 1; + uint8_t user_mode_change_failed : 1; + uint8_t failsafe_action_taken : 1; + }; + // the notify flags are static to allow direct class access // without declaring the object static struct notify_type flags; + static struct notify_events_type events; // initialisation void init(bool enable_external_leds); diff --git a/libraries/AP_Notify/ToneAlarm_PX4.cpp b/libraries/AP_Notify/ToneAlarm_PX4.cpp index fb745da2b0..081edf7af5 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.cpp +++ b/libraries/AP_Notify/ToneAlarm_PX4.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -33,10 +34,41 @@ extern const AP_HAL::HAL& hal; +const ToneAlarm_PX4::Tone ToneAlarm_PX4::_tones[] { + #define AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK 0 + { "MFT200L4<<B#A#2P8B#A#2", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2 + { "MFT200L4B#", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4 + { "MFT200L4A#B#", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED 6 + { "MFT100L4>G#6A#6B#4", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED 7 + { "MFT200L4B#B#B#B#", false }, + #define AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING 9 + { "MNT75L1O2G", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE 10 + { "MFT200L8G>C3", false }, + #define AP_NOTIFY_PX4_TONE_LOUD_LAND_WARNING_CTS 11 + { "MBT200L2A-G-A-G-A-G-", true }, + #define AP_NOTIFY_PX4_TONE_LOUD_LOST_COPTER_CTS 12 + { "MBT200>B#1", true }, + #define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 13 + { "MBNT255>B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8B#8", true }, +}; + bool ToneAlarm_PX4::init() { // open the tone alarm device - _tonealarm_fd = open(TONEALARM_DEVICE_PATH, 0); + _tonealarm_fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (_tonealarm_fd == -1) { hal.console->printf("Unable to open " TONEALARM_DEVICE_PATH); return false; @@ -46,16 +78,50 @@ bool ToneAlarm_PX4::init() // 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.user_mode_initialized = 0; + _cont_tone_playing = -1; return true; } // play_tune - play one of the pre-defined tunes -bool ToneAlarm_PX4::play_tune(const uint8_t tune_number) +void ToneAlarm_PX4::play_tone(const uint8_t tone_index) { - int ret = ioctl(_tonealarm_fd, TONE_SET_ALARM, tune_number); - return (ret == 0); + uint32_t tnow_ms = hal.scheduler->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::play_string(const char *str) { + write(_tonealarm_fd, str, strlen(str) + 1); +} + +void ToneAlarm_PX4::stop_cont_tone() { + if(_cont_tone_playing == _tone_playing) { + play_string("stop"); + _tone_playing = -1; + } + _cont_tone_playing = -1; +} + +void ToneAlarm_PX4::check_cont_tone() { + uint32_t tnow_ms = hal.scheduler->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::update() @@ -65,11 +131,80 @@ void ToneAlarm_PX4::update() return; } - // check for arming failure - if(flags.arming_failed != AP_Notify::flags.arming_failed) { + check_cont_tone(); + + // 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 && flags.user_mode_initialized) { + 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 && flags.user_mode_initialized) { + if (AP_Notify::flags.armed) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK); + } else { + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK); + } + } + + //set user_mode_initialized + if (AP_Notify::events.user_mode_change || AP_Notify::events.user_mode_change_failed) { + flags.user_mode_initialized = 1; + } + + if(AP_Notify::events.failsafe_action_taken) { + play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); + } + + // notify the user when arming fails + if (flags.arming_failed != AP_Notify::flags.arming_failed) { flags.arming_failed = AP_Notify::flags.arming_failed; - if(flags.arming_failed) { - play_tune(TONE_ARMING_FAILURE_TUNE); + if (flags.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_action_taken + 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); } } @@ -78,10 +213,11 @@ void ToneAlarm_PX4::update() flags.armed = AP_Notify::flags.armed; if (flags.armed) { // arming tune - play_tune(TONE_ARMING_WARNING_TUNE); + play_tone(AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING); }else{ // disarming tune - play_tune(TONE_NOTIFY_NEUTRAL_TUNE); + play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK); + stop_cont_tone(); } } @@ -89,35 +225,8 @@ void ToneAlarm_PX4::update() 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); - } - } - - // check gps glitch - if (flags.gps_glitching != AP_Notify::flags.gps_glitching) { - flags.gps_glitching = AP_Notify::flags.gps_glitching; - if (flags.gps_glitching) { - // gps glitch warning tune - play_tune(TONE_GPS_WARNING_TUNE); - } - } - - // check gps failsafe - if (flags.failsafe_gps != AP_Notify::flags.failsafe_gps) { - flags.failsafe_gps = AP_Notify::flags.failsafe_gps; - if (flags.failsafe_gps) { - // gps glitch warning tune - play_tune(TONE_GPS_WARNING_TUNE); - } - } - - // check baro glitch - if (flags.baro_glitching != AP_Notify::flags.baro_glitching) { - flags.baro_glitching = AP_Notify::flags.baro_glitching; - if (flags.baro_glitching) { - // gps glitch warning tune - play_tune(TONE_BARO_WARNING_TUNE); + // battery warning tune + play_tone(AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS); } } @@ -126,16 +235,7 @@ void ToneAlarm_PX4::update() flags.parachute_release = AP_Notify::flags.parachute_release; if (flags.parachute_release) { // parachute release warning tune - play_tune(TONE_PARACHUTE_RELEASE_TUNE); - } - } - - // check ekf has gone bad - if (flags.ekf_bad != AP_Notify::flags.ekf_bad) { - flags.ekf_bad = AP_Notify::flags.ekf_bad; - if (flags.ekf_bad) { - // ekf warning tune - play_tune(TONE_EKF_WARNING_TUNE); + play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED); } } } diff --git a/libraries/AP_Notify/ToneAlarm_PX4.h b/libraries/AP_Notify/ToneAlarm_PX4.h index fd52d8a1d1..9011878c37 100644 --- a/libraries/AP_Notify/ToneAlarm_PX4.h +++ b/libraries/AP_Notify/ToneAlarm_PX4.h @@ -20,6 +20,9 @@ #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: public NotifyDevice { public: @@ -31,21 +34,37 @@ public: private: /// play_tune - play one of the pre-defined tunes - bool play_tune(const uint8_t tune_number); + void play_tone(const uint8_t tone_index); + + void play_string(const char *str); + + void stop_cont_tone(); + void check_cont_tone(); 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 - uint8_t gps_glitching : 1; // 1 if gps position is not good - uint8_t failsafe_gps : 1; // 1 if gps failsafe - uint8_t baro_glitching : 1; // 1 if baro alt is glitching - uint8_t arming_failed : 1; // 0 = failing checks, 1 = passed - uint8_t parachute_release : 1; // 1 if parachute is being released - uint8_t ekf_bad : 1; // 1 if ekf position has gone bad + uint8_t armed : 1; // 0 = disarmed, 1 = armed + uint8_t failsafe_battery : 1; // 1 if battery failsafe + uint8_t failsafe_gps : 1; // 1 if gps failsafe + uint8_t arming_failed : 1; // 0 = failing checks, 1 = passed + uint8_t parachute_release : 1; // 1 if parachute is being released + uint8_t pre_arm_check : 1; + uint8_t failsafe_radio : 1; + uint8_t user_mode_initialized : 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_H__