diff --git a/src/drivers/samv7/tone_alarm/tone_alarm.cpp b/src/drivers/samv7/tone_alarm/tone_alarm.cpp index 2dbd1debcd..ce9e7ce1b8 100644 --- a/src/drivers/samv7/tone_alarm/tone_alarm.cpp +++ b/src/drivers/samv7/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013, 2018 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,62 +31,14 @@ * ****************************************************************************/ -/** - * Driver for the PX4 audio alarm port, /dev/tone_alarm. - * - * The tone_alarm driver supports a set of predefined "alarm" - * tunes and one user-supplied tune. - * - * The TONE_SET_ALARM ioctl can be used to select a predefined - * alarm tune, from 1 - . Selecting tune zero silences - * the alarm. - * - * Tunes follow the syntax of the Microsoft GWBasic/QBasic PLAY - * statement, with some exceptions and extensions. - * - * From Wikibooks: - * - * PLAY "[string expression]" - * - * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) - * immediately after the note letter. See this example: - * - * PLAY "C C# C C#" - * - * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators - * that change the properties are effective for the notes following that indicator. - * - * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration - * amount but rather a note type; L1 - whole note, L2 - half note, L4 - quarter note, etc. - * (L8, L16, L32, L64, ...). By default, n = 4. - * For triplets and quintets, use L3, L6, L12, ... and L5, L10, L20, ... series respectively. - * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" - * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. - * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. - * < > Changes the current octave respectively down or up one level. - * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) - * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. - * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. - * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. - * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. - * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. - * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. - * It can be used for a pause as well. - * - * Extensions/variations: - * - * MB MF The MF command causes the tune to play once and then stop. The MB command causes the - * tune to repeat when it ends. - * +/* + * Low Level Driver for the PX4 audio alarm port. Subscribes to + * tune_control and plays notes on this architecture specific + * timer HW */ #include +#include #include #include @@ -118,6 +70,13 @@ #include #include +#include + +#include +#include +#include + + /* Check that tone alarm and HRT timers are different */ #if defined(TONE_ALARM_CHANNEL) && defined(HRT_TIMER_CHANNEL) # if TONE_ALARM_CHANNEL == HRT_TIMER_CHANNEL @@ -212,16 +171,10 @@ class ToneAlarm : public cdev::CDev { public: ToneAlarm(); - ~ToneAlarm() = default; + ~ToneAlarm(); - virtual int init(); - - virtual int ioctl(file *filp, int cmd, unsigned long arg); - virtual ssize_t write(file *filp, const char *buffer, size_t len); - inline const char *name(int tune) - { - return _tune_names[tune]; - } + virtual int init(); + void status(); enum { CBRK_OFF = 0, @@ -230,81 +183,44 @@ public: }; private: - static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char *_default_tunes[TONE_NUMBER_OF_TUNES]; - const char *_tune_names[TONE_NUMBER_OF_TUNES]; - static const uint8_t _note_tab[]; + volatile bool _running; + volatile bool _should_run; + bool _play_tone; - unsigned _default_tune_number; // number of currently playing default tune (0 for none) + Tunes _tunes; - const char *_user_tune; + unsigned _silence_length; // if nonzero, silence before next note - const char *_tune; // current tune string - const char *_next; // next note in the string + int _cbrk; ///< if true, no audio output + int _tune_control_sub; - unsigned _tempo; - unsigned _note_length; - enum { MODE_NORMAL, MODE_LEGATO, MODE_STACCATO} _note_mode; - unsigned _octave; - unsigned _silence_length; // if nonzero, silence before next note - bool _repeat; // if true, tune restarts at end - int _cbrk; //if true, no audio output + tune_control_s _tune; - hrt_call _note_call; // HRT callout for note completion + static work_s _work; - // Convert a note value in the range C1 to B7 into a divisor for - // the configured timer's clock. + // Convert a frequency value into a divisor for the configured timer's clock. // - unsigned note_to_divisor(unsigned note); - - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of - // dots following in the play string. - // - unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); - - // Calculate the duration in microseconds of a rest corresponding to - // a given note length. - // - unsigned rest_duration(unsigned rest_length, unsigned dots); + unsigned frequency_to_divisor(unsigned frequency); // Start playing the note // - void start_note(unsigned note); + void start_note(unsigned frequency); // Stop playing the current note and make the player 'safe' // - void stop_note(); - - // Start playing the tune - // - void start_tune(const char *tune); + void stop_note(); // Parse the next note out of the string and play it // - void next_note(); + void next_note(); - // Find the next character in the string, discard any whitespace and - // return the canonical (uppercase) version. + // work queue trampoline for next_note // - int next_char(); - - // Extract a number from the string, consuming all the digit characters. - // - unsigned next_number(); - - // Consume dot characters from the string, returning the number consumed. - // - unsigned next_dots(); - - // hrt_call trampoline for next_note - // - static void next_trampoline(void *arg); + static void next_trampoline(void *arg); }; -// semitone offsets from C for the characters 'A'-'G' -const uint8_t ToneAlarm::_note_tab[] = {9, 11, 0, 2, 4, 5, 7}; +struct work_s ToneAlarm::_work = {}; /* * Driver 'main' command. @@ -313,50 +229,30 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev(TONEALARM0_DEVICE_PATH), - _default_tune_number(0), - _user_tune(nullptr), - _tune(nullptr), - _next(nullptr), - _cbrk(CBRK_UNINIT) + CDev(TONEALARM0_DEVICE_PATH), + _running(false), + _should_run(true), + _play_tone(false), + _tunes(), + _silence_length(0), + _cbrk(CBRK_UNINIT), + _tune_control_sub(-1) { // enable debug() calls //_debug_enabled = true; - _default_tunes[TONE_STARTUP_TUNE] = "MFT240L8 O4aO5dc O4aO5dc O4aO5dc L16dcdcdcdc"; // startup tune - _default_tunes[TONE_ERROR_TUNE] = "MBT200a8a8a8PaaaP"; // ERROR tone - _default_tunes[TONE_NOTIFY_POSITIVE_TUNE] = "MFT200e8a8a"; // Notify Positive tone - _default_tunes[TONE_NOTIFY_NEUTRAL_TUNE] = "MFT200e8e"; // Notify Neutral tone - _default_tunes[TONE_NOTIFY_NEGATIVE_TUNE] = "MFT200e8c8e8c8e8c8"; // Notify Negative tone - _default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning - _default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow - _default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast - _default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow - _default_tunes[TONE_ARMING_FAILURE_TUNE] = "MFT255L4<<= 0) { + orb_unsubscribe(_tune_control_sub); + } - // record the tune - _tune = tune; - _next = tune; + _running = false; + return; + } - // initialise player state - _tempo = 120; - _note_length = 4; - _note_mode = MODE_NORMAL; - _octave = 4; - _silence_length = 0; - _repeat = false; // otherwise command-line tunes repeat forever... + // subscribe to tune_control + if (_tune_control_sub < 0) { + _tune_control_sub = orb_subscribe(ORB_ID(tune_control)); + } - // schedule a callback to start playing - hrt_call_after(&_note_call, 0, (hrt_callout)next_trampoline, this); -} - -void -ToneAlarm::next_note() -{ // do we have an inter-note gap to wait for? if (_silence_length > 0) { stop_note(); - hrt_call_after(&_note_call, (hrt_abstime)_silence_length, (hrt_callout)next_trampoline, this); + work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(_silence_length)); _silence_length = 0; return; } - // make sure we still have a tune - may be removed by the write / ioctl handler - if ((_next == nullptr) || (_tune == nullptr)) { - stop_note(); - return; - } + // check for updates + bool updated = false; + orb_check(_tune_control_sub, &updated); - // parse characters out of the string until we have resolved a note - unsigned note = 0; - unsigned note_length = _note_length; - unsigned duration; + if (updated) { + orb_copy(ORB_ID(tune_control), _tune_control_sub, &_tune); - while (note == 0) { - // we always need at least one character from the string - int c = next_char(); - - if (c == 0) { - goto tune_end; - } - - _next++; - - switch (c) { - case 'L': // select note length - _note_length = next_number(); - - if (_note_length < 1) { - goto tune_error; - } - - break; - - case 'O': // select octave - _octave = next_number(); - - if (_octave > 6) { - _octave = 6; - } - - break; - - case '<': // decrease octave - if (_octave > 0) { - _octave--; - } - - break; - - case '>': // increase octave - if (_octave < 6) { - _octave++; - } - - break; - - case 'M': // select inter-note gap - c = next_char(); - - if (c == 0) { - goto tune_error; - } - - _next++; - - switch (c) { - case 'N': - _note_mode = MODE_NORMAL; - break; - - case 'L': - _note_mode = MODE_LEGATO; - break; - - case 'S': - _note_mode = MODE_STACCATO; - break; - - case 'F': - _repeat = false; - break; - - case 'B': - _repeat = true; - break; - - default: - goto tune_error; - } - - break; - - case 'P': // pause for a note length - stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); - return; - - case 'T': { // change tempo - unsigned nt = next_number(); - - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - - } else { - goto tune_error; - } - - break; - } - - case 'N': // play an arbitrary note - note = next_number(); - - if (note > 84) { - goto tune_error; - } - - if (note == 0) { - // this is a rest - pause for the current note length - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; - } - - break; - - case 'A'...'G': // play a note in the current octave - note = _note_tab[c - 'A'] + (_octave * 12) + 1; - c = next_char(); - - switch (c) { - case '#': // up a semitone - case '+': - if (note < 84) { - note++; - } - - _next++; - break; - - case '-': // down a semitone - if (note > 1) { - note--; - } - - _next++; - break; - - default: - // 0 / no next char here is OK - break; - } - - // shorthand length notation - note_length = next_number(); - - if (note_length == 0) { - note_length = _note_length; - } - - break; - - default: - goto tune_error; - } - } - - // compute the duration of the note and the following silence (if any) - duration = note_duration(_silence_length, note_length, next_dots()); - - // start playing the note - start_note(note); - - // and arrange a callback when the note should stop - hrt_call_after(&_note_call, (hrt_abstime)duration, (hrt_callout)next_trampoline, this); - return; - - // tune looks bad (unexpected EOF, bad character, etc.) -tune_error: - syslog(LOG_ERR, "tune error\n"); - _repeat = false; // don't loop on error - - // stop (and potentially restart) the tune -tune_end: - stop_note(); - - if (_repeat) { - start_tune(_tune); - - } else { - _tune = nullptr; - _default_tune_number = 0; - } - - return; -} - -int -ToneAlarm::next_char() -{ - while (isspace(*_next)) { - _next++; - } - - return toupper(*_next); -} - -unsigned -ToneAlarm::next_number() -{ - unsigned number = 0; - int c; - - for (;;) { - c = next_char(); - - if (!isdigit(c)) { - return number; - } - - _next++; - number = (number * 10) + (c - '0'); - } -} - -unsigned -ToneAlarm::next_dots() -{ - unsigned dots = 0; - - while (next_char() == '.') { - _next++; - dots++; - } - - return dots; -} - -void -ToneAlarm::next_trampoline(void *arg) -{ - ToneAlarm *ta = (ToneAlarm *)arg; - - ta->next_note(); -} - - -int -ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) -{ - int result = OK; - - PX4_DEBUG("ioctl %i %u", cmd, arg); - -// irqstate_t flags = enter_critical_section(); - - /* decide whether to increase the alarm level to cmd or leave it alone */ - switch (cmd) { - case TONE_SET_ALARM: - PX4_DEBUG("TONE_SET_ALARM %u", arg); - - if (arg < TONE_NUMBER_OF_TUNES) { - if (arg == TONE_STOP_TUNE) { - // stop the tune - _tune = nullptr; - _next = nullptr; - _repeat = false; - _default_tune_number = 0; - - } else { - /* always interrupt alarms, unless they are repeating and already playing */ - if (!(_repeat && _default_tune_number == arg)) { - /* play the selected tune */ - _default_tune_number = arg; - start_tune(_default_tunes[arg]); - } - } + if (_tunes.set_control(_tune) == 0) { + _play_tone = true; } else { - result = -EINVAL; + _play_tone = false; + } + } + + unsigned frequency = 0, duration = 0; + + if (_play_tone) { + _play_tone = false; + int parse_ret_val = _tunes.get_next_tune(frequency, duration, _silence_length); + + if (parse_ret_val >= 0) { + // a frequency of 0 correspond to stop_note + if (frequency > 0) { + // start playing the note + start_note(frequency); + + } else { + stop_note(); + } + + + if (parse_ret_val > 0) { + // continue playing + _play_tone = true; + } } - break; - - default: - result = -ENOTTY; - break; + } else { + // schedule a call with the tunes max interval + duration = _tunes.get_maximum_update_interval(); + // stop playing the last note after the duration elapsed + stop_note(); } -// leave_critical_section(flags); - - /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) { - result = CDev::ioctl(filp, cmd, arg); - } - - return result; + // and arrange a callback when the note should stop + work_queue(HPWORK, &_work, (worker_t)&ToneAlarm::next_trampoline, this, USEC2TICK(duration)); } -int -ToneAlarm::write(file *filp, const char *buffer, size_t len) +void ToneAlarm::next_trampoline(void *arg) { - // sanity-check the buffer for length and nul-termination - if (len > _tune_max) { - return -EFBIG; - } - - // if we have an existing user tune, free it - if (_user_tune != nullptr) { - - // if we are playing the user tune, stop - if (_tune == _user_tune) { - _tune = nullptr; - _next = nullptr; - } - - // free the old user tune - free((void *)_user_tune); - _user_tune = nullptr; - } - - // if the new tune is empty, we're done - if (buffer[0] == '\0') { - return OK; - } - - // allocate a copy of the new tune - _user_tune = strndup(buffer, len); - - if (_user_tune == nullptr) { - return -ENOMEM; - } - - // and play it - start_tune(_user_tune); - - return len; + ToneAlarm *ta = (ToneAlarm *)arg; + ta->next_note(); } /** @@ -898,130 +445,60 @@ namespace ToneAlarm *g_dev; -int -play_tune(unsigned tune) -{ - exit(0); - - int fd, ret; - - fd = open(TONEALARM0_DEVICE_PATH, 0); - - if (fd < 0) { - err(1, TONEALARM0_DEVICE_PATH); - } - - ret = ioctl(fd, TONE_SET_ALARM, tune); - close(fd); - - if (ret != 0) { - err(1, "TONE_SET_ALARM"); - } - - exit(0); -} - -int -play_string(const char *str, bool free_buffer) -{ - int fd, ret; - - fd = open(TONEALARM0_DEVICE_PATH, O_WRONLY); - - if (fd < 0) { - err(1, TONEALARM0_DEVICE_PATH); - } - - ret = write(fd, str, strlen(str) + 1); - close(fd); - - if (free_buffer) { - free((void *)str); - } - - if (ret < 0) { - err(1, "play tune"); - } - - exit(0); -} - } // namespace -int -tone_alarm_main(int argc, char *argv[]) +void tone_alarm_usage(); + +void tone_alarm_usage() { - unsigned tune = 0; + PX4_INFO("missing command, try 'start', status, 'stop'"); +} - /* start the driver lazily */ - if (g_dev == nullptr) { - g_dev = new ToneAlarm; - - if (g_dev == nullptr) { - errx(1, "couldn't allocate the ToneAlarm driver"); - } - - if (g_dev->init() != OK) { - delete g_dev; - errx(1, "ToneAlarm init failed"); - } - } +int tone_alarm_main(int argc, char *argv[]) +{ if (argc > 1) { const char *argv1 = argv[1]; if (!strcmp(argv1, "start")) { - play_tune(TONE_STOP_TUNE); + if (g_dev != nullptr) { + PX4_ERR("already started"); + exit(1); + } + + if (g_dev == nullptr) { + g_dev = new ToneAlarm(); + + if (g_dev == nullptr) { + PX4_ERR("couldn't allocate the ToneAlarm driver"); + exit(1); + } + + if (OK != g_dev->init()) { + delete g_dev; + g_dev = nullptr; + PX4_ERR("ToneAlarm init failed"); + exit(1); + } + } + + exit(0); } if (!strcmp(argv1, "stop")) { - play_tune(TONE_STOP_TUNE); + delete g_dev; + g_dev = nullptr; + exit(0); } - if ((tune = strtol(argv1, nullptr, 10)) != 0) { - play_tune(tune); - } - - /* It might be a tune name */ - for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) - if (!strcmp(g_dev->name(tune), argv1)) { - play_tune(tune); - } - - /* If it is a file name then load and play it as a string */ - if (*argv1 == '/') { - FILE *fd = fopen(argv1, "r"); - int sz; - char *buffer; - - if (fd == nullptr) { - errx(1, "couldn't open '%s'", argv1); - } - - fseek(fd, 0, SEEK_END); - sz = ftell(fd); - fseek(fd, 0, SEEK_SET); - buffer = (char *)malloc(sz + 1); - - if (buffer == nullptr) { - errx(1, "not enough memory memory"); - } - - fread(buffer, sz, 1, fd); - /* terminate the string */ - buffer[sz] = 0; - play_string(buffer, true); - } - - /* if it looks like a PLAY string... */ - if (strlen(argv1) > 2) { - if (*argv1 == 'M') { - play_string(argv1, false); - } + if (!strcmp(argv1, "status")) { + g_dev->status(); + exit(0); } } - errx(1, "unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); + tone_alarm_usage(); + exit(0); } #endif /* TONE_ALARM_CHANNEL */