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

HAL_Linux: updated to new tonealarm system

This commit is contained in:
Andrew Tridgell 2018-08-01 20:16:19 +10:00
parent 40e8453967
commit d9435261ce
8 changed files with 22 additions and 440 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -32,7 +32,6 @@ extern const AP_HAL::HAL& hal;
#define APM_LINUX_UART_PRIORITY 14
#define APM_LINUX_RCIN_PRIORITY 13
#define APM_LINUX_MAIN_PRIORITY 12
#define APM_LINUX_TONEALARM_PRIORITY 11
#define APM_LINUX_IO_PRIORITY 10
#define APM_LINUX_TIMER_RATE 1000
@ -43,11 +42,9 @@ extern const AP_HAL::HAL& hal;
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
#define APM_LINUX_RCIN_RATE 2000
#define APM_LINUX_TONEALARM_RATE 100
#define APM_LINUX_IO_RATE 50
#else
#define APM_LINUX_RCIN_RATE 100
#define APM_LINUX_TONEALARM_RATE 100
#define APM_LINUX_IO_RATE 50
#endif
@ -76,7 +73,6 @@ void Scheduler::init()
SCHED_THREAD(timer, TIMER),
SCHED_THREAD(uart, UART),
SCHED_THREAD(rcin, RCIN),
SCHED_THREAD(tonealarm, TONEALARM),
SCHED_THREAD(io, IO),
};
@ -128,8 +124,7 @@ void Scheduler::_debug_stack()
_timer_thread.get_stack_usage(),
_io_thread.get_stack_usage(),
_rcin_thread.get_stack_usage(),
_uart_thread.get_stack_usage(),
_tonealarm_thread.get_stack_usage());
_uart_thread.get_stack_usage());
_last_stack_debug_msec = now;
}
}
@ -280,12 +275,6 @@ void Scheduler::_uart_task()
_run_uarts();
}
void Scheduler::_tonealarm_task()
{
// process tone command
Util::from(hal.util)->_toneAlarm_timer_tick();
}
void Scheduler::_io_task()
{
// process any pending storage writes
@ -345,13 +334,11 @@ void Scheduler::teardown()
_io_thread.stop();
_rcin_thread.stop();
_uart_thread.stop();
_tonealarm_thread.stop();
_timer_thread.join();
_io_thread.join();
_rcin_thread.join();
_uart_thread.join();
_tonealarm_thread.join();
}
/*

2
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -86,13 +86,11 @@ private:
SchedulerThread _io_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_io_task, void), *this};
SchedulerThread _rcin_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_rcin_task, void), *this};
SchedulerThread _uart_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_uart_task, void), *this};
SchedulerThread _tonealarm_thread{FUNCTOR_BIND_MEMBER(&Scheduler::_tonealarm_task, void), *this};
void _timer_task();
void _io_task();
void _rcin_task();
void _uart_task();
void _tonealarm_task();
void _run_io();
void _run_uarts();

225
libraries/AP_HAL_Linux/ToneAlarm.cpp
View File

@ -8,33 +8,12 @@
#include <sys/types.h>
#include <unistd.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
using namespace Linux;
extern const AP_HAL::HAL& hal;
static uint16_t notes[] = { 0,
NOTE_C4, NOTE_CS4, NOTE_D4, NOTE_DS4, NOTE_E4, NOTE_F4, NOTE_FS4, NOTE_G4, NOTE_GS4, NOTE_A4, NOTE_AS4, NOTE_B4,
NOTE_C5, NOTE_CS5, NOTE_D5, NOTE_DS5, NOTE_E5, NOTE_F5, NOTE_FS5, NOTE_G5, NOTE_GS5, NOTE_A5, NOTE_AS5, NOTE_B5,
NOTE_C6, NOTE_CS6, NOTE_D6, NOTE_DS6, NOTE_E6, NOTE_F6, NOTE_FS6, NOTE_G6, NOTE_GS6, NOTE_A6, NOTE_AS6, NOTE_B6,
NOTE_C7, NOTE_CS7, NOTE_D7, NOTE_DS7, NOTE_E7, NOTE_F7, NOTE_FS7, NOTE_G7, NOTE_GS7, NOTE_A7, NOTE_AS7, NOTE_B7
};
//List of RTTTL tones
const char* ToneAlarm::tune[TONE_NUMBER_OF_TUNES] = {
"Startup:d=8,o=6,b=480:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7",
"Error:d=4,o=6,b=400:8a,8a,8a,p,a,a,a,p",
"notify_pos:d=4,o=6,b=400:8e,8e,a",
"notify_neut:d=4,o=6,b=400:8e,e",
"notify_neg:d=4,o=6,b=400:8e,8c,8e,8c,8e,8c",
"arming_warn:d=1,o=4,b=75:g",
"batt_war_slow:d=4,o=6,b=200:8a",
"batt_war_fast:d=4,o=6,b=512:8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a",
"GPS_war:d=4,o=6,b=512:a,a,a,1f#",
"Arm_fail:d=4,o=4,b=512:b,a,p",
"para_rel:d=16,o=6,b=512:a,g,a,g,a,g,a,g"};
//Tune Repeat true: play rtttl tune in loop, false: play only once
bool ToneAlarm::tune_repeat[TONE_NUMBER_OF_TUNES] = {false,true,false,false,false,false,true,true,false,false,false};
ToneAlarm::ToneAlarm()
{
@ -45,14 +24,10 @@ ToneAlarm::ToneAlarm()
duty_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/duty",O_WRONLY|O_CLOEXEC);
run_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/run",O_WRONLY|O_CLOEXEC);
#endif
tune_num = -1; //initialy no tune to play
tune_pos = 0;
}
bool ToneAlarm::init()
{
tune_num = 0; //play startup tune
if ((period_fd == -1) || (duty_fd == -1) || (run_fd == -1)) {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
@ -64,200 +39,14 @@ bool ToneAlarm::init()
return true;
}
void ToneAlarm::set_tune(uint8_t tone)
void ToneAlarm::set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
{
tune_num = tone;
}
bool ToneAlarm::is_tune_comp()
{
return tune_comp;
}
void ToneAlarm::stop()
{
dprintf(run_fd,"0");
}
bool ToneAlarm::play()
{
const uint32_t cur_time = AP_HAL::millis();
if(tune_num != prev_tune_num){
tune_changed = true;
return true;
}
if(cur_note != 0){
if (is_zero(frequency) || is_zero(volume)) {
dprintf(run_fd,"0");
dprintf(period_fd,"%u",1000000000/cur_note);
dprintf(duty_fd,"%u",500000000/cur_note);
} else {
dprintf(run_fd,"0");
dprintf(period_fd,"%u",(unsigned int)roundf(1000000000/frequency));
dprintf(duty_fd,"%u",(unsigned int)roundf(volume*500000000/frequency));
dprintf(run_fd,"1");
cur_note =0;
prev_time = cur_time;
}
if((cur_time - prev_time) > duration){
stop();
if(tune[tune_num][tune_pos] == '\0'){
if(!tune_repeat[tune_num]){
tune_num = -1;
}
tune_pos = 0;
tune_comp = true;
return false;
}
return true;
}
return false;
}
bool ToneAlarm::set_note(){
// first, get note duration, if available
uint16_t scale,note,num =0;
duration = 0;
while(isdigit(tune[tune_num][tune_pos])){ //this is a safe while loop as it can't go further than
//the length of the rtttl tone string
num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
}
if(num){
duration = wholenote / num;
} else{
duration = wholenote / 4; // we will need to check if we are a dotted note after
}
// now get the note
note = 0;
switch(tune[tune_num][tune_pos]){
case 'c':
note = 1;
break;
case 'd':
note = 3;
break;
case 'e':
note = 5;
break;
case 'f':
note = 6;
break;
case 'g':
note = 8;
break;
case 'a':
note = 10;
break;
case 'b':
note = 12;
break;
case 'p':
default:
note = 0;
}
tune_pos++;
// now, get optional '#' sharp
if(tune[tune_num][tune_pos] == '#'){
note++;
tune_pos++;
}
// now, get optional '.' dotted note
if(tune[tune_num][tune_pos] == '.'){
duration += duration/2;
tune_pos++;
}
// now, get scale
if(isdigit(tune[tune_num][tune_pos])){
scale = tune[tune_num][tune_pos] - '0';
tune_pos++;
} else{
scale = default_oct;
}
scale += OCTAVE_OFFSET;
if(tune[tune_num][tune_pos] == ','){
tune_pos++; // skip comma for next note (or we may be at the end)
}
// now play the note
if(note){
if(tune_changed == true){
tune_pos =0;
tune_changed = false;
}
cur_note = notes[(scale - 4) * 12 + note];
return true;
} else{
cur_note = 0;
return true;
}
}
bool ToneAlarm::init_tune(){
uint16_t num;
default_dur = 4;
default_oct = 6;
bpm = 63;
prev_tune_num = tune_num;
if(tune_num <0 || tune_num > TONE_NUMBER_OF_TUNES){
return false;
}
tune_comp = false;
while(tune[tune_num][tune_pos] != ':'){
if(tune[tune_num][tune_pos] == '\0'){
return false;
}
tune_pos++;
}
tune_pos++;
if(tune[tune_num][tune_pos] == 'd'){
tune_pos+=2;
num = 0;
while(isdigit(tune[tune_num][tune_pos])){
num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
}
if(num > 0){
default_dur = num;
}
tune_pos++; // skip comma
}
// get default octave
if(tune[tune_num][tune_pos] == 'o')
{
tune_pos+=2; // skip "o="
num = tune[tune_num][tune_pos++] - '0';
if(num >= 3 && num <=7){
default_oct = num;
}
tune_pos++; // skip comma
}
// get BPM
if(tune[tune_num][tune_pos] == 'b'){
tune_pos+=2; // skip "b="
num = 0;
while(isdigit(tune[tune_num][tune_pos])){
num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
}
bpm = num;
tune_pos++; // skip colon
}
// BPM usually expresses the number of quarter notes per minute
wholenote = (60 * 1000L / bpm) * 4; // this is the time for whole note (in milliseconds)
return true;
}

129
libraries/AP_HAL_Linux/ToneAlarm.h
View File

@ -2,140 +2,13 @@
#include <AP_HAL/AP_HAL.h>
#define OCTAVE_OFFSET 0
#define NOTE_B0 31
#define NOTE_C1 33
#define NOTE_CS1 35
#define NOTE_D1 37
#define NOTE_DS1 39
#define NOTE_E1 41
#define NOTE_F1 44
#define NOTE_FS1 46
#define NOTE_G1 49
#define NOTE_GS1 52
#define NOTE_A1 55
#define NOTE_AS1 58
#define NOTE_B1 62
#define NOTE_C2 65
#define NOTE_CS2 69
#define NOTE_D2 73
#define NOTE_DS2 78
#define NOTE_E2 82
#define NOTE_F2 87
#define NOTE_FS2 93
#define NOTE_G2 98
#define NOTE_GS2 104
#define NOTE_A2 110
#define NOTE_AS2 117
#define NOTE_B2 123
#define NOTE_C3 131
#define NOTE_CS3 139
#define NOTE_D3 147
#define NOTE_DS3 156
#define NOTE_E3 165
#define NOTE_F3 175
#define NOTE_FS3 185
#define NOTE_G3 196
#define NOTE_GS3 208
#define NOTE_A3 220
#define NOTE_AS3 233
#define NOTE_B3 247
#define NOTE_C4 262
#define NOTE_CS4 277
#define NOTE_D4 294
#define NOTE_DS4 311
#define NOTE_E4 330
#define NOTE_F4 349
#define NOTE_FS4 370
#define NOTE_G4 392
#define NOTE_GS4 415
#define NOTE_A4 440
#define NOTE_AS4 466
#define NOTE_B4 494
#define NOTE_C5 523
#define NOTE_CS5 554
#define NOTE_D5 587
#define NOTE_DS5 622
#define NOTE_E5 659
#define NOTE_F5 698
#define NOTE_FS5 740
#define NOTE_G5 784
#define NOTE_GS5 831
#define NOTE_A5 880
#define NOTE_AS5 932
#define NOTE_B5 988
#define NOTE_C6 1047
#define NOTE_CS6 1109
#define NOTE_D6 1175
#define NOTE_DS6 1245
#define NOTE_E6 1319
#define NOTE_F6 1397
#define NOTE_FS6 1480
#define NOTE_G6 1568
#define NOTE_GS6 1661
#define NOTE_A6 1760
#define NOTE_AS6 1865
#define NOTE_B6 1976
#define NOTE_C7 2093
#define NOTE_CS7 2217
#define NOTE_D7 2349
#define NOTE_DS7 2489
#define NOTE_E7 2637
#define NOTE_F7 2794
#define NOTE_FS7 2960
#define NOTE_G7 3136
#define NOTE_GS7 3322
#define NOTE_A7 3520
#define NOTE_AS7 3729
#define NOTE_B7 3951
#define NOTE_C8 4186
#define NOTE_CS8 4435
#define NOTE_D8 4699
#define NOTE_DS8 4978
#define TONE_STARTUP_TUNE 0
#define TONE_ERROR_TUNE 1
#define TONE_NOTIFY_POSITIVE_TUNE 2
#define TONE_NOTIFY_NEUTRAL_TUNE 3
#define TONE_NOTIFY_NEGATIVE_TUNE 4
#define TONE_ARMING_WARNING_TUNE 5
#define TONE_BATTERY_WARNING_SLOW_TUNE 6
#define TONE_BATTERY_WARNING_FAST_TUNE 7
#define TONE_GPS_WARNING_TUNE 8
#define TONE_ARMING_FAILURE_TUNE 9
#define TONE_PARACHUTE_RELEASE_TUNE 10
#define TONE_NUMBER_OF_TUNES 11
namespace Linux {
class ToneAlarm {
public:
ToneAlarm();
void set_tune(uint8_t tone);
virtual bool init();
virtual void stop();
virtual bool play();
bool is_tune_comp();
bool set_note();
bool init_tune();
protected:
bool tune_comp;
static const char *tune[TONE_NUMBER_OF_TUNES];
static bool tune_repeat[TONE_NUMBER_OF_TUNES];
bool tune_changed;
uint8_t default_oct;
uint8_t default_dur;
uint16_t bpm;
uint16_t wholenote;
uint16_t cur_note;
uint16_t duration;
int32_t prev_tune_num;
uint32_t prev_time;
int8_t tune_num;
uint8_t tune_pos;
virtual void set_buzzer_tone(float frequency, float volume, uint32_t duration_ms);
private:
int32_t period_fd;

49
libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp
View File

@ -20,63 +20,28 @@
#include <AP_HAL_Linux/RCOutput_Bebop.h>
#include <AP_HAL_Linux/RCOutput_Disco.h>
#include "ToneAlarm_Disco.h"
#include <AP_Math/AP_Math.h>
extern const AP_HAL::HAL &hal;
using namespace Linux;
ToneAlarm_Disco::ToneAlarm_Disco()
{
// initialy no tune to play
tune_num = -1;
tune_pos = 0;
}
ToneAlarm_Disco::ToneAlarm_Disco(){}
bool ToneAlarm_Disco::init()
{
// play startup tune
tune_num = 0;
bebop_out = RCOutput_Disco::from(hal.rcout);
return true;
}
void ToneAlarm_Disco::stop()
void ToneAlarm_Disco::set_buzzer_tone(float frequency, float volume, uint32_t duration_ms)
{
bebop_out->play_note(0, 0, 0);
}
bool ToneAlarm_Disco::play()
{
uint32_t cur_time = AP_HAL::millis();
if (tune_num != prev_tune_num){
tune_changed = true;
return true;
if (is_zero(frequency) || is_zero(volume)) {
bebop_out->play_note(0, 0, 0);
} else {
bebop_out->play_note(TONEALARM_PWM_POWER, (uint16_t)roundf(frequency), duration_ms);
}
if (cur_note != 0){
bebop_out->play_note(TONEALARM_PWM_POWER, cur_note, duration);
cur_note = 0;
prev_time = cur_time;
}
if ((cur_time - prev_time) > duration){
stop();
if (tune[tune_num][tune_pos] == '\0'){
if (!tune_repeat[tune_num]){
tune_num = -1;
}
tune_pos = 0;
tune_comp = true;
return false;
}
return true;
}
return false;
}
#endif

3
libraries/AP_HAL_Linux/ToneAlarm_Disco.h
View File

@ -13,8 +13,7 @@ class ToneAlarm_Disco : public ToneAlarm {
public:
ToneAlarm_Disco();
bool init() override;
void stop() override;
bool play() override;
void set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override;
private:
RCOutput_Bebop *bebop_out;

29
libraries/AP_HAL_Linux/Util.cpp
View File

@ -17,7 +17,6 @@ using namespace Linux;
extern const AP_HAL::HAL& hal;
static int state;
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
ToneAlarm_Disco Util::_toneAlarm;
#else
@ -63,34 +62,6 @@ void Util::commandline_arguments(uint8_t &argc, char * const *&argv)
argv = saved_argv;
}
bool Util::toneAlarm_init()
{
return _toneAlarm.init();
}
void Util::toneAlarm_set_tune(uint8_t tone)
{
_toneAlarm.set_tune(tone);
}
void Util::_toneAlarm_timer_tick() {
if(state == 0) {
state = state + _toneAlarm.init_tune();
} else if (state == 1) {
state = state + _toneAlarm.set_note();
}
if (state == 2) {
state = state + _toneAlarm.play();
} else if (state == 3) {
state = 1;
}
if (_toneAlarm.is_tune_comp()) {
state = 0;
}
}
void Util::set_hw_rtc(uint64_t time_utc_usec)
{
#if CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE

10
libraries/AP_HAL_Linux/Util.h
View File

@ -36,11 +36,6 @@ public:
*/
void commandline_arguments(uint8_t &argc, char * const *&argv);
bool toneAlarm_init();
void toneAlarm_set_tune(uint8_t tune);
void _toneAlarm_timer_tick();
/*
set system clock in UTC microseconds
*/
@ -100,6 +95,11 @@ public:
int get_hw_arm32();
bool toneAlarm_init() override { return _toneAlarm.init(); }
void toneAlarm_set_buzzer_tone(float frequency, float volume, uint32_t duration_ms) override {
_toneAlarm.set_buzzer_tone(frequency, volume, duration_ms);
}
private:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
static ToneAlarm_Disco _toneAlarm;