Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_Linux: make tonealarm generation a passthrough operation

Browse Source 
get rid of all delays and while loops(that may turn into infinite loop)
This commit is contained in:
bugobliterator 2014-11-12 15:31:21 +05:30 committed by Andrew Tridgell
parent e254b406d0
commit 3b94cb3072
2 changed files with 199 additions and 171 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

357
libraries/AP_HAL_Linux/ToneAlarmDriver.cpp
View File

@ -14,6 +14,7 @@
using namespace Linux;
extern const AP_HAL::HAL& hal;
static int state;
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,
@ -23,17 +24,17 @@ NOTE_C7, NOTE_CS7, NOTE_D7, NOTE_DS7, NOTE_E7, NOTE_F7, NOTE_FS7, NOTE_G7, NOTE_
//List of RTTTL tones
const char* LinuxUtil::tune[TONE_NUMBER_OF_TUNES] = {
"Startup:d=8,o=6,b=240:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7",
"Error:d=4,o=6,b=200:8a,8a,8a,p,a,a,a,p",
"notify_pos:d=4,o=6,b=200:8e,8e,a",
"notify_neut:d=4,o=6,b=200:8e,e",
"notify_neg:d=4,o=6,b=200:8e,8c,8e,8c,8e,8c",
"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=100:8a",
"batt_war_fast:d=4,o=6,b=255:8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a,8a",
"GPS_war:d=4,o=6,b=255:a,a,a,1f#",
"Arm_fail:d=4,o=4,b=255:b,a,p",
"para_rel:d=16,o=6,b=255:a,g,a,g,a,g,a,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 LinuxUtil::tune_repeat[TONE_NUMBER_OF_TUNES] = {false,true,false,false,false,false,true,true,false,false,false};
@ -44,6 +45,7 @@ void LinuxUtil::toneAlarm()
run_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/run",O_WRONLY);
tune_num = -1; //initialy no tune to play
tune_pos = 0;
}
int8_t LinuxUtil::toneAlarm_init()
{
@ -60,186 +62,203 @@ void LinuxUtil::stop()
write(run_fd,"0",sizeof(char));
}
void LinuxUtil::play(int tone,int duration)
bool LinuxUtil::play()
{
uint16_t cur_time = hal.scheduler->millis();
if(tune_num != prev_tune_num){
tune_changed = true;
return;
return true;
}
if(tone != 0){
if(cur_note != 0){
dprintf(run_fd,"0");
dprintf(period_fd,"%u",1000000000/tone);
dprintf(duty_fd,"%u",500000000/tone);
dprintf(period_fd,"%u",1000000000/cur_note);
dprintf(duty_fd,"%u",500000000/cur_note);
dprintf(run_fd,"1");
cur_note =0;
prev_time = cur_time;
}
hal.scheduler->delay(duration);
if((cur_time - prev_time) > duration){
stop();
if(tune[tune_num][tune_pos] == NULL){
if(!tune_repeat[tune_num]){
tune_num = -1;
}
tune_pos = 0;
state = 0;
return false;
}
return true;
}
return false;
}
void LinuxUtil::toneAlarm_set_tune(uint8_t tone)
{
tune_num = tone;
}
bool LinuxUtil::set_note(){
// first, get note duration, if available
uint16_t scale,note,num =0;
duration = 0;
void LinuxUtil::play_tune()
{
if(tune_num < 0 || tune_num > 10){
return;
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;
}
uint32_t p = 0;
uint8_t default_dur = 4;
uint8_t default_oct = 6;
uint16_t bpm = 63;
}
bool LinuxUtil::init_tune(){
uint16_t num;
uint32_t wholenote;
uint32_t duration;
uint8_t note;
uint8_t scale;
default_dur = 4;
default_oct = 6;
bpm = 63;
prev_tune_num = tune_num;
while(1){
while(tune[tune_num][p] != ':'){
p++;
}
p++;
if(tune[tune_num][p] == 'd'){
p+=2;
num = 0;
while(isdigit(tune[tune_num][p])){
num = (num * 10) + (tune[tune_num][p++] - '0');
}
if(num > 0){
default_dur = num;
}
p++; // skip comma
}
// get default octave
if(tune[tune_num][p] == 'o')
{
p+=2; // skip "o="
num = tune[tune_num][p++] - '0';
if(num >= 3 && num <=7){
default_oct = num;
}
p++; // skip comma
}
// get BPM
if(tune[tune_num][p] == 'b'){
p+=2; // skip "b="
num = 0;
while(isdigit(tune[tune_num][p])){
num = (num * 10) + (tune[tune_num][p++] - '0');
}
bpm = num;
p++; // 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)
// now begin note loop
while(tune[tune_num][p]){
// first, get note duration, if available
num = 0;
while(isdigit(tune[tune_num][p])){
num = (num * 10) + (tune[tune_num][p++] - '0');
}
if(num){
duration = wholenote / num;
} else{
duration = wholenote / default_dur; // we will need to check if we are a dotted note after
}
// now get the note
note = 0;
switch(tune[tune_num][p]){
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;
}
p++;
// now, get optional '#' sharp
if(tune[tune_num][p] == '#'){
note++;
p++;
}
// now, get optional '.' dotted note
if(tune[tune_num][p] == '.'){
duration += duration/2;
p++;
}
// now, get scale
if(isdigit(tune[tune_num][p])){
scale = tune[tune_num][p] - '0';
p++;
} else{
scale = default_oct;
}
scale += OCTAVE_OFFSET;
if(tune[tune_num][p] == ','){
p++; // skip comma for next note (or we may be at the end)
}
// now play the note
if(note){
play(notes[(scale - 4) * 12 + note],duration);
if(tune_changed == true){
tune_changed = false;
return;
}
stop();
} else{
hal.scheduler->delay(duration);
}
}
if(tune_repeat[tune_num]){
continue;
} else{
tune_num = -1;
return;
}
if(tune_num <0 || tune_num > TONE_NUMBER_OF_TUNES){
return false;
}
while(tune[tune_num][tune_pos] != ':'){
if(tune[tune_num][tune_pos] == NULL){
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;
}
void LinuxUtil::_toneAlarm_timer_tick(){
play_tune();
if(state == 0){
state = state + init_tune();
}else if(state == 1){
state = state + set_note();
}
if(state == 2){
state = state + play();
}else if(state == 3){
state = 1;
}
}
#endif

13
libraries/AP_HAL_Linux/Util.h
View File

@ -135,17 +135,26 @@ public:
private:
void toneAlarm(void);
void stop();
void play(int tone,int duration);
bool play();
void play_tune();
bool set_note();
bool init_tune();
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;
int32_t period_fd;
int32_t duty_fd;
int32_t run_fd;
int8_t tune_num;
uint8_t tune_pos;
int saved_argc;
char* const *saved_argv;