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

HAL_ChibiOS: support playing tunes by string

This commit is contained in:
Andrew Tridgell 2018-07-19 10:38:13 +10:00
parent ba7750de15
commit 4244daeb2d
4 changed files with 76 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
libraries/AP_HAL_ChibiOS/ToneAlarm.cpp
View File

@ -4,6 +4,7 @@
#ifdef HAL_PWM_ALARM
#include "ToneAlarm.h"
#include <AP_Math/crc.h>
using namespace ChibiOS;
@ -23,13 +24,13 @@ static uint16_t notes[] = { 0,
//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",
"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: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",
"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",
@ -37,12 +38,9 @@ const char* ToneAlarm::tune[TONE_NUMBER_OF_TUNES] = {
"modechangesoft:d=4,o=6,b=400:8e",
};
//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()
{
tune_num = -1; //initially no tune to play
tune_str[0] = 0; //initially no tune to play
tune_pos = 0;
}
@ -52,14 +50,33 @@ bool ToneAlarm::init()
pwm_group.pwm_cfg.period = 1000;
pwmStart(pwm_group.pwm_drv, &pwm_group.pwm_cfg);
tune_num = 0; //play startup tune
//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)
{
tune_num = tone;
if (tone < ARRAY_SIZE(tune)) {
set_tune_string(tune[tone]);
} else {
set_tune_string("");
}
}
bool ToneAlarm::is_tune_comp()
@ -76,7 +93,7 @@ void ToneAlarm::stop()
bool ToneAlarm::play()
{
const uint32_t cur_time = AP_HAL::millis();
if(tune_num != prev_tune_num) {
if (tune_crc != prev_tune_crc) {
stop();
tune_changed = true;
tune_pos = 0;
@ -99,11 +116,10 @@ bool ToneAlarm::play()
// yes, stop the PWM signal
stop();
// was that the last note?
if(tune[tune_num][tune_pos] == '\0') {
if(tune_str[tune_pos] == '\0') {
// this was the last note
// if this is not a repeating tune, disable playback
if(!tune_repeat[tune_num]){
tune_num = -1;
if (!repeat) {
tune_str[0] = 0;
}
// reset tune spec index to zero: this is the only place tune_pos is reset
tune_pos = 0;
@ -123,9 +139,14 @@ bool ToneAlarm::set_note()
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
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[tune_num][tune_pos++] - '0');
num = (num * 10) + (tune_str[tune_pos++] - '0');
}
if(num){
duration = wholenote / num;
@ -135,7 +156,7 @@ bool ToneAlarm::set_note()
// now get the note
note = 0;
switch(tune[tune_num][tune_pos]){
switch(tune_str[tune_pos]){
case 'c':
note = 1;
break;
@ -165,22 +186,22 @@ bool ToneAlarm::set_note()
tune_pos++;
// now, get optional '#' sharp
if(tune[tune_num][tune_pos] == '#'){
if(tune_str[tune_pos] == '#'){
note++;
tune_pos++;
}
// now, get optional '.' dotted note
if(tune[tune_num][tune_pos] == '.'){
if(tune_str[tune_pos] == '.'){
duration += duration/2;
tune_pos++;
}
// now, get scale
if(isdigit(tune[tune_num][tune_pos])){
scale = tune[tune_num][tune_pos] - '0';
if(isdigit(tune_str[tune_pos])){
scale = tune_str[tune_pos] - '0';
tune_pos++;
} else{
scale = default_oct;
@ -188,7 +209,7 @@ bool ToneAlarm::set_note()
scale += OCTAVE_OFFSET;
if(tune[tune_num][tune_pos] == ','){
if(tune_str[tune_pos] == ','){
tune_pos++; // skip comma for next note (or we may be at the end)
}
// now play the note
@ -209,27 +230,28 @@ bool ToneAlarm::init_tune()
default_dur = 4;
default_oct = 6;
bpm = 63;
prev_tune_num = tune_num;
prev_tune_crc = tune_crc;
tune_changed = false;
if(tune_num <0 || tune_num > TONE_NUMBER_OF_TUNES){
if (tune_str[0] == 0) {
return false;
}
tune_comp = false;
while(tune[tune_num][tune_pos] != ':'){
if(tune[tune_num][tune_pos] == '\0'){
while(tune_str[tune_pos] != ':'){
if(tune_str[tune_pos] == '\0'){
return false;
}
tune_pos++;
}
tune_pos++;
if(tune[tune_num][tune_pos] == 'd'){
if(tune_str[tune_pos] == 'd'){
tune_pos+=2;
num = 0;
while(isdigit(tune[tune_num][tune_pos])){
num = (num * 10) + (tune[tune_num][tune_pos++] - '0');
while(isdigit(tune_str[tune_pos])){
num = (num * 10) + (tune_str[tune_pos++] - '0');
}
if(num > 0){
default_dur = num;
@ -240,10 +262,10 @@ bool ToneAlarm::init_tune()
// get default octave
if(tune[tune_num][tune_pos] == 'o')
if(tune_str[tune_pos] == 'o')
{
tune_pos+=2; // skip "o="
num = tune[tune_num][tune_pos++] - '0';
num = tune_str[tune_pos++] - '0';
if(num >= 3 && num <=7){
default_oct = num;
}
@ -252,11 +274,11 @@ bool ToneAlarm::init_tune()
// get BPM
if(tune[tune_num][tune_pos] == 'b'){
if(tune_str[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');
while(isdigit(tune_str[tune_pos])){
num = (num * 10) + (tune_str[tune_pos++] - '0');
}
bpm = num;
tune_pos++; // skip colon
@ -267,4 +289,4 @@ bool ToneAlarm::init_tune()
return true;
}
#endif
#endif

8
libraries/AP_HAL_ChibiOS/ToneAlarm.h
View File

@ -121,6 +121,7 @@ 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();
@ -130,8 +131,9 @@ public:
protected:
bool tune_comp;
char tune_str[100];
bool repeat;
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;
@ -139,9 +141,9 @@ protected:
uint16_t wholenote;
uint16_t cur_note;
uint16_t duration;
int32_t prev_tune_num;
uint32_t tune_crc;
uint32_t prev_tune_crc;
uint32_t prev_time;
int8_t tune_num;
uint8_t tune_pos;
private:

5
libraries/AP_HAL_ChibiOS/Util.cpp
View File

@ -157,6 +157,11 @@ void Util::toneAlarm_set_tune(uint8_t tone)
_toneAlarm.set_tune(tone);
}
void Util::toneAlarm_set_tune_string(const char *str)
{
_toneAlarm.set_tune_string(str);
}
// (state 0) if init_tune() -> (state 1) complete=false
// (state 1) if set_note -> (state 2) -> if play -> (state 3)
// play returns true if tune has changed or tune is complete (repeating tunes never complete)

17
libraries/AP_HAL_ChibiOS/Util.h
View File

@ -27,13 +27,13 @@ public:
return static_cast<Util*>(util);
}
bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
bool run_debug_shell(AP_HAL::BetterStream *stream) override { return false; }
AP_HAL::Semaphore *new_semaphore(void) override { return new ChibiOS::Semaphore; }
uint32_t available_memory() override;
// Special Allocation Routines
void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type);
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type);
void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override;
void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override;
/*
return state of safety switch, if applicable
@ -41,16 +41,17 @@ public:
enum safety_state safety_switch_state(void) override;
// IMU temperature control
void set_imu_temp(float current);
void set_imu_target_temp(int8_t *target);
void set_imu_temp(float current) override;
void set_imu_target_temp(int8_t *target) override;
// get system ID as a string
bool get_system_id(char buf[40]) override;
#ifdef HAL_PWM_ALARM
bool toneAlarm_init();
void toneAlarm_set_tune(uint8_t tone);
void _toneAlarm_timer_tick();
bool toneAlarm_init() override;
void toneAlarm_set_tune(uint8_t tone) override;
void toneAlarm_set_tune_string(const char *str) override;
void _toneAlarm_timer_tick() override;
static ToneAlarm& get_ToneAlarm() { return _toneAlarm; }
#endif