diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index e4b36a7c78..ea7911e1b2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,8 +77,7 @@ then if ! mkblctrl $MKBLCTRL_ARG then - # Error tune. - tune_control play -t 2 + tune_control play error fi fi @@ -86,8 +85,7 @@ then then if ! pwm_out_sim start -m $OUTPUT_MODE then - # Error tune. - tune_control play -t 2 + tune_control play error fi fi @@ -96,8 +94,7 @@ then if ! $OUTPUT_CMD mode_$FMU_MODE then echo "$OUTPUT_CMD start failed" >> $LOG_FILE - # Error tune. - tune_control play -t 2 + tune_control play error fi fi @@ -176,7 +173,7 @@ then else echo "ERROR [init] Failed loading mixer: ${MIXER_FILE}" echo "ERROR [init] Failed loading mixer: ${MIXER_FILE}" >> $LOG_FILE - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi else @@ -184,7 +181,7 @@ else then echo "ERROR [init] Mixer undefined" echo "ERROR [init] Mixer undefined" >> $LOG_FILE - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi fi @@ -234,7 +231,7 @@ then fi else echo "ERROR: Could not start: pwm_out mode_pwm" >> $LOG_FILE - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR set PWM_AUX_OUT none set FAILSAFE_AUX none fi diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 30fee02863..f7b4565ac2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -18,6 +18,6 @@ then px4io recovery else echo "PX4IO start failed" >> $LOG_FILE - tune_control play -t 20 + tune_control play -t 18 # PROG_PX4IO_ERR fi fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6ce1350fd6..f9eafe6fdc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -130,14 +130,14 @@ then else # tune SD_INIT - set STARTUP_TUNE 16 + set STARTUP_TUNE 14 # tune 14 = SD_INIT if mkfatfs /dev/mmcsd0 then if mount -t vfat /dev/mmcsd0 /fs/microsd then echo "INFO [init] card formatted" else - set STARTUP_TUNE 17 + set STARTUP_TUNE 15 # tune 15 = SD_ERROR echo "ERROR [init] format failed" set LOG_FILE /dev/null fi @@ -186,15 +186,6 @@ else set AUTOCNF yes fi - # - # Play the startup tune (if not disabled or there is an error) - # - param compare CBRK_BUZZER 782090 - if [ $? != 0 -o $STARTUP_TUNE != 1 ] - then - tune_control play -t $STARTUP_TUNE - fi - # # Optional board defaults: rc.board_defaults # @@ -206,6 +197,21 @@ else fi unset BOARD_RC_DEFAULTS + # + # Start the tone_alarm driver. + # Needs to be started after the parameters are loaded (for CBRK_BUZZER). + # + tone_alarm start + + # + # Play the startup tune (if not disabled or there is an error) + # + param compare CBRK_BUZZER 782090 + if [ $? != 0 -o $STARTUP_TUNE != 1 ] + then + tune_control play -t $STARTUP_TUNE + fi + # # Waypoint storage. # REBOOTWORK this needs to start in parallel. @@ -236,13 +242,6 @@ else fi fi - # - # Start the tone_alarm driver. - # Needs to be started after the parameters are loaded (for CBRK_BUZZER). - # Note that this will still play the already published startup tone. - # - tone_alarm start - # # Set parameters and env variables for selected AUTOSTART. # @@ -286,7 +285,7 @@ else set IO_PRESENT yes else # tune Program PX4IO - tune_control play -t 18 + tune_control play -t 16 # tune 16 = PROG_PX4IO if px4io start then @@ -305,8 +304,7 @@ else if px4io checkcrc ${IOFW} then echo "PX4IO CRC OK after updating" >> $LOG_FILE - #tune MLL8CDE Program PX4IO success - tune_control play -t 19 + tune_control play -t 17 # tune 17 = PROG_PX4IO_OK set IO_PRESENT yes fi fi @@ -314,8 +312,7 @@ else if [ $IO_PRESENT = no ] then echo "PX4IO update failed" >> $LOG_FILE - # Error tune. - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR fi fi fi @@ -332,8 +329,7 @@ else if [ $USE_IO = yes -a $IO_PRESENT = no ] then echo "PX4IO not found" >> $LOG_FILE - # Error tune. - tune_control play -t 2 + tune_control play error fi # @@ -447,8 +443,7 @@ else fi fi else - # Error tune. - tune_control play -t 2 + tune_control play error fi fi diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index e9c132c7df..08232cc6ec 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -30,11 +30,10 @@ if [ $? = 0 ] then echo "[i] card mounted at /fs/microsd" # Start playing the startup tune - tune_control play -t 1 + tune_control play -t 1 # tune 1 = STARTUP else echo "[i] no microSD card found" - # Play SOS - tune_control play -t 2 + tune_control play error fi # @@ -90,22 +89,22 @@ then echo "PX4IO CRC OK" else echo "PX4IO CRC failure" - tune_control play -t 18 + tune_control play -t 16 # tune 16 = PROG_PX4IO if px4io forceupdate 14662 $io_file then if px4io start then echo "PX4IO restart OK" - tune_control play -t 19 + tune_control play-t 17 # tune 17 = PROG_PX4IO_OK else echo "PX4IO restart failed" - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR set unit_test_failure 1 set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi else echo "PX4IO update failed" - tune_control play -t 20 + tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR set unit_test_failure 1 set unit_test_failure_list "${unit_test_failure_list} px4io_flash" fi diff --git a/msg/tune_control.msg b/msg/tune_control.msg index 832ad4784a..000b2c3248 100644 --- a/msg/tune_control.msg +++ b/msg/tune_control.msg @@ -3,8 +3,29 @@ uint64 timestamp # time since system start (microseconds) +uint8 TUNE_ID_STOP = 0 +uint8 TUNE_ID_STARTUP = 1 +uint8 TUNE_ID_ERROR = 2 +uint8 TUNE_ID_NOTIFY_POSITIVE = 3 +uint8 TUNE_ID_NOTIFY_NEUTRAL = 4 +uint8 TUNE_ID_NOTIFY_NEGATIVE = 5 +uint8 TUNE_ID_ARMING_WARNING = 6 +uint8 TUNE_ID_BATTERY_WARNING_SLOW = 7 +uint8 TUNE_ID_BATTERY_WARNING_FAST = 8 +uint8 TUNE_ID_GPS_WARNING = 9 +uint8 TUNE_ID_ARMING_FAILURE = 10 +uint8 TUNE_ID_PARACHUTE_RELEASE = 11 +uint8 TUNE_ID_SINGLE_BEEP = 12 +uint8 TUNE_ID_HOME_SET = 13 +uint8 TUNE_ID_SD_INIT = 14 +uint8 TUNE_ID_SD_ERROR = 15 +uint8 TUNE_ID_PROG_PX4IO = 16 +uint8 TUNE_ID_PROG_PX4IO_OK = 17 +uint8 TUNE_ID_PROG_PX4IO_ERR = 18 +uint8 NUMBER_OF_TUNES = 19 + uint8 tune_id # tune_id corresponding to TuneID::* from the tune_defaults.h in the tunes library -uint8 tune_override # if set to 1 the tune which is playing will be stopped and the new started +bool tune_override # if true the tune which is playing will be stopped and the new started uint16 frequency # in Hz uint32 duration # in us uint32 silence # in us diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp index 4975dc1220..20f5ebf29b 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp @@ -44,6 +44,8 @@ class ScheduledWorkItem : public WorkItem { public: + bool Scheduled() { return !hrt_called(&_call); } + /** * Schedule next run with a delay in microseconds. * @@ -59,6 +61,13 @@ public: */ void ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us = 0); + /** + * Schedule next run at a specific time. + * + * @param time_us The time in microseconds. + */ + void ScheduleAt(hrt_abstime time_us); + /** * Clear any scheduled work. */ diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index e7ca6379b2..3c57ff45cd 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -57,6 +57,11 @@ void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_ hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); } +void ScheduledWorkItem::ScheduleAt(hrt_abstime time_us) +{ + hrt_call_at(&_call, time_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this); +} + void ScheduledWorkItem::ScheduleClear() { // first clear any scheduled hrt call, then remove the item from the runnable queue diff --git a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp index b394ea88ea..c864ac245e 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/imxrt/tone_alarm/ToneAlarmInterface.cpp @@ -163,8 +163,9 @@ void init() #endif /* TONE_ALARM_TIMER */ } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { + hrt_abstime time_started = 0; #if defined(TONE_ALARM_TIMER) float period = 0.5f / frequency; @@ -182,8 +183,13 @@ void start_note(unsigned frequency) rCR |= GPT_CR_EN; // configure the GPIO to enable timer output + irqstate_t flags = enter_critical_section(); + time_started = hrt_absolute_time(); px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); #endif /* TONE_ALARM_TIMER */ + + return time_started; } void stop_note() diff --git a/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp index df36cd725c..6c81e949ef 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/kinetis/tone_alarm/ToneAlarmInterface.cpp @@ -159,7 +159,7 @@ void init() rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this. } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { // Calculate the signal switching period. // (Signal switching period is one half of the frequency period). @@ -173,7 +173,12 @@ void start_note(unsigned frequency) rSC |= (TPM_SC_CMOD_LPTPM_CLK); // Configure the GPIO to enable timer output. + irqstate_t flags = enter_critical_section(); + const hrt_abstime time_started = hrt_absolute_time(); px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); + + return time_started; } void stop_note() diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp index df36cd725c..9de7464b53 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/tone_alarm/ToneAlarmInterface.cpp @@ -159,7 +159,7 @@ void init() rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this. } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { // Calculate the signal switching period. // (Signal switching period is one half of the frequency period). @@ -173,7 +173,12 @@ void start_note(unsigned frequency) rSC |= (TPM_SC_CMOD_LPTPM_CLK); // Configure the GPIO to enable timer output. + irqstate_t flags = enter_critical_section(); + hrt_abstime time_started = hrt_absolute_time(); px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); + + return time_started; } void stop_note() diff --git a/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp index 8b8be4c00c..b36f028838 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfaceGPIO.cpp @@ -44,9 +44,10 @@ void init() px4_arch_configgpio(GPIO_TONE_ALARM_IDLE); } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { px4_arch_gpiowrite(GPIO_TONE_ALARM_GPIO, 1); + return hrt_absolute_time(); } void stop_note() diff --git a/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp index f83cdfebea..231ff62603 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp +++ b/platforms/nuttx/src/px4/stm/stm32_common/tone_alarm/ToneAlarmInterfacePWM.cpp @@ -299,7 +299,7 @@ void init() rCR1 = GTIM_CR1_CEN; // Ensure the timer is running. } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { // Calculate the signal switching period. // (Signal switching period is one half of the frequency period). @@ -321,7 +321,12 @@ void start_note(unsigned frequency) rCCER |= TONE_CCER; // Enable the output. // Configure the GPIO to enable timer output. + hrt_abstime time_started = hrt_absolute_time(); + irqstate_t flags = enter_critical_section(); px4_arch_configgpio(GPIO_TONE_ALARM); + leave_critical_section(flags); + + return time_started; } void stop_note() diff --git a/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp index 551cf64722..61f9fc69ca 100644 --- a/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp @@ -46,9 +46,10 @@ void init() // Nothing to be done in simulation. } -void start_note(unsigned frequency) +hrt_abstime start_note(unsigned frequency) { // Nothing to be done in simulation. + return 0; } void stop_note() diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index f22d479842..3eeb97a4ea 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -34,64 +34,16 @@ /** * @file drv_tone_alarm.h * - * Driver for the PX4 audio alarm port, /dev/tone_alarm. + * Driver for the PX4 audio alarm port. * - * The tone_alarm driver supports a set of predefined "alarm" - * patterns and one user-supplied pattern. Patterns are ordered by - * priority, with a higher-priority pattern interrupting any - * lower-priority pattern that might be playing. - * - * The TONE_SET_ALARM ioctl can be used to select a predefined - * alarm pattern, from 1 - . Selecting pattern zero silences - * the alarm. - * - * To supply a custom pattern, write an array of 1 - tone_note - * structures to /dev/tone_alarm. The custom pattern has a priority - * of zero. - * - * Patterns will normally play once and then silence (if a pattern - * was overridden it will not resume). A pattern may be made to - * repeat by inserting a note with the duration set to - * DURATION_REPEAT. This pattern will loop until either a - * higher-priority pattern is started or pattern zero is requested - * via the ioctl. */ #pragma once +#include "drv_hrt.h" #include #include -#define CBRK_BUZZER_KEY 782097 -#define CBRK_OFF 0 -#define CBRK_ON 1 -#define CBRK_UNINIT 2 - -#define _TONE_ALARM_BASE 0x7400 -#define TONE_SET_ALARM _PX4_IOC(_TONE_ALARM_BASE, 1) -#define TONE_ALARM0_DEVICE_PATH "/dev/tone_alarm0" - -// TODO: remove this once the tone_alarm driver is changed to the new tunelib -enum { - TONE_STOP_TUNE = 0, - TONE_STARTUP_TUNE, - TONE_ERROR_TUNE, - TONE_NOTIFY_POSITIVE_TUNE, - TONE_NOTIFY_NEUTRAL_TUNE, - TONE_NOTIFY_NEGATIVE_TUNE, - TONE_ARMING_WARNING_TUNE, - TONE_BATTERY_WARNING_SLOW_TUNE, - TONE_BATTERY_WARNING_FAST_TUNE, - TONE_GPS_WARNING_TUNE, - TONE_ARMING_FAILURE_TUNE, - TONE_PARACHUTE_RELEASE_TUNE, - TONE_EKF_WARNING_TUNE, - TONE_BARO_WARNING_TUNE, - TONE_SINGLE_BEEP_TUNE, - TONE_HOME_SET, - TONE_NUMBER_OF_TUNES -}; - namespace ToneAlarmInterface { @@ -102,8 +54,9 @@ void init(); /** * @brief Starts playing the note. + * @return the time the note started played. */ -void start_note(unsigned frequency); +hrt_abstime start_note(unsigned frequency); /** * @brief Stops playing the current note and makes the player 'safe'. diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index e48d0e150d..bb2ec9933b 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include +#include #include #include #include @@ -161,7 +162,7 @@ private: char _device[20]; orb_advert_t _t_outputs; orb_advert_t _t_esc_status; - orb_advert_t _tune_control_sub; + uORB::PublicationQueued _tune_control_pub{ORB_ID(tune_control)}; unsigned int _num_outputs; bool _primary_pwm_device; bool _motortest; @@ -448,14 +449,14 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) void MK::play_beep(int count) { - tune_control_s tune = {}; + tune_control_s tune{}; tune.tune_id = static_cast(TuneID::SINGLE_BEEP); for (int i = 0; i < count; i++) { - orb_publish(ORB_ID(tune_control), _tune_control_sub, &tune); + tune.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune); usleep(300000); } - } int @@ -490,12 +491,6 @@ MK::task_main() memset(&esc, 0, sizeof(esc)); _t_esc_status = orb_advertise(ORB_ID(esc_status), &esc); - /* - * advertise the tune_control. - */ - tune_control_s tune = {}; - _tune_control_sub = orb_advertise_queue(ORB_ID(tune_control), &tune, tune_control_s::ORB_QUEUE_LENGTH); - pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; diff --git a/src/drivers/safety_button/SafetyButton.cpp b/src/drivers/safety_button/SafetyButton.cpp index c671c3a8de..8c7babd4f9 100644 --- a/src/drivers/safety_button/SafetyButton.cpp +++ b/src/drivers/safety_button/SafetyButton.cpp @@ -129,14 +129,11 @@ SafetyButton::CheckPairingRequest(bool button_pressed) ++_pairing_button_counter; } - if (_pairing_button_counter == 3) { - - vehicle_command_s vcmd{}; vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR; - vcmd.timestamp = now; vcmd.param1 = 10.f; // GCS pairing request handled by a companion. + vcmd.timestamp = hrt_absolute_time(); _to_command.publish(vcmd); PX4_DEBUG("Sending GCS pairing request"); @@ -150,9 +147,8 @@ SafetyButton::CheckPairingRequest(bool button_pressed) _to_led_control.publish(led_control); tune_control_s tune_control{}; - tune_control.tune_id = TONE_NOTIFY_POSITIVE_TUNE; + tune_control.tune_id = tune_control_s::TUNE_ID_NOTIFY_POSITIVE; tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - tune_control.tune_override = 0; tune_control.timestamp = hrt_absolute_time(); _to_tune_control.publish(tune_control); diff --git a/src/drivers/safety_button/SafetyButton.hpp b/src/drivers/safety_button/SafetyButton.hpp index 84bc0bf0f5..405c9617d5 100644 --- a/src/drivers/safety_button/SafetyButton.hpp +++ b/src/drivers/safety_button/SafetyButton.hpp @@ -77,7 +77,6 @@ private: void FlashButton(); void CheckPairingRequest(bool button_pressed); - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; uORB::Publication _to_safety{ORB_ID(safety)}; uORB::PublicationQueued _to_command{ORB_ID(vehicle_command)}; diff --git a/src/drivers/telemetry/iridiumsbd/module.yaml b/src/drivers/telemetry/iridiumsbd/module.yaml index f9b0bdb33d..5fa8d138c4 100644 --- a/src/drivers/telemetry/iridiumsbd/module.yaml +++ b/src/drivers/telemetry/iridiumsbd/module.yaml @@ -7,7 +7,7 @@ serial_config: then mavlink start -d /dev/iridium -m iridium -b 115200 else - tune_control play -t 20 + tune_control play error fi port_config_param: name: ISBD_CONFIG diff --git a/src/drivers/tone_alarm/CMakeLists.txt b/src/drivers/tone_alarm/CMakeLists.txt index a298671f8b..3e9f6ba482 100644 --- a/src/drivers/tone_alarm/CMakeLists.txt +++ b/src/drivers/tone_alarm/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2015-2019 PX4 Development Team. All rights reserved. +# Copyright (C) 2015-2020 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 @@ -34,8 +34,11 @@ px4_add_module( MODULE drivers__tone_alarm MAIN tone_alarm + COMPILE_FLAGS + #-DDEBUG_BUILD SRCS ToneAlarm.cpp + ToneAlarm.h DEPENDS circuit_breaker arch_tone_alarm diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index cbeaff319b..8deac60f64 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2020 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 @@ -38,195 +38,228 @@ #include "ToneAlarm.h" #include +#include + +using namespace time_literals; ToneAlarm::ToneAlarm() : - CDev(TONE_ALARM0_DEVICE_PATH), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + // ensure ORB_ID(tune_control) is advertised with correct queue depth + orb_advertise_queue(ORB_ID(tune_control), nullptr, tune_control_s::ORB_QUEUE_LENGTH); } ToneAlarm::~ToneAlarm() { - _should_run = false; - int counter = 0; - - while (_running && counter < 10) { - px4_usleep(100000); - counter++; - } + ToneAlarmInterface::stop_note(); } -int ToneAlarm::init() +bool ToneAlarm::Init() { - if (CDev::init() != OK) { - return PX4_ERROR; - } - // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. ToneAlarmInterface::init(); - _running = true; + _tune_control_sub.set_interval_us(10_ms); + + if (!_tune_control_sub.registerCallback()) { + PX4_ERR("tune_control callback registration failed!"); + return PX4_ERROR; + } ScheduleNow(); - return OK; + return true; } -void ToneAlarm::next_note() +void ToneAlarm::InterruptStopNote(void *arg) { - if (!_should_run) { - _running = false; - return; - } - - // Check for updates - orb_update(); - - unsigned int frequency = 0; - unsigned int duration = 0; - - // Does an inter-note silence occur? - if (_silence_length > 0) { - stop_note(); - duration = _silence_length; - _silence_length = 0; - - } else if (_play_tone) { - int parse_ret_val = _tunes.get_next_note(frequency, duration, _silence_length); - - if (parse_ret_val > 0) { - // Continue playing. - _play_tone = true; - - // A frequency of 0 corresponds to stop_note(); - if (frequency > 0) { - // Start playing the note. - start_note(frequency); - } - - } else { - _play_tone = false; - stop_note(); - } - - } else { - // Schedule a callback with the tunes max interval. - duration = _tunes.get_maximum_update_interval(); - stop_note(); - } - - // Schedule a callback when the note should stop. - ScheduleDelayed(duration); + ToneAlarmInterface::stop_note(); } void ToneAlarm::Run() -{ - next_note(); -} - -void ToneAlarm::orb_update() -{ - // Check for updates - if (_tune_control_sub.updated()) { - _tune_control_sub.copy(&_tune); - - if (_tune.timestamp > 0) { - _play_tone = _tunes.set_control(_tune) == 0; - } - } -} - -void ToneAlarm::status() -{ - if (_running) { - PX4_INFO("running"); - - } else { - PX4_INFO("stopped"); - } -} - -void ToneAlarm::start_note(unsigned frequency) { // Check if circuit breaker is enabled. - if (_cbrk == CBRK_UNINIT) { - _cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY); + if (!_circuit_break_initialized) { + if (circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY)) { + request_stop(); + } + + _circuit_break_initialized = true; } - if (_cbrk != CBRK_OFF) { + if (should_exit()) { + _tune_control_sub.unregisterCallback(); + exit_and_cleanup(); return; } - // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. - ToneAlarmInterface::start_note(frequency); -} + // Check for next tune_control when not currently playing + if (_tune_control_sub.updated()) { + tune_control_s tune_control; -void ToneAlarm::stop_note() -{ - // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. - ToneAlarmInterface::stop_note(); -} + if (_tune_control_sub.copy(&tune_control)) { + if (tune_control.timestamp > 0) { + if (!_play_tone || (_play_tone && tune_control.tune_override)) { + PX4_DEBUG("new tune %d", tune_control.tune_id); -/** - * Local functions in support of the shell command. - */ -namespace -{ + if (_tunes.set_control(tune_control) == PX4_OK) { + if (tune_control.tune_override) { + // clear existing + ToneAlarmInterface::stop_note(); + _next_note_time = 0; + hrt_cancel(&_hrt_call); + } -ToneAlarm *g_dev; + _play_tone = true; -} // namespace +#if (!defined(TONE_ALARM_TIMER) && !defined(GPIO_TONE_ALARM_GPIO)) || defined(DEBUG_BUILD) -/** - * Tone alarm Driver 'main' command. - * Entry point for the tone_alarm driver module. - */ -extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]) -{ - if (argc > 1) { - const char *argv1 = argv[1]; + switch (tune_control.tune_id) { + case tune_control_s::TUNE_ID_STARTUP: + PX4_INFO("startup tune"); + break; - if (!strcmp(argv1, "start")) { - if (g_dev == nullptr) { - g_dev = new ToneAlarm(); + case tune_control_s::TUNE_ID_ERROR: + PX4_ERR("error tune"); + break; - if (g_dev == nullptr) { - PX4_ERR("could not allocate the driver."); + case tune_control_s::TUNE_ID_NOTIFY_POSITIVE: + PX4_INFO("notify positive"); + break; + + case tune_control_s::TUNE_ID_NOTIFY_NEUTRAL: + PX4_INFO("notify neutral"); + break; + + case tune_control_s::TUNE_ID_NOTIFY_NEGATIVE: + PX4_ERR("notify negative"); + break; + + case tune_control_s::TUNE_ID_ARMING_WARNING: + PX4_WARN("arming warning"); + break; + + case tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW: + PX4_WARN("battery warning (slow)"); + break; + + case tune_control_s::TUNE_ID_BATTERY_WARNING_FAST: + PX4_WARN("battery warning (fast)"); + break; + + case tune_control_s::TUNE_ID_ARMING_FAILURE: + PX4_ERR("arming failure"); + break; + + case tune_control_s::TUNE_ID_SINGLE_BEEP: + PX4_WARN("beep"); + break; + + case tune_control_s::TUNE_ID_HOME_SET: + PX4_INFO("home set"); + break; + } + +#endif // (!TONE_ALARM_TIMER && !GPIO_TONE_ALARM_GPIO) || DEBUG_BUILD + } + + } else if (_play_tone && !tune_control.tune_override) { + // otherwise re-publish tune to process next + PX4_DEBUG("tune already playing, requeing tune: %d", tune_control.tune_id); + uORB::PublicationQueued tune_control_pub{ORB_ID(tune_control)}; + tune_control.timestamp = hrt_absolute_time(); + tune_control_pub.publish(tune_control); } + } + } + } - if (g_dev->init() != OK) { - delete g_dev; - g_dev = nullptr; - PX4_ERR("driver init failed."); - } + unsigned int frequency = 0; + unsigned int duration = 0; + unsigned int silence_length = 0; - } else { - PX4_INFO("already started"); + // Does an inter-note silence occur? + if ((_next_note_time != 0) && (hrt_absolute_time() < _next_note_time)) { + PX4_DEBUG("inter-note silence"); + ScheduleAt(_next_note_time); + + } else if (_play_tone && (_tunes.get_next_note(frequency, duration, silence_length) == Tunes::Status::Continue)) { + PX4_DEBUG("Play frequency: %d, duration: %d us, silence: %d us", frequency, duration, silence_length); + + if (frequency > 0) { + // Start playing the note. + const hrt_abstime time_started = ToneAlarmInterface::start_note(frequency); + + if (time_started > 0) { + // schedule stop with HRT + hrt_call_at(&_hrt_call, time_started + duration, (hrt_callout)&InterruptStopNote, this); + _next_note_time = time_started + duration + silence_length; + + // schedule next note + ScheduleAt(_next_note_time); } - return 0; - } - - if (!strcmp(argv1, "stop")) { - delete g_dev; - g_dev = nullptr; - return 0; - } - - if (!strcmp(argv1, "status")) { - if (g_dev != nullptr) { - g_dev->status(); - - } else { - PX4_INFO("driver stopped"); - } - - return 0; + } else { + // A frequency of 0 corresponds to ToneAlarmInterface::stop_note() + _next_note_time = hrt_absolute_time() + duration + silence_length; + ToneAlarmInterface::stop_note(); + ScheduleAt(_next_note_time); } } else { - PX4_INFO("missing command, try 'start', status, 'stop'"); + PX4_DEBUG("stopping"); + ToneAlarmInterface::stop_note(); + _play_tone = false; + _next_note_time = 0; } + // if done playing and a new tune_control is still available re-schedule + if (!Scheduled() && _tune_control_sub.updated()) { + ScheduleDelayed(_tunes.get_maximum_update_interval()); + } +} + +int ToneAlarm::task_spawn(int argc, char *argv[]) +{ + ToneAlarm *instance = new ToneAlarm(); + + if (!instance) { + PX4_ERR("alloc failed"); + return -1; + } + + if (!instance->Init()) { + delete instance; + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + return PX4_OK; +} + +int ToneAlarm::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module is responsible for the tone alarm. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("tone_alarm", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; -}; +} + +extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]) +{ + return ToneAlarm::main(argc, argv); +} diff --git a/src/drivers/tone_alarm/ToneAlarm.h b/src/drivers/tone_alarm/ToneAlarm.h index e4dc4e5171..fa04993e8c 100644 --- a/src/drivers/tone_alarm/ToneAlarm.h +++ b/src/drivers/tone_alarm/ToneAlarm.h @@ -40,79 +40,50 @@ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include +#include #include +#include #include +#include + #include -#if !defined(UNUSED) -# define UNUSED(a) ((void)(a)) -#endif - -class ToneAlarm : public cdev::CDev, public px4::ScheduledWorkItem +class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem { public: ToneAlarm(); - ~ToneAlarm(); + ~ToneAlarm() override; - /** - * @brief Initializes the character device and hardware registers. - */ - int init() override; + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** - * @brief Prints the driver status to the console. - */ - void status(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]) { return 0; }; -protected: - - /** - * @brief Parses the next note out of the string and plays it. - */ - void next_note(); - - /** - * @brief Trampoline for the work queue. - */ - void Run() override; - - /** - * @brief Updates the uORB topics for local subscribers. - */ - void orb_update(); - - /** - * @brief Starts playing the note. - */ - void start_note(unsigned frequency); - - /** - * @brief Stops playing the current note and makes the player 'safe'. - */ - void stop_note(); - - volatile bool _running{false}; ///< Flag to indicate the current driver status. - - int _cbrk{CBRK_UNINIT}; ///< If true, no audio output. + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); private: + static void InterruptStopNote(void *arg); - volatile bool _should_run{true}; + bool Init(); + void Run() override; + hrt_call _hrt_call{}; + + Tunes _tunes; + + hrt_abstime _next_note_time{0}; + + uORB::SubscriptionCallbackWorkItem _tune_control_sub{this, ORB_ID(tune_control)}; + + bool _circuit_break_initialized{false}; bool _play_tone{false}; - - unsigned int _silence_length{0}; ///< If nonzero, silence before next note. - - uORB::Subscription _tune_control_sub{ORB_ID(tune_control)}; - - tune_control_s _tune{}; - - Tunes _tunes = Tunes(); }; diff --git a/src/lib/circuit_breaker/circuit_breaker.h b/src/lib/circuit_breaker/circuit_breaker.h index fa7ace2d50..1e19e8d2f3 100644 --- a/src/lib/circuit_breaker/circuit_breaker.h +++ b/src/lib/circuit_breaker/circuit_breaker.h @@ -49,6 +49,8 @@ * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT. */ + +#define CBRK_BUZZER_KEY 782097 #define CBRK_SUPPLY_CHK_KEY 894281 #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 diff --git a/src/lib/tunes/tune_definition.desc b/src/lib/tunes/tune_definition.desc index 7aec40f9d4..1219afec1e 100644 --- a/src/lib/tunes/tune_definition.desc +++ b/src/lib/tunes/tune_definition.desc @@ -97,12 +97,10 @@ PX4_DEFINE_TUNE(8, BATTERY_WARNING_FAST, "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a PX4_DEFINE_TUNE(9, GPS_WARNING, "MFT255L4AAAL1F#", false /* gps warning slow */) PX4_DEFINE_TUNE(10, ARMING_FAILURE, "MFT255L4<<G#6A#6B#4", false /* home set tune */) -PX4_DEFINE_TUNE(16, SD_INIT, "MFAGPAG", false /* Make FS */) -PX4_DEFINE_TUNE(17, SD_ERROR, "MNBG", false /* format failed */) -PX4_DEFINE_TUNE(18, PROG_PX4IO, "MLL32CP8MB", false /* Program PX4IO */) -PX4_DEFINE_TUNE(19, PROG_PX4IO_OK, "MLL8CDE", false /* Program PX4IO success */) -PX4_DEFINE_TUNE(20, PROG_PX4IO_ERR, "ML<G#6A#6B#4", false /* home set tune */) +PX4_DEFINE_TUNE(14, SD_INIT, "MFAGPAG", false /* Make FS */) +PX4_DEFINE_TUNE(15, SD_ERROR, "MNBG", false /* format failed */) +PX4_DEFINE_TUNE(16, PROG_PX4IO, "MLL32CP8MB", false /* Program PX4IO */) +PX4_DEFINE_TUNE(17, PROG_PX4IO_OK, "MLL8CDE", false /* Program PX4IO success */) +PX4_DEFINE_TUNE(18, PROG_PX4IO_ERR, "ML< #include -#define TUNE_CONTINUE 1 -#define TUNE_ERROR -1 -#define TUNE_STOP 0 - #define BEAT_TIME_CONVERSION_MS (60 * 1000 * 4) #define BEAT_TIME_CONVERSION_US BEAT_TIME_CONVERSION_MS * 1000 #define BEAT_TIME_CONVERSION BEAT_TIME_CONVERSION_US @@ -153,10 +149,9 @@ void Tunes::set_string(const char *const string, uint8_t volume) } } -int Tunes::get_next_note(unsigned &frequency, unsigned &duration, - unsigned &silence, uint8_t &volume) +Tunes::Status Tunes::get_next_note(unsigned &frequency, unsigned &duration, unsigned &silence, uint8_t &volume) { - int ret = get_next_note(frequency, duration, silence); + Tunes::Status ret = get_next_note(frequency, duration, silence); // Check if note should not be heard -> adjust volume to 0 to be safe. if (frequency == 0 || duration == 0) { @@ -169,8 +164,7 @@ int Tunes::get_next_note(unsigned &frequency, unsigned &duration, return ret; } -int Tunes::get_next_note(unsigned &frequency, unsigned &duration, - unsigned &silence) +Tunes::Status Tunes::get_next_note(unsigned &frequency, unsigned &duration, unsigned &silence) { // Return the values for frequency and duration if the custom msg was received. if (_using_custom_msg) { @@ -178,7 +172,7 @@ int Tunes::get_next_note(unsigned &frequency, unsigned &duration, duration = _duration; frequency = _frequency; silence = _silence; - return TUNE_CONTINUE; + return Tunes::Status::Continue; } // Make sure we still have a tune. @@ -274,7 +268,7 @@ int Tunes::get_next_note(unsigned &frequency, unsigned &duration, frequency = 0; duration = 0; silence = rest_duration(next_number(), next_dots()); - return TUNE_CONTINUE; + return Tunes::Status::Continue; case 'T': { // Change tempo. unsigned nt = next_number(); @@ -299,7 +293,7 @@ int Tunes::get_next_note(unsigned &frequency, unsigned &duration, if (note == 0) { // This is a rest - pause for the current note length. silence = rest_duration(_note_length, next_dots()); - return TUNE_CONTINUE; + return Tunes::Status::Continue; } break; @@ -350,29 +344,29 @@ int Tunes::get_next_note(unsigned &frequency, unsigned &duration, // Compute the note frequency. frequency = note_to_frequency(note); - return TUNE_CONTINUE; + return Tunes::Status::Continue; } -int Tunes::tune_end() +Tunes::Status Tunes::tune_end() { // Restore intial parameters. reset(_repeat); // Stop or restart the tune. if (_repeat) { - return TUNE_CONTINUE; + return Tunes::Status::Continue; } else { - return TUNE_STOP; + return Tunes::Status::Stop; } } -int Tunes::tune_error() +Tunes::Status Tunes::tune_error() { // The tune appears to be bad (unexpected EOF, bad character, etc.). _repeat = false; // Don't loop on error. reset(_repeat); - return TUNE_ERROR; + return Tunes::Status::Error; } uint32_t Tunes::note_to_frequency(unsigned note) const diff --git a/src/lib/tunes/tunes.h b/src/lib/tunes/tunes.h index c30335afa8..b2d807eaf0 100644 --- a/src/lib/tunes/tunes.h +++ b/src/lib/tunes/tunes.h @@ -58,6 +58,12 @@ class Tunes public: enum class NoteMode {NORMAL, LEGATO, STACCATO}; + enum class Status { + Continue = 1, + Stop = 0, + Error = -1, + }; + /** * Constructor with the default parameters set to: * default_tempo: TUNE_DEFAULT_TEMPO @@ -99,7 +105,7 @@ public: * @param silence return silence duration (us) * @return -1 for error, 0 for play one tone and 1 for continue a sequence */ - int get_next_note(unsigned &frequency, unsigned &duration, unsigned &silence); + Tunes::Status get_next_note(unsigned &frequency, unsigned &duration, unsigned &silence); /** * Get next note in the current tune, which has been provided by either @@ -110,8 +116,8 @@ public: * @param volume return the volume level of the note (between 0-100) * @return -1 for no tune available/error, 0 to not play anything and 1 to play */ - int get_next_note(unsigned &frequency, unsigned &duration, - unsigned &silence, uint8_t &volume); + Tunes::Status get_next_note(unsigned &frequency, unsigned &duration, + unsigned &silence, uint8_t &volume); /** * Get the number of default tunes. This is useful for when a tune is @@ -182,9 +188,9 @@ private: */ void reset(bool repeat_flag); - int tune_end(); + Tunes::Status tune_end(); - int tune_error(); + Tunes::Status tune_error(); static const char *const _default_tunes[]; static const bool _default_tunes_interruptable[]; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 434f75dc7c..46c7336154 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1593,7 +1593,7 @@ Commander::run() if (_safety.safety_switch_available && previous_safety_off != _safety.safety_off) { if (_safety.safety_off) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE); } else { tune_neutral(true); @@ -2271,7 +2271,7 @@ Commander::run() armed.force_failsafe = true; _flight_termination_triggered = true; mavlink_log_emergency(&mavlink_log_pub, "Critical failure detected: terminate flight"); - set_tune_override(TONE_PARACHUTE_RELEASE_TUNE); + set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE); } } } @@ -2439,25 +2439,25 @@ Commander::run() (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) { /* play tune when armed */ - set_tune(TONE_ARMING_WARNING_TUNE); + set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); _arm_tune_played = true; } else if (!status_flags.usb_connected && (status.hil_state != vehicle_status_s::HIL_STATE_ON) && (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { /* play tune on battery critical */ - set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); } else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) && (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); } else if (status.failsafe) { tune_failsafe(true); } else { - set_tune(TONE_STOP_TUNE); + set_tune(tune_control_s::TUNE_ID_STOP); } /* reset arm_tune_played when disarmed */ @@ -2477,7 +2477,7 @@ Commander::run() if (!sensor_fail_tune_played && (!status_flags.condition_system_sensors_initialized && status_flags.condition_system_hotplug_timeout)) { - set_tune_override(TONE_GPS_WARNING_TUNE); + set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); sensor_fail_tune_played = true; _status_changed = true; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 3edbcf60aa..6e5ef47f7c 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -538,7 +538,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, /* inform user about already handled side */ if (side_data_collected[orient]) { orientation_failures++; - set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE); calibration_log_info(mavlink_log_pub, "[cal] %s side already completed", detect_orientation_str(orient)); px4_usleep(20000); continue; @@ -566,7 +566,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, side_data_collected[orient] = true; // output neutral tune - set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL); // temporary priority boost for the white blinking led to come trough rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST, 3, 1); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 029757a12c..25f12befce 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -125,8 +125,8 @@ bool is_ground_rover(const struct vehicle_status_s *current_status) static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence -static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end -static unsigned int tune_durations[TONE_NUMBER_OF_TUNES] {}; +static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; // currently playing tune, can be interrupted after tune_end +static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {}; static int fd_leds{-1}; @@ -137,15 +137,17 @@ static orb_advert_t tune_control_pub = nullptr; int buzzer_init() { - tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; - tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; - tune_durations[TONE_HOME_SET] = 800000; - tune_durations[TONE_BATTERY_WARNING_FAST_TUNE] = 800000; - tune_durations[TONE_BATTERY_WARNING_SLOW_TUNE] = 800000; - tune_durations[TONE_SINGLE_BEEP_TUNE] = 300000; + tune_durations[tune_control_s::TUNE_ID_NOTIFY_POSITIVE] = 800000; + tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEGATIVE] = 900000; + tune_durations[tune_control_s::TUNE_ID_NOTIFY_NEUTRAL] = 500000; + tune_durations[tune_control_s::TUNE_ID_ARMING_WARNING] = 3000000; + tune_durations[tune_control_s::TUNE_ID_HOME_SET] = 800000; + tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_FAST] = 800000; + tune_durations[tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW] = 800000; + tune_durations[tune_control_s::TUNE_ID_SINGLE_BEEP] = 300000; + tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); + return PX4_OK; } @@ -158,7 +160,7 @@ void set_tune_override(int tune) { tune_control.tune_id = tune; tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - tune_control.tune_override = 1; + tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); } @@ -173,7 +175,7 @@ void set_tune(int tune) if (tune != tune_current || new_tune_duration != 0) { tune_control.tune_id = tune; tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - tune_control.tune_override = 0; + tune_control.tune_override = false; tune_control.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); } @@ -195,7 +197,7 @@ void tune_home_set(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_HOME_SET); + set_tune(tune_control_s::TUNE_ID_HOME_SET); } } @@ -205,7 +207,7 @@ void tune_mission_ok(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL); } } @@ -215,7 +217,7 @@ void tune_mission_fail(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE); } } @@ -228,7 +230,7 @@ void tune_positive(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_GREEN, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_POSITIVE); } } @@ -241,7 +243,7 @@ void tune_neutral(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_WHITE, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEUTRAL); } } @@ -254,7 +256,7 @@ void tune_negative(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_RED, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + set_tune(tune_control_s::TUNE_ID_NOTIFY_NEGATIVE); } } @@ -264,7 +266,7 @@ void tune_failsafe(bool use_buzzer) rgbled_set_color_and_mode(led_control_s::COLOR_PURPLE, led_control_s::MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7cc13f63db..d5cd517d4b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -255,7 +255,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta float mag_sphere_radius = get_sphere_radius(); // notify user to start rotating - set_tune(TONE_SINGLE_BEEP_TUNE); + set_tune(tune_control_s::TUNE_ID_SINGLE_BEEP); calibration_log_info(worker_data->mavlink_log_pub, "[cal] Rotate vehicle"); diff --git a/src/modules/events/rc_loss_alarm.cpp b/src/modules/events/rc_loss_alarm.cpp index cd522f2850..a1df1c38ba 100644 --- a/src/modules/events/rc_loss_alarm.cpp +++ b/src/modules/events/rc_loss_alarm.cpp @@ -45,8 +45,6 @@ #include -#include - namespace events { namespace rc_loss @@ -84,32 +82,20 @@ void RC_Loss_Alarm::process() void RC_Loss_Alarm::play_tune() { - struct tune_control_s tune_control = {}; - tune_control.timestamp = hrt_absolute_time(); - tune_control.tune_id = static_cast(TuneID::ERROR_TUNE); - tune_control.tune_override = 1; - tune_control.volume = tune_control_s::VOLUME_LEVEL_MAX; - - if (_tune_control_pub == nullptr) { - _tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(tune_control), _tune_control_pub, &tune_control); - } + tune_control_s tune_control{}; + tune_control.tune_id = tune_control_s::TUNE_ID_ERROR; + tune_control.tune_override = true; + tune_control.volume = tune_control_s::VOLUME_LEVEL_MAX; + tune_control.timestamp = hrt_absolute_time(); + _tune_control_pub.publish(tune_control); } void RC_Loss_Alarm::stop_tune() { - struct tune_control_s tune_control = {}; + tune_control_s tune_control{}; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); - - if (_tune_control_pub == nullptr) { - _tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(tune_control), _tune_control_pub, &tune_control); - } + _tune_control_pub.publish(tune_control); } } /* namespace rc_loss */ diff --git a/src/modules/events/rc_loss_alarm.h b/src/modules/events/rc_loss_alarm.h index d51ab71a03..2e106b74e5 100644 --- a/src/modules/events/rc_loss_alarm.h +++ b/src/modules/events/rc_loss_alarm.h @@ -39,8 +39,10 @@ #pragma once +#include #include #include +#include namespace events { @@ -63,12 +65,12 @@ private: /** Publish tune control to interrupt any sound */ void stop_tune(); + uORB::PublicationQueued _tune_control_pub{ORB_ID(tune_control)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; bool _was_armed = false; bool _had_rc = false; // Don't trigger alarm for systems without RC bool _alarm_playing = false; - orb_advert_t _tune_control_pub = nullptr; }; } /* namespace rc_loss */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 454157a6e6..0879e16c02 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -44,7 +44,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/mavlink/tune_publisher.cpp b/src/modules/mavlink/tune_publisher.cpp index 2e6bb29eb4..baba588503 100644 --- a/src/modules/mavlink/tune_publisher.cpp +++ b/src/modules/mavlink/tune_publisher.cpp @@ -72,17 +72,13 @@ void TunePublisher::publish_next_tune(const hrt_abstime now) unsigned silence; uint8_t volume; - if (_tunes.get_next_note(frequency, duration, silence, volume) > 0) { - tune_control_s tune_control {}; - tune_control.tune_id = 0; - tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - - tune_control.tune_id = 0; + if (_tunes.get_next_note(frequency, duration, silence, volume) == Tunes::Status::Continue) { + tune_control_s tune_control{}; tune_control.frequency = static_cast(frequency); tune_control.duration = static_cast(duration); tune_control.silence = static_cast(silence); tune_control.volume = static_cast(volume); - tune_control.timestamp = now; + tune_control.timestamp = hrt_absolute_time(); _tune_control_pub.publish(tune_control); _next_publish_time = now + duration + silence; diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index be1f31262e..877c257e78 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -69,7 +69,6 @@ set(srcs test_servo.c test_sleep.c test_smooth_z.cpp - test_tone.cpp test_uart_baudchange.c test_uart_console.c test_uart_loopback.c diff --git a/src/systemcmds/tests/test_tone.cpp b/src/systemcmds/tests/test_tone.cpp deleted file mode 100644 index 283562bef4..0000000000 --- a/src/systemcmds/tests/test_tone.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2019 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 - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file test_tone.cpp - * Test for audio tones. - */ - -#include - -#include -#include -#include -#include -#include - -#include "tests_main.h" - -int test_tone(int argc, char *argv[]) -{ - int result = PX4_ERROR; - tune_control_s tune_control = {}; - tune_control.tune_id = static_cast(TuneID::NOTIFY_NEGATIVE); - - orb_advert_t tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, - tune_control_s::ORB_QUEUE_LENGTH); - - if (argc == 1) { - PX4_INFO("Volume silenced for testing predefined tunes 0-20."); - tune_control.volume = tune_control_s::VOLUME_LEVEL_MIN; - - for (size_t i = 0; i <= 20; i++) { - tune_control.tune_id = i; - result = orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); - - if (result != PX4_OK) { - PX4_INFO("Error publishing TuneID: %d", tune_control.tune_id); - return result; - } - } - - tune_control.tune_id = static_cast(TuneID::NOTIFY_POSITIVE); - } - - if (argc == 2) { - tune_control.tune_id = atoi(argv[1]); - - if (tune_control.tune_id <= 20) { - PX4_INFO("TuneID: %d", tune_control.tune_id); - } - } - - if (argc == 3) { - int volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - Tunes tunes{}; - tunes.set_string(argv[2], volume); - PX4_INFO("Custom tune."); - } - - tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; - result = orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); - - orb_unadvertise(tune_control_pub); - return result; -} diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index fbfa260eb8..3d17548ca3 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -41,6 +41,7 @@ #include "tests_main.h" #include +#include #include #include @@ -114,7 +115,6 @@ const struct { {"servo", test_servo, OPT_NOJIGTEST | OPT_NOALLTEST}, {"sleep", test_sleep, OPT_NOJIGTEST}, {"smoothz", test_smooth_z, 0}, - {"tone", test_tone, 0}, {"uart_loopback", test_uart_loopback, OPT_NOJIGTEST | OPT_NOALLTEST}, {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST}, {"versioning", test_versioning, 0}, @@ -246,51 +246,26 @@ test_runner(unsigned option) return (failcount > 0); } -__EXPORT int tests_main(int argc, char *argv[]); - -/** - * Executes system tests. - */ -int tests_main(int argc, char *argv[]) +__EXPORT int tests_main(int argc, char *argv[]) { if (argc < 2) { - printf("tests: missing test name - 'tests help' for a list of tests\n"); + PX4_WARN("tests: missing test name - 'tests help' for a list of tests"); return 1; } - int tone_test_index = -1; - char *tone_test = {"tone"}; - char *tone_fail[2] = {NULL, "2"}; - char *tone_pass[2] = {NULL, "14"}; - - // Identify the tone test index for later use. - for (size_t i = 0; tests[i].name; i++) { - if (*tone_test == *tests[i].name) { - tone_test_index = i; - } - } - for (size_t i = 0; tests[i].name; i++) { if (!strcmp(tests[i].name, argv[1])) { if (tests[i].fn(argc - 1, argv + 1) == 0) { - if (tone_test_index != -1) { - tests[tone_test_index].fn(2, tone_pass); // Play a notification. - } - - printf("%s PASSED\n", tests[i].name); + PX4_INFO("%s PASSED", tests[i].name); return 0; } else { - if (tone_test_index != -1) { - tests[tone_test_index].fn(2, tone_fail); // Play an error notification. - } - - printf("%s FAILED\n", tests[i].name); + PX4_ERR("%s FAILED", tests[i].name); return -1; } } } - printf("tests: no test called '%s' - 'tests help' for a list of tests\n", argv[1]); + PX4_WARN("tests: no test called '%s' - 'tests help' for a list of tests", argv[1]); return 1; } diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 8b3d49756c..7ac3669f36 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -82,7 +82,6 @@ extern int test_servo(int argc, char *argv[]); extern int test_sleep(int argc, char *argv[]); extern int test_smooth_z(int argc, char *argv[]); extern int test_time(int argc, char *argv[]); -extern int test_tone(int argc, char *argv[]); extern int test_uart_baudchange(int argc, char *argv[]); extern int test_uart_break(int argc, char *argv[]); extern int test_uart_console(int argc, char *argv[]); diff --git a/src/systemcmds/tune_control/CMakeLists.txt b/src/systemcmds/tune_control/CMakeLists.txt index c90ae25793..d00586054f 100644 --- a/src/systemcmds/tune_control/CMakeLists.txt +++ b/src/systemcmds/tune_control/CMakeLists.txt @@ -33,6 +33,8 @@ px4_add_module( MODULE systemcmds__tune_control MAIN tune_control + PRIORITY + "SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads COMPILE_FLAGS SRCS tune_control.cpp diff --git a/src/systemcmds/tune_control/tune_control.cpp b/src/systemcmds/tune_control/tune_control.cpp index 70da7fdc41..c0bdfb5ecb 100644 --- a/src/systemcmds/tune_control/tune_control.cpp +++ b/src/systemcmds/tune_control/tune_control.cpp @@ -49,35 +49,23 @@ #include #include +#include #include #include #define MAX_NOTE_ITERATION 50 -static void usage(); - -static orb_advert_t tune_control_pub = nullptr; - -extern "C" { - __EXPORT int tune_control_main(int argc, char *argv[]); -} +static void usage(); static void publish_tune_control(tune_control_s &tune_control) { + uORB::PublicationQueued tune_control_pub{ORB_ID(tune_control)}; tune_control.timestamp = hrt_absolute_time(); - - if (tune_control_pub == nullptr) { - // We have a minimum of 3 so that tune, stop, tune will fit - tune_control_pub = orb_advertise_queue(ORB_ID(tune_control), &tune_control, tune_control_s::ORB_QUEUE_LENGTH); - - } else { - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); - } + tune_control_pub.publish(tune_control); } -int -tune_control_main(int argc, char *argv[]) +extern "C" __EXPORT int tune_control_main(int argc, char *argv[]) { Tunes tunes; bool string_input = false; @@ -86,8 +74,7 @@ tune_control_main(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; unsigned int value; - tune_control_s tune_control = {}; - tune_control.tune_id = 0; + tune_control_s tune_control{}; tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; while ((ch = px4_getopt(argc, argv, "f:d:t:m:s:", &myoptind, &myoptarg)) != EOF) { @@ -163,11 +150,15 @@ tune_control_main(int argc, char *argv[]) int exit_counter = 0; if (!strcmp(argv[myoptind], "play")) { - if (string_input) { + if (argc >= 2 && !strcmp(argv[2], "error")) { + tune_control.tune_id = tune_control_s::TUNE_ID_ERROR; + publish_tune_control(tune_control); + + } else if (string_input) { PX4_INFO("Start playback..."); tunes.set_string(tune_string, tune_control.volume); - while (tunes.get_next_note(frequency, duration, silence, volume) > 0) { + while (tunes.get_next_note(frequency, duration, silence, volume) == Tunes::Status::Continue) { tune_control.tune_id = 0; tune_control.frequency = (uint16_t)frequency; tune_control.duration = (uint32_t)duration; @@ -185,12 +176,13 @@ tune_control_main(int argc, char *argv[]) PX4_INFO("Playback finished."); - } else { // tune id instead of string has been provided + } else { + // tune id instead of string has been provided if (tune_control.tune_id == 0) { tune_control.tune_id = 1; } - PX4_INFO("Publishing standard tune %d", tune_control.tune_id); + PX4_DEBUG("Publishing standard tune %d", tune_control.tune_id); publish_tune_control(tune_control); } @@ -201,7 +193,7 @@ tune_control_main(int argc, char *argv[]) PX4_WARN("Tune ID not recognized."); } - while (tunes.get_next_note(frequency, duration, silence, volume) > 0) { + while (tunes.get_next_note(frequency, duration, silence, volume) == Tunes::Status::Continue) { PX4_INFO("frequency: %d, duration %d, silence %d, volume %d", frequency, duration, silence, volume); @@ -235,10 +227,8 @@ tune_control_main(int argc, char *argv[]) return 0; } -static void -usage() +static void usage() { - PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description @@ -258,14 +248,12 @@ $ tune_control play -t 2 )DESCR_STR"); PRINT_MODULE_USAGE_NAME("tune_control", "system"); - PRINT_MODULE_USAGE_COMMAND_DESCR("play","Play system tune or single note."); + PRINT_MODULE_USAGE_COMMAND_DESCR("play", "Play system tune or single note."); PRINT_MODULE_USAGE_PARAM_INT('t', 1, 1, 21, "Play predefined system tune", true); PRINT_MODULE_USAGE_PARAM_INT('f', -1, 0, 22, "Frequency of note in Hz (0-22kHz)", true); PRINT_MODULE_USAGE_PARAM_INT('d', -1, 1, 21, "Duration of note in us", true); PRINT_MODULE_USAGE_PARAM_INT('s', 40, 0, 100, "Volume level (loudness) of the note (0-100)", true); - PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, R"( - e.g. "MFT200e8a8a")", - "Melody in string form", true); - PRINT_MODULE_USAGE_COMMAND_DESCR("libtest","Test library"); - PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop playback (use for repeated tunes)"); - + PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, R"( - e.g. "MFT200e8a8a")", "Melody in string form", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("libtest", "Test library"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop playback (use for repeated tunes)"); }