Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 09:53:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: Rework ToneAlarm_PX4 to play tones from local array

Browse Source
This commit is contained in:
Jonathan Challinger 2014-10-23 05:04:34 -07:00 committed by Randy Mackay
parent fd55a2d9c4
commit 3dfcdde6be
4 changed files with 194 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Notify/AP_Notify.cpp
View File

@ -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));
}

14
libraries/AP_Notify/AP_Notify.h
View File

@ -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);

198
libraries/AP_Notify/ToneAlarm_PX4.cpp
View File

@ -26,6 +26,7 @@
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <drivers/drv_tone_alarm.h>
#include <stdio.h>
@ -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#2", false },
#define AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK 1
{ "MFT100L4>B#A#2P8B#A#2", false },
#define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2
{ "MFT200L4<B#", false },
#define AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK 3
{ "MFT100L4>B#", false },
#define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4
{ "MFT200L4<A#B#", false },
#define AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK 5
{ "MFT100L4>A#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
{ "MFT200L4<G#6A#6B#4", 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
{ "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);
}
}
}

37
libraries/AP_Notify/ToneAlarm_PX4.h
View File

@ -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__