Jonathan Challinger
7 years ago
committed by
Andrew Tridgell
6 changed files with 19 additions and 520 deletions
@ -1,292 +0,0 @@
@@ -1,292 +0,0 @@
|
||||
#include <AP_HAL/AP_HAL.h> |
||||
#include <AP_BoardConfig/AP_BoardConfig.h> |
||||
|
||||
#ifdef HAL_PWM_ALARM |
||||
|
||||
#include "ToneAlarm.h" |
||||
#include <AP_Math/crc.h> |
||||
|
||||
using namespace ChibiOS; |
||||
|
||||
struct ToneAlarm::pwmGroup ToneAlarm::pwm_group = HAL_PWM_ALARM; |
||||
|
||||
#define isdigit(n) (n >= '0' && n <= '9') |
||||
|
||||
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,pR", |
||||
"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:8aR", |
||||
"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,8aR", |
||||
"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", |
||||
"modechangeloud:d=4,o=6,b=400:8e", |
||||
"modechangesoft:d=4,o=6,b=400:8e", |
||||
}; |
||||
|
||||
ToneAlarm::ToneAlarm() |
||||
{ |
||||
tune_str[0] = 0; //initially no tune to play
|
||||
tune_pos = 0; |
||||
} |
||||
|
||||
bool ToneAlarm::init() |
||||
{ |
||||
// start PWM driver
|
||||
pwm_group.pwm_cfg.period = 1000; |
||||
pwmStart(pwm_group.pwm_drv, &pwm_group.pwm_cfg); |
||||
|
||||
//play startup tune
|
||||
set_tune(0); |
||||
|
||||
return true; |
||||
} |
||||
|
||||
void ToneAlarm::set_tune_string(const char *str) |
||||
{ |
||||
tune_crc = 0; |
||||
strncpy(tune_str, str, sizeof(tune_str)); |
||||
tune_str[sizeof(tune_str)-1] = 0; |
||||
tune_crc = crc_crc32(0, (const uint8_t *)tune_str, strlen(tune_str)); |
||||
if (tune_str[strlen(tune_str)-1] == 'R') { |
||||
repeat = true; |
||||
tune_str[strlen(tune_str)-1] = 0; |
||||
} else { |
||||
repeat = false; |
||||
} |
||||
} |
||||
|
||||
void ToneAlarm::set_tune(uint8_t tone) |
||||
{ |
||||
if (tone < ARRAY_SIZE(tune)) { |
||||
set_tune_string(tune[tone]); |
||||
} else { |
||||
set_tune_string(""); |
||||
} |
||||
} |
||||
|
||||
bool ToneAlarm::is_tune_comp() |
||||
{ |
||||
return tune_comp; |
||||
} |
||||
|
||||
void ToneAlarm::stop() |
||||
{ |
||||
pwmDisableChannel(pwm_group.pwm_drv, pwm_group.chan); |
||||
|
||||
} |
||||
|
||||
bool ToneAlarm::play() |
||||
{ |
||||
const uint32_t cur_time = AP_HAL::millis(); |
||||
if (tune_crc != prev_tune_crc) { |
||||
stop(); |
||||
tune_changed = true; |
||||
tune_pos = 0; |
||||
tune_comp = true; |
||||
return false; |
||||
} |
||||
if(cur_note != 0) { |
||||
// specify alarm timer and channel in hwdef.dat
|
||||
pwmChangePeriod(pwm_group.pwm_drv, |
||||
pwm_group.pwm_cfg.frequency/cur_note); |
||||
|
||||
pwmEnableChannel(pwm_group.pwm_drv, pwm_group.chan, |
||||
(pwm_group.pwm_cfg.frequency/2)/cur_note); |
||||
|
||||
cur_note = 0; |
||||
prev_time = cur_time; |
||||
} |
||||
// has note duration elapsed?
|
||||
if((cur_time - prev_time) > duration) { |
||||
// yes, stop the PWM signal
|
||||
stop(); |
||||
// was that the last note?
|
||||
if(tune_str[tune_pos] == '\0') { |
||||
// this was the last note
|
||||
if (!repeat) { |
||||
tune_str[0] = 0; |
||||
} |
||||
// reset tune spec index to zero: this is the only place tune_pos is reset
|
||||
tune_pos = 0; |
||||
tune_comp = true; |
||||
// indicate tune is complete by returning false
|
||||
return false; |
||||
} |
||||
// indicate tune is still playing by returning true
|
||||
return true; |
||||
} |
||||
return false; |
||||
} |
||||
|
||||
bool ToneAlarm::set_note() |
||||
{ |
||||
// first, get note duration, if available
|
||||
uint16_t scale,note,num =0; |
||||
duration = 0; |
||||
|
||||
if (tune_str[tune_pos] == 'R' && tune_str[tune_pos+1] == 0) { |
||||
init_tune(); |
||||
return false; |
||||
} |
||||
|
||||
while(isdigit(tune_str[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_str[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_str[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_str[tune_pos] == '#'){ |
||||
note++; |
||||
tune_pos++; |
||||
} |
||||
|
||||
// now, get optional '.' dotted note
|
||||
|
||||
if(tune_str[tune_pos] == '.'){ |
||||
duration += duration/2; |
||||
tune_pos++; |
||||
} |
||||
|
||||
// now, get scale
|
||||
|
||||
if(isdigit(tune_str[tune_pos])){ |
||||
scale = tune_str[tune_pos] - '0'; |
||||
tune_pos++; |
||||
} else{ |
||||
scale = default_oct; |
||||
} |
||||
|
||||
scale += OCTAVE_OFFSET; |
||||
|
||||
if(tune_str[tune_pos] == ','){ |
||||
tune_pos++; // skip comma for next note (or we may be at the end)
|
||||
} |
||||
// now play the note
|
||||
|
||||
if(note){ |
||||
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_crc = tune_crc; |
||||
tune_changed = false; |
||||
|
||||
if (tune_str[0] == 0) { |
||||
return false; |
||||
} |
||||
|
||||
tune_comp = false; |
||||
while(tune_str[tune_pos] != ':'){ |
||||
if(tune_str[tune_pos] == '\0'){ |
||||
return false; |
||||
} |
||||
tune_pos++; |
||||
} |
||||
tune_pos++; |
||||
|
||||
if(tune_str[tune_pos] == 'd'){ |
||||
tune_pos+=2; |
||||
num = 0; |
||||
|
||||
while(isdigit(tune_str[tune_pos])){ |
||||
num = (num * 10) + (tune_str[tune_pos++] - '0'); |
||||
} |
||||
if(num > 0){ |
||||
default_dur = num; |
||||
} |
||||
tune_pos++; // skip comma
|
||||
} |
||||
|
||||
|
||||
// get default octave
|
||||
|
||||
if(tune_str[tune_pos] == 'o') |
||||
{ |
||||
tune_pos+=2; // skip "o="
|
||||
num = tune_str[tune_pos++] - '0'; |
||||
if(num >= 3 && num <=7){ |
||||
default_oct = num; |
||||
} |
||||
tune_pos++; // skip comma
|
||||
} |
||||
|
||||
// get BPM
|
||||
|
||||
if(tune_str[tune_pos] == 'b'){ |
||||
tune_pos+=2; // skip "b="
|
||||
num = 0; |
||||
while(isdigit(tune_str[tune_pos])){ |
||||
num = (num * 10) + (tune_str[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; |
||||
} |
||||
#endif |
||||
@ -1,159 +0,0 @@
@@ -1,159 +0,0 @@
|
||||
#pragma once |
||||
|
||||
#include "AP_HAL_ChibiOS.h" |
||||
|
||||
#include "ch.h" |
||||
#include "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_NOTIFY_MODE_CHANGE_LOUD 11 |
||||
#define TONE_NOTIFY_MODE_CHANGE_SOFT 12 |
||||
|
||||
#define TONE_NUMBER_OF_TUNES 13 |
||||
|
||||
#ifdef HAL_PWM_ALARM |
||||
|
||||
namespace ChibiOS { |
||||
|
||||
class ToneAlarm { |
||||
public: |
||||
ToneAlarm(); |
||||
void set_tune(uint8_t tone); |
||||
void set_tune_string(const char *str); |
||||
virtual bool init(); |
||||
virtual void stop(); |
||||
virtual bool play(); |
||||
bool is_tune_comp(); |
||||
bool set_note(); |
||||
bool init_tune(); |
||||
|
||||
protected: |
||||
bool tune_comp; |
||||
char tune_str[100]; |
||||
bool repeat; |
||||
static const char *tune[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; |
||||
uint32_t tune_crc; |
||||
uint32_t prev_tune_crc; |
||||
uint32_t prev_time; |
||||
uint8_t tune_pos; |
||||
|
||||
private: |
||||
struct pwmGroup { |
||||
pwmchannel_t chan; |
||||
PWMConfig pwm_cfg; |
||||
PWMDriver* pwm_drv; |
||||
}; |
||||
static pwmGroup pwm_group; |
||||
}; |
||||
|
||||
} |
||||
#endif // HAL_PWM_ALARM
|
||||
Loading…
Reference in new issue