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

HAL_Linux: Add ToneAlarm Driver to Linux_HAL

Browse Source 
ToneAlarm is setup for Beaglebone Black using pwm, toneAlarm thread runs on second last priority.
This commit is contained in:
bugobliterator 2014-08-23 13:22:43 +05:30 committed by Andrew Tridgell
parent 5d6af51517
commit 298b27444a
4 changed files with 427 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AP_HAL_Linux/Scheduler.cpp
View File

@ -6,6 +6,7 @@
#include "Storage.h"
#include "RCInput.h"
#include "UARTDriver.h"
#include "Util.h"
#include <sys/time.h>
#include <poll.h>
#include <unistd.h>
@ -18,11 +19,12 @@ using namespace Linux;
extern const AP_HAL::HAL& hal;
#define APM_LINUX_TIMER_PRIORITY 14
#define APM_LINUX_UART_PRIORITY 13
#define APM_LINUX_RCIN_PRIORITY 12
#define APM_LINUX_MAIN_PRIORITY 11
#define APM_LINUX_IO_PRIORITY 10
#define APM_LINUX_TIMER_PRIORITY 15
#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
LinuxScheduler::LinuxScheduler()
{}
@ -76,7 +78,15 @@ void LinuxScheduler::init(void* machtnichts)
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_rcin_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_rcin_thread, this);
// the Tone Alarm thread runs at highest priority
param.sched_priority = APM_LINUX_TONEALARM_PRIORITY;
pthread_attr_init(&thread_attr);
(void)pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_tonealarm_thread_ctx, &thread_attr, (pthread_startroutine_t)&Linux::LinuxScheduler::_tonealarm_thread, this);
// the IO thread runs at lower priority
pthread_attr_init(&thread_attr);
param.sched_priority = APM_LINUX_IO_PRIORITY;
@ -308,6 +318,21 @@ void *LinuxScheduler::_uart_thread(void)
return NULL;
}
void *LinuxScheduler::_tonealarm_thread(void)
{
_setup_realtime(32768);
while (system_initializing()) {
poll(NULL, 0, 1);
}
while (true) {
_microsleep(20000);
// process tone command
((LinuxUtil *)hal.util)->_toneAlarm_timer_tick();
}
return NULL;
}
void *LinuxScheduler::_io_thread(void)
{
_setup_realtime(32768);

2
libraries/AP_HAL_Linux/Scheduler.h
View File

@ -71,11 +71,13 @@ private:
pthread_t _io_thread_ctx;
pthread_t _rcin_thread_ctx;
pthread_t _uart_thread_ctx;
pthread_t _tonealarm_thread_ctx;
void *_timer_thread(void);
void *_io_thread(void);
void *_rcin_thread(void);
void *_uart_thread(void);
void *_tonealarm_thread(void);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);

264
libraries/AP_HAL_Linux/ToneAlarmDriver.cpp Normal file
View File

