Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: merge ToneAlarm files, use new AP_HAL tonealarm interface

This commit is contained in:
Jonathan Challinger 2018-07-24 18:30:05 -07:00 committed by Andrew Tridgell
parent f5f6220283
commit b31ddedfef
9 changed files with 473 additions and 449 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Notify/AP_Notify.cpp
View File

@ -23,9 +23,7 @@
#include "PCA9685LED_I2C.h"
#include "OreoLED_PX4.h"
#include "RCOutputRGBLed.h"
#include "ToneAlarm_Linux.h"
#include "ToneAlarm_ChibiOS.h"
#include "ToneAlarm_PX4.h"
#include "ToneAlarm.h"
#include "ToshibaLED_I2C.h"
#include "VRBoard_LED.h"
#include "DiscreteRGBLed.h"
@ -259,7 +257,7 @@ void AP_Notify::add_backends(void)
// Add noise making devices
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || \
CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
ADD_BACKEND(new ToneAlarm_PX4());
ADD_BACKEND(new ToneAlarm());
// ChibiOS noise makers
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
@ -267,7 +265,7 @@ void AP_Notify::add_backends(void)
ADD_BACKEND(new Buzzer());
#endif
#ifdef HAL_PWM_ALARM
ADD_BACKEND(new ToneAlarm_ChibiOS());
ADD_BACKEND(new ToneAlarm());
#endif
// Linux noise makers
@ -286,7 +284,7 @@ void AP_Notify::add_backends(void)
ADD_BACKEND(new Buzzer());
#else // other linux
ADD_BACKEND(new ToneAlarm_Linux());
ADD_BACKEND(new ToneAlarm());
#endif
// F4Light noise makers

274
libraries/AP_Notify/MMLPlayer.cpp Normal file
View File

