mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-28 01:13:35 +08:00
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
@@ -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();
|
||||
}
|
||||
|
||||
_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_s> 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);
|
||||
}
|
||||
|
||||
@@ -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();
|
||||
};
|
||||
|
||||
Reference in New Issue
Block a user