@ -0,0 +1,264 @@
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "Util.h"
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <iostream>
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
};
void LinuxUtil::toneAlarm()
{
tune[TONE_STARTUP_TUNE] = "Startup:d=8,o=6,b=240:a,d7,c7,a,d7,c7,a,d7,16d7,16c7,16d7,16c7,16d7,16c7,16d7,16c7";
tune[TONE_ERROR_TUNE] = "Error:d=4,o=6,b=200:8a,8a,8a,p,a,a,a,p";
tune[TONE_NOTIFY_POSITIVE_TUNE] = "notify_pos:d=4,o=6,b=200:8e,8e,a";
tune[TONE_NOTIFY_NEUTRAL_TUNE] = "notify_neut:d=4,o=6,b=200:8e,e";
tune[TONE_NOTIFY_NEGATIVE_TUNE] = "notify_neg:d=4,o=6,b=200:8e,8c,8e,8c,8e,8c";
tune[TONE_ARMING_WARNING_TUNE] = "arming_warn:d=1,o=4,b=75:g";
tune[TONE_BATTERY_WARNING_SLOW_TUNE] = "batt_war_slow:d=4,o=6,b=100:8a";
tune[TONE_BATTERY_WARNING_FAST_TUNE] = "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";
tune[TONE_GPS_WARNING_TUNE] = "GPS_war:d=4,o=6,b=255:a,a,a,1f#";
tune[TONE_ARMING_FAILURE_TUNE] = "Arm_fail:d=4,o=4,b=255:b,a,p";
tune[TONE_PARACHUTE_RELEASE_TUNE] = "para_rel:d=16,o=6,b=255:a,g,a,g,a,g,a,g";
tune_repeat[TONE_STARTUP_TUNE] = false;
tune_repeat[TONE_ERROR_TUNE] = true;
tune_repeat[TONE_NOTIFY_POSITIVE_TUNE] = false;
tune_repeat[TONE_NOTIFY_NEUTRAL_TUNE] = false;
tune_repeat[TONE_NOTIFY_NEGATIVE_TUNE] = false;
tune_repeat[TONE_ARMING_WARNING_TUNE] = false;
tune_repeat[TONE_BATTERY_WARNING_SLOW_TUNE] = true;
tune_repeat[TONE_BATTERY_WARNING_FAST_TUNE] = true;
tune_repeat[TONE_GPS_WARNING_TUNE] = false;
tune_repeat[TONE_ARMING_FAILURE_TUNE] = false;
tune_repeat[TONE_PARACHUTE_RELEASE_TUNE] = false;
char str[]="/sys/devices/ocp.3/pwm_test_P8_36.12/period";
char str1[]="/sys/devices/ocp.3/pwm_test_P8_36.12/duty";
char str2[]="/sys/devices/ocp.3/pwm_test_P8_36.12/run";
period_fd = open(str,O_WRONLY);
duty_fd = open(str1,O_WRONLY);
run_fd = open(str2,O_WRONLY);
tune_num = -1; //initialy no tune to play
}
uint8_t LinuxUtil::toneAlarm_init()
{
tune_num = 0; //play startup tune
if((period_fd == -1) || (duty_fd == -1) || (run_fd == -1)){
return -1;
}
return 0;
}
void LinuxUtil::stop()
{
write(run_fd,"0",sizeof(char));
}
void LinuxUtil::play(int tone,int duration)
{
char buf[10];
if(tune_num != prev_tune_num){
tune_changed = true;
return;
}
if(tone != 0){
write(run_fd,"0",sizeof(char));
sprintf(buf,"%u",1000000000/tone);
write(period_fd,buf,sizeof(buf));
sprintf(buf,"%u",500000000/tone);
write(duty_fd,buf,sizeof(buf));
write(run_fd,"1",sizeof(char));
}
hal.scheduler->delay(duration);
}
void LinuxUtil::toneAlarm_set_tune(uint8_t tone)
{
tune_num = tone;
}
void LinuxUtil::play_tune()
{
if(tune_num < 0 || tune_num > 10){
return;
}
uint32_t p = 0;
uint8_t default_dur = 4;
uint8_t default_oct = 6;
uint16_t bpm = 63;
uint16_t num;
uint32_t wholenote;
uint32_t duration;
uint8_t note;
uint8_t scale;
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;
}
}
}
void LinuxUtil::_toneAlarm_timer_tick(){
play_tune();
}
#endif

131
libraries/AP_HAL_Linux/Util.h
View File

@ -5,13 +5,121 @@
#include <AP_HAL.h>
#include "AP_HAL_Linux_Namespace.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
class Linux::LinuxUtil : public AP_HAL::Util {
public:
void init(int argc, char * const *argv) {
saved_argc = argc;
saved_argv = argv;
toneAlarm();
}
bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
/**
@ -19,9 +127,30 @@ public:
*/
void commandline_arguments(uint8_t &argc, char * const *&argv);
private:
uint8_t toneAlarm_init();
void toneAlarm_set_tune(uint8_t tune);
void _toneAlarm_timer_tick();
private:
void toneAlarm(void);
void stop();
void play(int tone,int duration);
void play_tune();
const char *tune[TONE_NUMBER_OF_TUNES];
bool tune_repeat[TONE_NUMBER_OF_TUNES];
bool tune_changed;
uint32_t prev_tune_num;
int32_t period_fd;
int32_t duty_fd;
int32_t run_fd;
uint8_t tune_num;
int saved_argc;
char* const *saved_argv;
};
#endif // __AP_HAL_LINUX_UTIL_H__