@ -0,0 +1,274 @@
#include "MMLPlayer.h"
#include <ctype.h>
#include <math.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
void MMLPlayer::update() {
// Check if note is over
if (_playing && AP_HAL::micros()-_note_start_us > _note_duration_us) {
next_action();
}
}
void MMLPlayer::play(const char* string) {
stop();
_string = string;
_next = 0;
_tempo = 120;
_default_note_length = 4;
_note_mode = MODE_NORMAL;
_octave = 4;
_volume = 255;
_silence_duration = 0;
_repeat = false;
_playing = true;
next_action();
}
void MMLPlayer::stop() {
_playing = false;
hal.util->toneAlarm_set_buzzer_tone(0,0);
}
void MMLPlayer::start_silence(float duration) {
_note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(0, 0);
}
void MMLPlayer::start_note(float duration, float frequency, float volume) {
_note_start_us = AP_HAL::micros();
_note_duration_us = duration*1e6;
hal.util->toneAlarm_set_buzzer_tone(frequency, volume);
}
char MMLPlayer::next_char() {
while (_string[_next] != '\0' && isspace(_string[_next])) {
_next++;
}
return toupper(_string[_next]);
}
uint8_t MMLPlayer::next_number() {
uint8_t ret = 0;
while (isdigit(next_char())) {
ret = (ret*10) + (next_char() - '0');
_next++;
}
return ret;
}
size_t MMLPlayer::next_dots() {
size_t ret = 0;
while (next_char() == '.') {
ret++;
_next++;
}
return ret;
}
float MMLPlayer::rest_duration(uint32_t rest_length, uint8_t dots) {
float whole_note_period = 240.0f / _tempo;
if (rest_length == 0) {
rest_length = 1;
}
float rest_period = whole_note_period/rest_length;
float dot_extension = rest_period/2;
while (dots--) {
rest_period += dot_extension;
dot_extension *= 0.5f;
}
return rest_period;
}
void MMLPlayer::next_action() {
if (_silence_duration > 0) {
start_silence(_silence_duration);
_silence_duration = 0;
return;
}
uint8_t note = 0;
uint8_t note_length;
while (note == 0) {
char c = next_char();
if (c == '\0') {
if (_repeat) {
play(_string);
} else {
stop();
}
return;
}
_next++;
switch(c) {
case 'V': {
_volume = next_number();
break;
}
case 'L': {
_default_note_length = next_number();
if (_default_note_length == 0) {
stop();
return;
}
break;
}
case 'O':
_octave = next_number();
if (_octave > 6) {
_octave = 6;
}
break;
case '<':
if (_octave > 0) {
_octave--;
}
break;
case '>':
if (_octave < 6) {
_octave++;
}
break;
case 'M':
c = next_char();
if (c == '\0') {
stop();
return;
}
_next++;
switch (c) {
case 'N':
_note_mode = MODE_NORMAL;
break;
case 'L':
_note_mode = MODE_LEGATO;
break;
case 'S':
_note_mode = MODE_STACCATO;
break;
case 'F':
_repeat = false;
break;
case 'B':
_repeat = true;
break;
default:
stop();
return;
}
break;
case 'R':
case 'P': {
uint8_t num = next_number();
uint8_t dots = next_dots();
start_silence(rest_duration(num, dots));
return;
}
case 'T':
_tempo = next_number();
if (_tempo < 32) {
stop();
return;
}
break;
case 'N':
note = next_number();
note_length = _default_note_length;
if (note > 84) {
stop();
return;
}
if (note == 0) {
uint8_t num = next_number();
uint8_t dots = next_dots();
start_silence(rest_duration(num, dots));
return;
}
break;
case 'A':
case 'B':
case 'C':
case 'D':
case 'E':
case 'F':
case 'G': {
uint8_t note_tab[] = {9,11,0,2,4,5,7};
note = note_tab[c-'A'] + (_octave*12) + 1;
c = next_char();
switch (c) {
case '#':
case '+':
if (note < 84) {
note++;
}
_next++;
break;
case '-':
if (note > 1) {
note--;
}
_next++;
break;
default:
break;
}
note_length = next_number();
if (note_length == 0) {
note_length = _default_note_length;
}
break;
}
default:
stop();
return;
}
}
// Avoid division by zero
if (_tempo == 0 || note_length == 0) {
stop();
return;
}
float note_period = 240.0f / (float)_tempo / (float)note_length;
switch (_note_mode) {
case MODE_NORMAL:
_silence_duration = note_period/8;
break;
case MODE_STACCATO:
_silence_duration = note_period/4;
break;
case MODE_LEGATO:
_silence_duration = 0;
break;
}
note_period -= _silence_duration;
float dot_extension = note_period/2;
uint8_t dots = next_dots();
while (dots--) {
note_period += dot_extension;
dot_extension *= 0.5f;
}
float note_frequency = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f);
float note_volume = _volume/255.0f;
start_note(note_period, note_frequency, note_volume);
}

50
libraries/AP_Notify/MMLPlayer.h Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#include <stdint.h>
#include <stdlib.h>
class MMLPlayer {
public:
void update();
void play(const char* string);
void stop();
private:
bool _playing;
uint32_t _note_duration_us;
uint32_t _note_start_us;
const char* _string;
uint8_t _tempo;
uint8_t _default_note_length;
uint8_t _volume;
size_t _next;
uint8_t _octave;
float _silence_duration;
bool _repeat;
enum node_mode_t {
MODE_NORMAL,
MODE_LEGATO,
MODE_STACCATO
} _note_mode;
void start_silence(float duration);
void start_note(float duration, float frequency, float volume);
char next_char();
uint8_t next_number();
size_t next_dots();
float rest_duration(uint32_t rest_length, uint8_t dots);
// Called when the MML player should start the next action
void next_action();
};

227
libraries/AP_Notify/ToneAlarm_PX4.cpp → libraries/AP_Notify/ToneAlarm.cpp
View File

