ardupilot/libraries/AP_HAL_ChibiOS/ToneAlarm.cpp

#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