diff --git a/src/lib/tunes/tunes.cpp b/src/lib/tunes/tunes.cpp index 1bacaa696cf9..16be96efd523 100644 --- a/src/lib/tunes/tunes.cpp +++ b/src/lib/tunes/tunes.cpp @@ -59,14 +59,14 @@ Tunes::Tunes(unsigned default_tempo, unsigned default_octave, unsigned default_n _default_mode(default_mode), _default_octave(default_octave) { - config_tone(false); + reset(false); } -Tunes::Tunes(): Tunes(120, 4, 4, NoteMode::NORMAL) +Tunes::Tunes(): Tunes(TUNE_DEFAULT_TEMPO, TUNE_DEFAULT_OCTAVE, TUNE_DEFAULT_NOTE_LENGTH, NoteMode::NORMAL) { } -void Tunes::config_tone(bool repeat_flag) +void Tunes::reset(bool repeat_flag) { // reset pointer if (!repeat_flag) { @@ -87,58 +87,89 @@ void Tunes::config_tone(bool repeat_flag) int Tunes::set_control(const tune_control_s &tune_control) { - bool reset_playing_tune = false; + // Sanity check + if (tune_control.tune_id >= _default_tunes_size) { + return -EINVAL; + } - if (tune_control.tune_id < _default_tunes_size) { - switch (tune_control.tune_id) { - case static_cast(TuneID::CUSTOM): - _frequency = (unsigned)tune_control.frequency; - _duration = (unsigned)tune_control.duration; - _silence = (unsigned)tune_control.silence; - _using_custom_msg = true; - break; + // Accept new tune ? + if (_repeat || // Repeated tunes can always be interrupted + _tune == nullptr || // No tune is currently being played + tune_control.tune_override || // Override interrupts everything + tune_control.tune_id == static_cast(TuneID::STARTUP) || + tune_control.tune_id == static_cast(TuneID::ERROR_TUNE) || + tune_control.tune_id == static_cast(TuneID::NOTIFY_POSITIVE) || + tune_control.tune_id == static_cast(TuneID::NOTIFY_NEUTRAL) || + tune_control.tune_id == static_cast(TuneID::NOTIFY_NEGATIVE)) { - // tunes that have a high priority - case static_cast(TuneID::STARTUP): - case static_cast(TuneID::ERROR_TUNE): - case static_cast(TuneID::NOTIFY_POSITIVE): - case static_cast(TuneID::NOTIFY_NEUTRAL): - case static_cast(TuneID::NOTIFY_NEGATIVE): - reset_playing_tune = true; - config_tone(false); + // Reset repeat flag. Can jump to true again while tune is being parsed later + _repeat = false; - /* FALLTHROUGH */ - default: + // Reset octave, tempo etc. + reset(_repeat); - // TODO: come up with a better strategy - if (_tune == nullptr || reset_playing_tune || tune_control.tune_override) { - _tune = _default_tunes[tune_control.tune_id]; - _tune_start_ptr = _default_tunes[tune_control.tune_id]; - _next = _tune; - } + // Strength will remain valid for the entire tune, unless interrupted. + if ((unsigned)tune_control.strength <= TUNE_MAX_STRENGTH) { + _strength = (unsigned)tune_control.strength; - break; + } else { + _strength = TUNE_MAX_STRENGTH; } - } else { - PX4_WARN("Tune ID not recognized."); - return -EINVAL; + // Special treatment for custom tunes + if (tune_control.tune_id == static_cast(TuneID::CUSTOM)) { + _using_custom_msg = true; + _frequency = (unsigned)tune_control.frequency; + _duration = (unsigned)tune_control.duration; + _silence = (unsigned)tune_control.silence; + + } else { + _using_custom_msg = false; + _tune = _default_tunes[tune_control.tune_id]; + _tune_start_ptr = _default_tunes[tune_control.tune_id]; + _next = _tune; + } } return OK; } -void Tunes::set_string(const char *string) +void Tunes::set_string(const char *const string, uint8_t strength) { - // set tune string the first time + // Only play new tune if nothing is being played currently if (_tune == nullptr) { + // set tune string the first time _tune = string; _tune_start_ptr = string; _next = _tune; + + if (strength <= TUNE_MAX_STRENGTH) { + _strength = strength; + + } else { + _strength = TUNE_MAX_STRENGTH; + } } } -int Tunes::get_next_tune(unsigned &frequency, unsigned &duration, unsigned &silence) +int Tunes::get_next_tune(unsigned &frequency, unsigned &duration, + unsigned &silence, uint8_t &strength) +{ + int ret = get_next_tune(frequency, duration, silence); + + // Check if note should not be heard -> adjust strength to 0 to be safe + if (frequency == 0 || duration == 0) { + strength = 0; + + } else { + strength = _strength; + } + + return ret; +} + +int Tunes::get_next_tune(unsigned &frequency, unsigned &duration, + unsigned &silence) { // Return the vaules for frequency and duration if the custom msg was received if (_using_custom_msg) { @@ -318,19 +349,18 @@ int Tunes::get_next_tune(unsigned &frequency, unsigned &duration, unsigned &sile // compute the note frequency frequency = note_to_frequency(note); - return TUNE_CONTINUE; // tune looks bad (unexpected EOF, bad character, etc.) tune_error: // syslog(LOG_ERR, "tune error\n"); _repeat = false; // don't loop on error - config_tone(_repeat); + reset(_repeat); return TUNE_ERROR; // stop (and potentially restart) the tune tune_end: // restore intial parameter - config_tone(_repeat); + reset(_repeat); if (_repeat) { return TUNE_CONTINUE; @@ -340,13 +370,13 @@ int Tunes::get_next_tune(unsigned &frequency, unsigned &duration, unsigned &sile } } -unsigned Tunes::note_to_frequency(unsigned note) +uint32_t Tunes::note_to_frequency(unsigned note) const { // compute the frequency (Hz) return (unsigned)(880.0f * powf(2.0f, ((int)note - 46) / 12.0f)); } -unsigned Tunes::note_duration(unsigned &silence, unsigned note_length, unsigned dots) +unsigned Tunes::note_duration(unsigned &silence, unsigned note_length, unsigned dots) const { unsigned whole_note_period = BEAT_TIME_CONVERSION / _tempo; @@ -383,7 +413,7 @@ unsigned Tunes::note_duration(unsigned &silence, unsigned note_length, unsigned return note_period; } -unsigned Tunes::rest_duration(unsigned rest_length, unsigned dots) +unsigned Tunes::rest_duration(unsigned rest_length, unsigned dots) const { unsigned whole_note_period = BEAT_TIME_CONVERSION / _tempo; diff --git a/src/lib/tunes/tunes.h b/src/lib/tunes/tunes.h index f384c8b27128..d50efb21dc31 100644 --- a/src/lib/tunes/tunes.h +++ b/src/lib/tunes/tunes.h @@ -43,6 +43,17 @@ #define TUNE_MAX_UPDATE_INTERVAL_US 100000 +#define TUNE_DEFAULT_TEMPO 120 +#define TUNE_DEFAULT_OCTAVE 4 +#define TUNE_DEFAULT_NOTE_LENGTH 4 +#define TUNE_MAX_STRENGTH 100 + + +/** + * Library for parsing tunes from melody-strings or dedicated tune messages. + * Needs to be instantiated as it keeps track of which tune is to be played + * next. Also handles repeated tunes. + */ class Tunes { public: @@ -50,9 +61,9 @@ class Tunes /** * Constructor with the default parameter set to: - * default_tempo: 120 - * default_octave: 4 - * default_note_length: 4 + * default_tempo: TUNE_DEFAULT_TEMPO + * default_octave: TUNE_DEFAULT_OCTAVE + * default_note_length: TUNE_DEFAULT_NOTE_LENGTH * default_mode: NORMAL */ Tunes(); @@ -65,22 +76,27 @@ class Tunes ~Tunes() = default; /** - * Set tune to be played. + * Set tune to be played using the message. If a tune is already being played + * the call to this function will be ignored, unless the override flag is set + * or the tune being already played is a repeated tune. * @param tune_control struct containig the uORB message * @return return -EINVAL if the default tune does not exist. */ int set_control(const tune_control_s &tune_control); /** - * Parse a tune string, formatted with the syntax of the Microsoft GWBasic/QBasic. - * This has to be kept in memory for the whole duration of the melody. + * Set tune to be played using a string. + * Parses a tune string, formatted with the syntax of the Microsoft GWBasic/QBasic. + * Ownership of the string is NOT transferred. The string has to be kept in + * memory for the whole duration of the melody. * * @param string tune input string */ - void set_string(const char *string); + void set_string(const char *const string, uint8_t strength); /** - * Get next note in the setted string in set_control or play_string + * Get next note in the current tune, which has been provided by either + * set_control or play_string * @param frequency return frequency value (Hz) * @param duration return duration of the tone (us) * @param silence return silence duration (us) @@ -88,7 +104,24 @@ class Tunes */ int get_next_tune(unsigned &frequency, unsigned &duration, unsigned &silence); - unsigned int get_default_tunes_size() {return _default_tunes_size;} + /** + * Get next note in the current tune, which has been provided by either + * set_control or play_string + * @param frequency return frequency value (Hz) + * @param duration return duration of the tone (us) + * @param silence return silence duration (us) + * @param strength return the strength of the note (between 0-100) + * @return -1 for error, 0 for play one tone and 1 for continue a sequence + */ + int get_next_tune(unsigned &frequency, unsigned &duration, unsigned &silence, + uint8_t &strength); + + /** + * Get the number of default tunes. This is useful for when a tune is + * requested via its tune ID. + * @return Number of default tunes accessible via tune ID + */ + unsigned int get_default_tunes_size() const {return _default_tunes_size;} unsigned int get_maximum_update_interval() {return (unsigned int)TUNE_MAX_UPDATE_INTERVAL_US;} @@ -96,8 +129,7 @@ class Tunes static const char *_default_tunes[]; static const uint8_t _note_tab[]; static const unsigned int _default_tunes_size; - bool _repeat; ///< if true, tune restarts at end - + bool _repeat = false; ///< if true, tune restarts at end const char *_tune = nullptr; ///< current tune string const char *_next = nullptr; ///< next note in the string const char *_tune_start_ptr = nullptr; ///< pointer to repeat tune @@ -107,14 +139,15 @@ class Tunes NoteMode _note_mode; unsigned _octave; - unsigned _default_tempo = 120; - unsigned _default_note_length = 4; - NoteMode _default_mode = NoteMode::NORMAL; - unsigned _default_octave = 4; + unsigned _default_tempo; + unsigned _default_note_length; + NoteMode _default_mode; + unsigned _default_octave; unsigned _frequency; unsigned _duration; unsigned _silence; + uint8_t _strength; bool _using_custom_msg = false; /** @@ -123,7 +156,7 @@ class Tunes * @param note unsigned value of the semitone from C * @return frequency (Hz) */ - uint32_t note_to_frequency(unsigned note); + uint32_t note_to_frequency(unsigned note) const; /** * Calculate the duration in microseconds of play and silence for a @@ -135,7 +168,7 @@ class Tunes * @param dots extention of the note length * @return duration of the note (us) */ - unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); + unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots) const; /** * Calculate the duration in microseconds of a rest corresponding to @@ -145,7 +178,7 @@ class Tunes * @param dots number of extension dots * @return rest duration (us) */ - unsigned rest_duration(unsigned rest_length, unsigned dots); + unsigned rest_duration(unsigned rest_length, unsigned dots) const; /** * Find the next character in the string, discard any whitespace. @@ -169,8 +202,10 @@ class Tunes unsigned next_dots(); /** - * if repeat false set the tune parameters to default else point to the beginning of the tune + * Reset the tune parameters. This is necessary when for example a tune moved + * one or more octaves up or down. reset() should always be called before + * (re)-starting a tune. */ - void config_tone(bool repeat); + void reset(bool repeat_flag); }; diff --git a/src/systemcmds/tune_control/tune_control.cpp b/src/systemcmds/tune_control/tune_control.cpp index 11a4228c9c5f..cb1e8b156eb4 100644 --- a/src/systemcmds/tune_control/tune_control.cpp +++ b/src/systemcmds/tune_control/tune_control.cpp @@ -71,6 +71,8 @@ usage() "\t-d \t\tDuration of the tone in us\n" "\t-s \t\tStrength of the tone between 0-100\n" "\t-m \t\tMelody in a string form ex: \"MFT200e8a8a\"\n" + "\n" + "tune_control stop \t\tStops playback, useful for repeated tunes\n" ); } @@ -145,7 +147,7 @@ tune_control_main(int argc, char *argv[]) break; case 's': - value = (uint16_t)(strtol(myoptarg, nullptr, 0)); + value = (uint8_t)(strtol(myoptarg, nullptr, 0)); if (value > 0 && value < 100) { tune_control.strength = value; @@ -169,23 +171,25 @@ tune_control_main(int argc, char *argv[]) } unsigned frequency, duration, silence; + uint8_t strength; int exit_counter = 0; if (!strcmp(argv[myoptind], "play")) { if (string_input) { PX4_INFO("Start playback..."); - tunes.set_string(tune_string); + tunes.set_string(tune_string, tune_control.strength); - while (tunes.get_next_tune(frequency, duration, silence) > 0) { + while (tunes.get_next_tune(frequency, duration, silence, strength) > 0) { tune_control.tune_id = 0; tune_control.frequency = (uint16_t)frequency; tune_control.duration = (uint32_t)duration; tune_control.silence = (uint32_t)silence; + tune_control.strength = (uint8_t)strength; publish_tune_control(tune_control); usleep(duration + silence); exit_counter++; - // exit if the loop is doing more thatn 50 iteration + // exit if the loop is doing too many iterations if (exit_counter > MAX_NOTE_ITERATION) { break; } @@ -193,7 +197,7 @@ tune_control_main(int argc, char *argv[]) PX4_INFO("Playback finished."); - } else { + } else { // tune id instead of string has been provided if (tune_control.tune_id == 0) { tune_control.tune_id = 1; } @@ -203,20 +207,34 @@ tune_control_main(int argc, char *argv[]) } } else if (!strcmp(argv[myoptind], "libtest")) { - tunes.set_control(tune_control); + int ret = tunes.set_control(tune_control); - while (tunes.get_next_tune(frequency, duration, silence) > 0) { - PX4_INFO("frequency: %d, duration %d, silence %d", frequency, duration, silence); + if (ret == -EINVAL) { + PX4_WARN("Tune ID not recognized."); + } + + while (tunes.get_next_tune(frequency, duration, silence, strength) > 0) { + PX4_INFO("frequency: %d, duration %d, silence %d, strength%d", + frequency, duration, silence, strength); usleep(500000); exit_counter++; - // exit if the loop is doing more thatn 50 iteration + // exit if the loop is doing too many iterations if (exit_counter > MAX_NOTE_ITERATION) { break; } } - } else { + } else if (!strcmp(argv[myoptind], "stop")) { + PX4_INFO("Stopping playback..."); + tune_control.tune_id = 0; + tune_control.frequency = 0; + tune_control.duration = 0; + tune_control.silence = 0; + tune_control.tune_override = true; + publish_tune_control(tune_control); + + } else { usage(); return 1; }