@ -1,120 +1,120 @@
/*
ToneAlarm PX4 driver
*/
* ToneAlarm ChibiOS 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/>.
* 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 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include "ToneAlarm_PX4.h"
#ifdef HAL_PWM_ALARM
#include "ToneAlarm.h"
#include "AP_Notify.h"
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
// #include <sys/types.h>
// #include <sys/stat.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::Tone ToneAlarm_PX4::_tones[] {
#define AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK 0
const ToneAlarm::Tone ToneAlarm::_tones[] {
#define AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK 0
{ "MFT200L4<<<B#A#2", false },
#define AP_NOTIFY_PX4_TONE_LOUD_NEG_FEEDBACK 1
#define AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK 1
{ "MFT100L4>B#A#2P8B#A#2", false },
#define AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK 2
#define AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK 2
{ "MFT200L4<B#", false },
#define AP_NOTIFY_PX4_TONE_LOUD_NEU_FEEDBACK 3
#define AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK 3
{ "MFT100L4>B#", false },
#define AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK 4
#define AP_NOTIFY_TONE_QUIET_POS_FEEDBACK 4
{ "MFT200L4<A#B#", false },
#define AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK 5
#define AP_NOTIFY_TONE_LOUD_POS_FEEDBACK 5
{ "MFT100L4>A#B#", false },
#define AP_NOTIFY_PX4_TONE_LOUD_READY_OR_FINISHED 6
#define AP_NOTIFY_TONE_LOUD_READY_OR_FINISHED 6
{ "MFT100L4>G#6A#6B#4", false },
#define AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED 7
#define AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED 7
{ "MFT200L4<G#6A#6B#4", false },
#define AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED 8
#define AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED 8
{ "MFT100L4>A#A#A#A#", false },
#define AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING 9
#define AP_NOTIFY_TONE_QUIET_ARMING_WARNING 9
{ "MNT75L1O2G", false },
#define AP_NOTIFY_PX4_TONE_LOUD_WP_COMPLETE 10
#define AP_NOTIFY_TONE_LOUD_WP_COMPLETE 10
{ "MFT200L8G>C3", false },
#define AP_NOTIFY_PX4_TONE_LOUD_LAND_WARNING_CTS 11
#define AP_NOTIFY_TONE_LOUD_LAND_WARNING_CTS 11
{ "MBT200L2A-G-A-G-A-G-", true },
#define AP_NOTIFY_PX4_TONE_LOUD_VEHICLE_LOST_CTS 12
#define AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS 12
{ "MBT200>A#1", true },
#define AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS 13
#define AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS 13
{ "MBNT255>A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8A#8", true },
#define AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS 14
#define AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS 14
{ "MBNT255<C16P2", true },
#define AP_NOTIFY_PX4_TONE_WAITING_FOR_THROW 15
#define AP_NOTIFY_TONE_WAITING_FOR_THROW 15
{ "MBNT90L4O2A#O3DFN0N0N0", true},
#define AP_NOTIFY_PX4_TONE_LOUD_1 16
#define AP_NOTIFY_TONE_LOUD_1 16
{ "MFT100L8>B", false},
#define AP_NOTIFY_PX4_TONE_LOUD_2 17
#define AP_NOTIFY_TONE_LOUD_2 17
{ "MFT100L8>BB", false},
#define AP_NOTIFY_PX4_TONE_LOUD_3 18
#define AP_NOTIFY_TONE_LOUD_3 18
{ "MFT100L8>BBB", false},
#define AP_NOTIFY_PX4_TONE_LOUD_4 19
#define AP_NOTIFY_TONE_LOUD_4 19
{ "MFT100L8>BBBB", false},
#define AP_NOTIFY_PX4_TONE_LOUD_5 20
#define AP_NOTIFY_TONE_LOUD_5 20
{ "MFT100L8>BBBBB", false},
#define AP_NOTIFY_PX4_TONE_LOUD_6 21
#define AP_NOTIFY_TONE_LOUD_6 21
{ "MFT100L8>BBBBBB", false},
#define AP_NOTIFY_PX4_TONE_LOUD_7 22
#define AP_NOTIFY_TONE_LOUD_7 22
{ "MFT100L8>BBBBBBB", false},
#define AP_NOTIFY_PX4_TONE_TUNING_START 23
#define AP_NOTIFY_TONE_TUNING_START 23
{ "MFT100L20>C#D#", false},
#define AP_NOTIFY_PX4_TONE_TUNING_SAVE 24
#define AP_NOTIFY_TONE_TUNING_SAVE 24
{ "MFT100L10DBDB>", false},
#define AP_NOTIFY_PX4_TONE_TUNING_ERROR 25
#define AP_NOTIFY_TONE_TUNING_ERROR 25
{ "MFT100L10>BBBBBBBB", false},
#define AP_NOTIFY_PX4_TONE_LEAK_DETECTED 26
#define AP_NOTIFY_TONE_LEAK_DETECTED 26
{ "MBT255L8>A+AA-", true},
#define AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN 27
#define AP_NOTIFY_TONE_QUIET_SHUTDOWN 27
{ "MFMST200L32O3ceP32cdP32ceP32c<c>c<cccP8L32>c>c<P32<c<c", false },
#define AP_NOTIFY_PX4_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
#define AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED 28
{ "MFT200L4<B#4A#6G#6", false },
#define AP_NOTIFY_TONE_STARTUP 29
{ "MFT240L8O4aO5dcO4aO5dcO4aO5dcL16dcdcdcdc", false },
};
bool ToneAlarm_PX4::init()
bool ToneAlarm::init()
{
// open the tone alarm device
_tonealarm_fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY);
if (_tonealarm_fd == -1) {
hal.console->printf("ToneAlarm_PX4: Unable to open " TONEALARM0_DEVICE_PATH);
if (!hal.util->toneAlarm_init()) {
return false;
}
_sem = hal.util->new_semaphore();
// 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;
_cont_tone_playing = -1;
hal.scheduler->register_timer_process(FUNCTOR_BIND(this, &ToneAlarm::_timer_task, void));
play_tone(AP_NOTIFY_TONE_STARTUP);
return true;
}
// play_tune - play one of the pre-defined tunes
void ToneAlarm_PX4::play_tone(const uint8_t tone_index)
void ToneAlarm::play_tone(const uint8_t tone_index)
{
uint32_t tnow_ms = AP_HAL::millis();
const Tone &tone_requested = _tones[tone_index];
@ -129,11 +129,26 @@ void ToneAlarm_PX4::play_tone(const uint8_t tone_index)
play_string(tone_requested.str);
}
void ToneAlarm_PX4::play_string(const char *str) {
write(_tonealarm_fd, str, strlen(str) + 1);
void ToneAlarm::_timer_task() {
if (_sem) {
_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
_mml_player.update();
_sem->give();
}
}
void ToneAlarm_PX4::stop_cont_tone() {
void ToneAlarm::play_string(const char *str) {
if (_sem) {
_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER);
_mml_player.stop();
strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
_tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
_mml_player.play(_tone_buf);
_sem->give();
}
}
void ToneAlarm::stop_cont_tone() {
if(_cont_tone_playing == _tone_playing) {
play_string("");
_tone_playing = -1;
@ -141,7 +156,7 @@ void ToneAlarm_PX4::stop_cont_tone() {
_cont_tone_playing = -1;
}
void ToneAlarm_PX4::check_cont_tone() {
void ToneAlarm::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,
@ -153,7 +168,7 @@ void ToneAlarm_PX4::check_cont_tone() {
}
// update - updates led according to timed_updated. Should be called at 50Hz
void ToneAlarm_PX4::update()
void ToneAlarm::update()
{
// exit if buzzer is not enabled
if (pNotify->buzzer_enabled() == false) {
@ -161,10 +176,10 @@ void ToneAlarm_PX4::update()
}
check_cont_tone();
if (AP_Notify::flags.powering_off) {
if (!flags.powering_off) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_SHUTDOWN);
play_tone(AP_NOTIFY_TONE_QUIET_SHUTDOWN);
}
flags.powering_off = AP_Notify::flags.powering_off;
return;
@ -172,10 +187,10 @@ void ToneAlarm_PX4::update()
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);
play_tone(AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS);
play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
} else {
if(_cont_tone_playing == AP_NOTIFY_PX4_TONE_QUIET_COMPASS_CALIBRATING_CTS) {
if(_cont_tone_playing == AP_NOTIFY_TONE_QUIET_COMPASS_CALIBRATING_CTS) {
stop_cont_tone();
}
}
@ -183,22 +198,22 @@ void ToneAlarm_PX4::update()
flags.compass_cal_running = AP_Notify::flags.compass_cal_running;
if (AP_Notify::events.compass_cal_canceled) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
return;
}
if (AP_Notify::events.initiated_compass_cal) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
return;
}
if (AP_Notify::events.compass_cal_saved) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_READY_OR_FINISHED);
play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED);
return;
}
if (AP_Notify::events.compass_cal_failed) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
return;
}
@ -209,45 +224,45 @@ void ToneAlarm_PX4::update()
// 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);
play_tone(AP_NOTIFY_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);
play_tone(AP_NOTIFY_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);
play_tone(AP_NOTIFY_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);
play_tone(AP_NOTIFY_TONE_LOUD_NEU_FEEDBACK);
} else {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK);
play_tone(AP_NOTIFY_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);
play_tone(AP_NOTIFY_TONE_LOUD_NEG_FEEDBACK);
} else {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEG_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
}
}
// failsafe initiated mode change
if(AP_Notify::events.failsafe_mode_change) {
play_tone(AP_NOTIFY_PX4_TONE_LOUD_ATTENTION_NEEDED);
play_tone(AP_NOTIFY_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);
play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
}
// notify the user when RC contact is lost
@ -256,13 +271,13 @@ void ToneAlarm_PX4::update()
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);
play_tone(AP_NOTIFY_TONE_QUIET_NEG_FEEDBACK);
}
} else {
if (AP_Notify::flags.armed) {
play_tone(AP_NOTIFY_PX4_TONE_LOUD_POS_FEEDBACK);
play_tone(AP_NOTIFY_TONE_LOUD_POS_FEEDBACK);
} else {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_POS_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_POS_FEEDBACK);
}
}
}
@ -271,12 +286,12 @@ void ToneAlarm_PX4::update()
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);
play_tone(AP_NOTIFY_TONE_QUIET_READY_OR_FINISHED);
_have_played_ready_tone = true;
} else {
// only play sad tone if we've ever played happy tone:
if (_have_played_ready_tone) {
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NOT_READY_OR_NOT_FINISHED);
play_tone(AP_NOTIFY_TONE_QUIET_NOT_READY_OR_NOT_FINISHED);
}
}
}
@ -286,12 +301,12 @@ void ToneAlarm_PX4::update()
flags.armed = AP_Notify::flags.armed;
if (flags.armed) {
// arming tune
play_tone(AP_NOTIFY_PX4_TONE_QUIET_ARMING_WARNING);
play_tone(AP_NOTIFY_TONE_QUIET_ARMING_WARNING);
}else{
// disarming tune
play_tone(AP_NOTIFY_PX4_TONE_QUIET_NEU_FEEDBACK);
play_tone(AP_NOTIFY_TONE_QUIET_NEU_FEEDBACK);
if (!flags.leak_detected) {
stop_cont_tone();
stop_cont_tone();
}
}
}
@ -301,7 +316,7 @@ void ToneAlarm_PX4::update()
flags.failsafe_battery = AP_Notify::flags.failsafe_battery;
if (flags.failsafe_battery) {
// battery warning tune
play_tone(AP_NOTIFY_PX4_TONE_LOUD_BATTERY_ALERT_CTS);
play_tone(AP_NOTIFY_TONE_LOUD_BATTERY_ALERT_CTS);
}
}
@ -310,7 +325,7 @@ void ToneAlarm_PX4::update()
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);
play_tone(AP_NOTIFY_TONE_LOUD_ATTENTION_NEEDED);
}
}
@ -318,7 +333,7 @@ void ToneAlarm_PX4::update()
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);
play_tone(AP_NOTIFY_TONE_LOUD_VEHICLE_LOST_CTS);
} else {
stop_cont_tone();
}
@ -328,7 +343,7 @@ void ToneAlarm_PX4::update()
if (flags.waiting_for_throw != AP_Notify::flags.waiting_for_throw) {
flags.waiting_for_throw = AP_Notify::flags.waiting_for_throw;
if (flags.waiting_for_throw) {
play_tone(AP_NOTIFY_PX4_TONE_WAITING_FOR_THROW);
play_tone(AP_NOTIFY_TONE_WAITING_FOR_THROW);
} else {
stop_cont_tone();
}
@ -337,44 +352,44 @@ void ToneAlarm_PX4::update()
if (flags.leak_detected != AP_Notify::flags.leak_detected) {
flags.leak_detected = AP_Notify::flags.leak_detected;
if (flags.leak_detected) {
play_tone(AP_NOTIFY_PX4_TONE_LEAK_DETECTED);
play_tone(AP_NOTIFY_TONE_LEAK_DETECTED);
} else {
stop_cont_tone();
}
}
if (AP_Notify::events.tune_started) {
play_tone(AP_NOTIFY_PX4_TONE_TUNING_START);
AP_Notify::events.tune_started = 0;
play_tone(AP_NOTIFY_TONE_TUNING_START);
AP_Notify::events.tune_started = 0;
}
if (AP_Notify::events.tune_next) {
// signify which parameter in the set is starting
play_tone(AP_NOTIFY_PX4_TONE_LOUD_1 + (AP_Notify::events.tune_next-1));
AP_Notify::events.tune_next = 0;
play_tone(AP_NOTIFY_TONE_LOUD_1 + (AP_Notify::events.tune_next-1));
AP_Notify::events.tune_next = 0;
}
if (AP_Notify::events.tune_save) {
play_tone(AP_NOTIFY_PX4_TONE_TUNING_SAVE);
play_tone(AP_NOTIFY_TONE_TUNING_SAVE);
AP_Notify::events.tune_save = 0;
}
if (AP_Notify::events.tune_error) {
play_tone(AP_NOTIFY_PX4_TONE_TUNING_ERROR);
play_tone(AP_NOTIFY_TONE_TUNING_ERROR);
AP_Notify::events.tune_error = 0;
}
}
/*
handle a PLAY_TUNE message
*/
void ToneAlarm_PX4::handle_play_tune(mavlink_message_t *msg)
* handle a PLAY_TUNE message
*/
void ToneAlarm::handle_play_tune(mavlink_message_t *msg)
{
// decode mavlink message
mavlink_play_tune_t packet;
mavlink_msg_play_tune_decode(msg, &packet);
play_string(packet.tune);
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4
#endif // HAL_PWM_ALARM

41
libraries/AP_Notify/ToneAlarm_PX4.h → libraries/AP_Notify/ToneAlarm.h
View File

@ -1,28 +1,30 @@
/*
ToneAlarm PX4 driver
*/
* 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/>.
* 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/>.
*/
#pragma once
#include "NotifyDevice.h"
#include "MMLPlayer.h"
// wait 2 seconds before assuming a tone is done and continuing the continuous tone
#define AP_NOTIFY_PX4_MAX_TONE_LENGTH_MS 2000
#define AP_NOTIFY_TONEALARM_TONE_BUF_SIZE 512
class ToneAlarm_PX4: public NotifyDevice
class ToneAlarm: public NotifyDevice
{
public:
/// init - initialised the tone alarm
@ -33,7 +35,7 @@ public:
// handle a PLAY_TUNE message
void handle_play_tune(mavlink_message_t *msg);
private:
/// play_tune - play one of the pre-defined tunes
void play_tone(const uint8_t tone_index);
@ -47,7 +49,8 @@ private:
// 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
// timer task - runs at 1khz
void _timer_task();
/// tonealarm_type - bitmask of states we track
struct tonealarm_type {
@ -74,4 +77,8 @@ private:
};
const static Tone _tones[];
AP_HAL::Semaphore* _sem;
MMLPlayer _mml_player;
char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE];
};

