fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 08:10:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/tone_alarm and tune_control small improvements/cleanup

Browse Source 
- 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
2020-10-05 21:39:26 -04:00
committed by GitHub
parent c7072b61a3
commit 08bf71b73d
39 changed files with 429 additions and 577 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+7 -7
View File
@@ -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;
}
src/modules/commander/calibration_routines.cpp
+2 -2
View File
@@ -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);
src/modules/commander/commander_helper.cpp
+21 -19
View File
@@ -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);
}
}
src/modules/commander/mag_calibration.cpp
+1 -1
View File
@@ -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");
src/modules/events/rc_loss_alarm.cpp
+8 -22
View File
@@ -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.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 */
src/modules/events/rc_loss_alarm.h
+3 -1
View File
@@ -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 */
src/modules/mavlink/mavlink_receiver.cpp
-1
View File
@@ -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>
src/modules/mavlink/tune_publisher.cpp
+3 -7
View File
@@ -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<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;