AP_Notify: add ToneAlarm_PX4_Solo

9 years ago · a7f959e6f9
4 changed files with 430 additions and 1 deletions
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@ -25,6 +25,7 @@
				@@ -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;
				@@ -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;
+
+#if AP_NOTIFY_SOLO_TONES == 1
+    ToneAlarm_PX4_Solo tonealarm;
+#else
    ToneAlarm_PX4 tonealarm;
+#endif

-#ifdef OREOLED_ENABLED
+#if AP_NOTIFY_OREOLED == 1
    OreoLED_PX4 oreoled;
    NotifyDevice *AP_Notify::_devices[] = {&boardled, &toshibaled, &tonealarm, &oreoled};
 #else
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@ -22,6 +22,15 @@
				@@ -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
--- a/libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp
+++ b/libraries/AP_Notify/ToneAlarm_PX4_Solo.cpp
@ -0,0 +1,335 @@
				@@ -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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include "ToneAlarm_PX4_Solo.h"
+#include "AP_Notify.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <drivers/drv_tone_alarm.h>
+#include <stdio.h>
+#include <errno.h>
+
+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>c<P32f>c", false },
+    #define AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK 3
+    { "MFMLT200L64O3f>c<P32f>c", 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>c<P64g>c", false },
+    #define AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED 7
+    { "MFMLT200L32O3cdef#gf#P32g>c<P64g>c", 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#bP8L32<cg>cg>c", false },
+    #define AP_NOTIFY_PX4_TONE_QUIET_DISARMED 10
+    { "MFMLT220L128O3cc#dd#eff#gcc#dd#eff#gP32cc#dd#eff#gcc#dd#eff#gcP8L20MS>>c<gc<gc<c<L10c", false },
+    #define AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE 11
+    { "MFT200L8G>C3", 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
+    { "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false }
+};
+
+bool ToneAlarm_PX4_Solo::init()
+{
+    // open the tone alarm device
+    _tonealarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
+    if (_tonealarm_fd == -1) {
+        hal.console->printf("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
--- a/libraries/AP_Notify/ToneAlarm_PX4_Solo.h
+++ b/libraries/AP_Notify/ToneAlarm_PX4_Solo.h
@ -0,0 +1,79 @@
				@@ -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 <http://www.gnu.org/licenses/>.
+ */
+#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__