128
libraries/AP_Notify/ToneAlarm_ChibiOS.cpp
View File

@ -1,128 +0,0 @@
/*
ToneAlarm Linux 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_CHIBIOS
#include "ToneAlarm_ChibiOS.h"
#include <AP_HAL_ChibiOS/ToneAlarm.h>
#include <AP_Math/AP_Math.h>
#include "AP_Notify.h"
extern const AP_HAL::HAL& hal;
bool ToneAlarm_ChibiOS::init()
{
// open the tone alarm device
bool _initialized = hal.util->toneAlarm_init();
if (!_initialized) {
hal.console->printf("AP_Notify: Failed to initialise ToneAlarm");
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;
return true;
}
// play_tune - play one of the pre-defined tunes
bool ToneAlarm_ChibiOS::play_tune(uint8_t tune_number)
{
hal.util->toneAlarm_set_tune(tune_number);
return true;
}
// update - updates led according to timed_updated. Should be called at 50Hz
void ToneAlarm_ChibiOS::update()
{
// exit if buzzer is not enabled
if (pNotify->buzzer_enabled() == false) {
return;
}
// check for arming failure
if (AP_Notify::events.arming_failed) {
play_tune(TONE_ARMING_FAILURE_TUNE);
}
// 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_tune(TONE_ARMING_WARNING_TUNE);
}else{
// disarming tune
play_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
// notify the user when their mode change was successful
if (AP_Notify::events.user_mode_change) {
if (AP_Notify::flags.armed) {
play_tune(TONE_NOTIFY_MODE_CHANGE_LOUD);
} else {
play_tune(TONE_NOTIFY_MODE_CHANGE_SOFT);
}
}
// 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) {
// low battery warning tune
play_tune(TONE_BATTERY_WARNING_FAST_TUNE);
}
}
// 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_tune(TONE_PARACHUTE_RELEASE_TUNE);
}
}
}
/*
handle a PLAY_TUNE message
*/
void ToneAlarm_ChibiOS::handle_play_tune(mavlink_message_t *msg)
{
// decode mavlink message
mavlink_play_tune_t packet;
mavlink_msg_play_tune_decode(msg, &packet);
char str[100];
strncpy(str, packet.tune, MIN(sizeof(packet.tune), sizeof(str)-1));
str[sizeof(str)-1] = 0;
uint8_t len = strlen(str);
uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
len2 = MIN((sizeof(str)-1)-len, len2);
strncpy(str+len, packet.tune2, len2);
str[sizeof(str)-1] = 0;
hal.util->toneAlarm_set_tune_string(str);
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS

44
libraries/AP_Notify/ToneAlarm_ChibiOS.h
View File

@ -1,44 +0,0 @@
/*
ToneAlarm Linux 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/>.
*/
#pragma once
#include "NotifyDevice.h"
class ToneAlarm_ChibiOS: 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();
// handle a PLAY_TUNE message
void handle_play_tune(mavlink_message_t *msg);
private:
/// play_tune - play one of the pre-defined tunes
bool play_tune(uint8_t tune_number);
/// tonealarm_type - bitmask of states we track
struct tonealarm_type {
bool armed : 1; // false = disarmed, true = armed
bool failsafe_battery : 1; // true if battery failsafe
bool parachute_release : 1; // true if parachute is being released
} flags;
};

