zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_Linux: reflect changes to tonealarm interface

mission-4.1.18
Jonathan Challinger 7 years ago committed by Andrew Tridgell
parent
0dd151c0f3
commit
ddf0333a24
8 changed files with 19 additions and 439 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 15
      libraries/AP_HAL_Linux/Scheduler.cpp
  2. 2
      libraries/AP_HAL_Linux/Scheduler.h
  3. 225
      libraries/AP_HAL_Linux/ToneAlarm.cpp
  4. 129
      libraries/AP_HAL_Linux/ToneAlarm.h
  5. 48
      libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp
  6. 3
      libraries/AP_HAL_Linux/ToneAlarm_Disco.h
  7. 28
      libraries/AP_HAL_Linux/Util.cpp
  8. 8
      libraries/AP_HAL_Linux/Util.h

15
libraries/AP_HAL_Linux/Scheduler.cpp
Unescape View File

@ -32,7 +32,6 @@ extern const AP_HAL::HAL& hal; @@ -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; @@ -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() @@ -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() @@ -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() @@ -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() @@ -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
Unescape View File

@ -86,13 +86,11 @@ private: @@ -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
Unescape View File

@ -8,33 +8,12 @@ @@ -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() @@ -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() @@ -64,200 +39,14 @@ bool ToneAlarm::init()
return true;
}
void ToneAlarm::set_tune(uint8_t tone)
{
tune_num = tone;
}
bool ToneAlarm::is_tune_comp()
{
return tune_comp;
}
void ToneAlarm::stop()
{
dprintf(run_fd,"0");
}
bool ToneAlarm::play()
void ToneAlarm::set_buzzer_tone(float frequency, float volume)
{
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");
} else {
dprintf(run_fd,"0");
dprintf(period_fd,"%u",1000000000/cur_note);
dprintf(duty_fd,"%u",500000000/cur_note);
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
Unescape View File

@ -2,140 +2,13 @@ @@ -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);
private:
int32_t period_fd;

48
libraries/AP_HAL_Linux/ToneAlarm_Disco.cpp
Unescape View File

@ -25,58 +25,22 @@ extern const AP_HAL::HAL &hal; @@ -25,58 +25,22 @@ 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)
{
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), 10000);
}
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
Unescape View File

@ -13,8 +13,7 @@ class ToneAlarm_Disco : public ToneAlarm { @@ -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) override;
private:
RCOutput_Bebop *bebop_out;

28
libraries/AP_HAL_Linux/Util.cpp
Unescape View File

@ -63,34 +63,6 @@ void Util::commandline_arguments(uint8_t &argc, char * const *&argv) @@ -63,34 +63,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

8
libraries/AP_HAL_Linux/Util.h
Unescape View File

@ -36,11 +36,6 @@ public: @@ -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,9 @@ public: @@ -100,6 +95,9 @@ public:
int get_hw_arm32();
bool toneAlarm_init() override { return _toneAlarm.init(); }
void toneAlarm_set_buzzer_tone(float frequency, float volume) override { _toneAlarm.set_buzzer_tone(frequency, volume); }
private:
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO
static ToneAlarm_Disco _toneAlarm;

Write Preview
Loading…
Cancel
Save