forked from Archive/PX4-Autopilot
drivers/tone_alarm and tune_control small improvements/cleanup
- drivers/tone_alarm: move to ModuleBase and purge CDev (/dev/tone_alarm0) - drivers/tone_alarm: only run on tune_control publication (or scheduled note) rather than continuously - drivers/tone_alarm: use HRT to schedule tone stop (prevents potential disruption) - msg/tune_control: add tune_id numbering - systemcmds/tune_control: add "error" special case tune_id - move all tune_control publication to new uORB::PublicationQueued<> - start tone_alarm immediately after board defaults are loaded to fix potential startup issues - for SITL (or other boards with no TONE output) print common messages (startup, error, etc)
This commit is contained in:
Daniel Agar
GitHub
parent
c7072b61a3
commit
08bf71b73d
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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 - <TBD>. Selecting pattern zero silences
|
||||
* the alarm.
|
||||
*
|
||||
* To supply a custom pattern, write an array of 1 - <TBD> 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 <lib/tunes/tune_definition.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#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'.
|
||||
|
|
|
|||
|
|
@ -74,6 +74,7 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <lib/mixer/MixerGroup.hpp>
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
|
|
@ -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_s> _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<int>(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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -77,7 +77,6 @@ private:
|
|||
void FlashButton();
|
||||
void CheckPairingRequest(bool button_pressed);
|
||||
|
||||
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Publication<safety_s> _to_safety{ORB_ID(safety)};
|
||||
uORB::PublicationQueued<vehicle_command_s> _to_command{ORB_ID(vehicle_command)};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <px4_platform_common/time.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
if (_cbrk != CBRK_OFF) {
|
||||
_circuit_break_initialized = true;
|
||||
}
|
||||
|
||||
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.
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace
|
||||
_play_tone = true;
|
||||
|
||||
#if (!defined(TONE_ALARM_TIMER) && !defined(GPIO_TONE_ALARM_GPIO)) || defined(DEBUG_BUILD)
|
||||
|
||||
switch (tune_control.tune_id) {
|
||||
case tune_control_s::TUNE_ID_STARTUP:
|
||||
PX4_INFO("startup tune");
|
||||
break;
|
||||
|
||||
case tune_control_s::TUNE_ID_ERROR:
|
||||
PX4_ERR("error tune");
|
||||
break;
|
||||
|
||||
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_s> tune_control_pub{ORB_ID(tune_control)};
|
||||
tune_control.timestamp = hrt_absolute_time();
|
||||
tune_control_pub.publish(tune_control);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int frequency = 0;
|
||||
unsigned int duration = 0;
|
||||
unsigned int silence_length = 0;
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
||||
} 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_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();
|
||||
|
||||
ToneAlarm *g_dev;
|
||||
if (!instance) {
|
||||
PX4_ERR("alloc failed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
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;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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];
|
||||
|
||||
if (!strcmp(argv1, "start")) {
|
||||
if (g_dev == nullptr) {
|
||||
g_dev = new ToneAlarm();
|
||||
|
||||
if (g_dev == nullptr) {
|
||||
PX4_ERR("could not allocate the driver.");
|
||||
return ToneAlarm::main(argc, argv);
|
||||
}
|
||||
|
||||
if (g_dev->init() != OK) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
PX4_ERR("driver init failed.");
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_INFO("already started");
|
||||
}
|
||||
|
||||
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 {
|
||||
PX4_INFO("missing command, try 'start', status, 'stop'");
|
||||
}
|
||||
|
||||
return 0;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -40,79 +40,50 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <circuit_breaker/circuit_breaker.h>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <lib/circuit_breaker/circuit_breaker.h>
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#if !defined(UNUSED)
|
||||
# define UNUSED(a) ((void)(a))
|
||||
#endif
|
||||
|
||||
class ToneAlarm : public cdev::CDev, public px4::ScheduledWorkItem
|
||||
class ToneAlarm : public ModuleBase<ToneAlarm>, 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();
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<<<BAP", false /* arming failure tune */)
|
||||
PX4_DEFINE_TUNE(11, PARACHUTE_RELEASE, "MFT255L16agagagag", false /* parachute release */)
|
||||
PX4_DEFINE_TUNE(12, EKF_WARNING, "MFT255L8ddd#d#eeff", false /* ekf warning */)
|
||||
PX4_DEFINE_TUNE(13, BARO_WARNING, "MFT255L4gf#fed#d", false /* baro warning */)
|
||||
PX4_DEFINE_TUNE(14, SINGLE_BEEP, "MFT100a8", false /* single beep */)
|
||||
PX4_DEFINE_TUNE(15, HOME_SET, "MFT100L4>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<<CP4CP4CP4CP4CP4", true /* Program PX4IO fail */)
|
||||
PX4_DEFINE_TUNE(12, SINGLE_BEEP, "MFT100a8", false /* single beep */)
|
||||
PX4_DEFINE_TUNE(13, HOME_SET, "MFT100L4>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<<CP4CP4CP4CP4CP4", true /* Program PX4IO fail */)
|
||||
|
|
|
|||
|
|
@ -42,10 +42,6 @@
|
|||
#include <ctype.h>
|
||||
#include <errno.h>
|
||||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -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,7 +116,7 @@ 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,
|
||||
Tunes::Status get_next_note(unsigned &frequency, unsigned &duration,
|
||||
unsigned &silence, uint8_t &volume);
|
||||
|
||||
/**
|
||||
|
|
@ -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[];
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
||||
|
|
|
|||
|
|
@ -45,8 +45,6 @@
|
|||
|
||||
#include <tunes/tune_definition.h>
|
||||
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
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<int>(TuneID::ERROR_TUNE);
|
||||
tune_control.tune_override = 1;
|
||||
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;
|
||||
|
||||
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.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 */
|
||||
|
|
|
|||
|
|
@ -39,8 +39,10 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
namespace events
|
||||
{
|
||||
|
|
@ -63,12 +65,12 @@ private:
|
|||
/** Publish tune control to interrupt any sound */
|
||||
void stop_tune();
|
||||
|
||||
uORB::PublicationQueued<tune_control_s> _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 */
|
||||
|
|
|
|||
|
|
@ -44,7 +44,6 @@
|
|||
#include <commander/px4_custom_mode.h>
|
||||
#include <conversion/rotation.h>
|
||||
#include <drivers/drv_rc_input.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <ecl/geo/geo.h>
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
if (_tunes.get_next_note(frequency, duration, silence, volume) == Tunes::Status::Continue) {
|
||||
tune_control_s tune_control{};
|
||||
tune_control.tune_id = 0;
|
||||
tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT;
|
||||
|
||||
tune_control.tune_id = 0;
|
||||
tune_control.frequency = static_cast<uint16_t>(frequency);
|
||||
tune_control.duration = static_cast<uint32_t>(duration);
|
||||
tune_control.silence = static_cast<uint32_t>(silence);
|
||||
tune_control.volume = static_cast<uint8_t>(volume);
|
||||
tune_control.timestamp = now;
|
||||
tune_control.timestamp = hrt_absolute_time();
|
||||
_tune_control_pub.publish(tune_control);
|
||||
|
||||
_next_publish_time = now + duration + silence;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 <stdlib.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#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<int>(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<int>(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;
|
||||
}
|
||||
|
|
@ -41,6 +41,7 @@
|
|||
#include "tests_main.h"
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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[]);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -49,6 +49,7 @@
|
|||
#include <px4_platform_common/module.h>
|
||||
|
||||
#include <lib/tunes/tunes.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/tune_control.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
|
@ -57,27 +58,14 @@
|
|||
|
||||
static void usage();
|
||||
|
||||
static orb_advert_t tune_control_pub = nullptr;
|
||||
|
||||
extern "C" {
|
||||
__EXPORT int tune_control_main(int argc, char *argv[]);
|
||||
}
|
||||
|
||||
static void publish_tune_control(tune_control_s &tune_control)
|
||||
{
|
||||
uORB::PublicationQueued<tune_control_s> 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
|
||||
|
|
@ -263,9 +253,7 @@ $ tune_control play -t 2
|
|||
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"(<string> - e.g. "MFT200e8a8a")",
|
||||
"Melody in string form", true);
|
||||
PRINT_MODULE_USAGE_PARAM_STRING('m', nullptr, R"(<string> - 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)");
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue