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

264 lines
7.2 KiB
C++
Raw Blame History

#include "ToneAlarm.h"
#include <fcntl.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.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()
{
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
period_fd = open("/sys/devices/ocp.3/pwm_test_P8_36.12/period",O_WRONLY|O_CLOEXEC);
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 || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI
hal.console->printf("ToneAlarm: Error!! please check if PWM overlays are loaded correctly");
#endif
return false;
}
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()
{
uint16_t cur_time = AP_HAL::millis();
if(tune_num != prev_tune_num){
tune_changed = true;
return true;
}
if(cur_note != 0){
dprintf(run_fd,"0");
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;
}
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;
}
Reference in New Issue View Git Blame Copy Permalink