105
libraries/AP_Notify/ToneAlarm_Linux.cpp
View File

@ -1,105 +0,0 @@
/*
ToneAlarm Linux 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_LINUX
#include "ToneAlarm_Linux.h"
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <AP_HAL_Linux/Util.h>
#include "AP_Notify.h"
extern const AP_HAL::HAL& hal;
bool ToneAlarm_Linux::init()
{
// open the tone alarm device
bool initialized = hal.util->toneAlarm_init();
if (!initialized) {
hal.console->printf("AP_Notify: Failed to initialise ToneAlarm");
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;
return true;
}
// play_tune - play one of the pre-defined tunes
bool ToneAlarm_Linux::play_tune(uint8_t tune_number)
{
hal.util->toneAlarm_set_tune(tune_number);
return true;
}
// update - updates led according to timed_updated. Should be called at 50Hz
void ToneAlarm_Linux::update()
{
// exit if buzzer is not enabled
if (pNotify->buzzer_enabled() == false) {
return;
}
// check for arming failure
if (AP_Notify::events.arming_failed) {
play_tune(TONE_ARMING_FAILURE_TUNE);
}
// 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_tune(TONE_ARMING_WARNING_TUNE);
}else{
// disarming tune
play_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
// 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) {
// low battery warning tune
play_tune(TONE_BATTERY_WARNING_FAST_TUNE);
}
}
// 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_tune(TONE_PARACHUTE_RELEASE_TUNE);
}
}
}
#endif // CONFIG_HAL_BOARD == HAL_BOARD_PX4

43
libraries/AP_Notify/ToneAlarm_Linux.h
View File

@ -1,43 +0,0 @@
/*
ToneAlarm Linux 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/>.
*/
#pragma once
#include "NotifyDevice.h"
class ToneAlarm_Linux: 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
bool play_tune(uint8_t tune_number);
bool _initialized = false;
/// tonealarm_type - bitmask of states we track
struct tonealarm_type {
bool armed : 1; // false = disarmed, true = armed
bool failsafe_battery : 1; // true if battery failsafe
bool parachute_release : 1; // true if parachute is being released
} flags;
};