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

moved rc_loss_alarm into the event module

Browse Source
This commit is contained in:
Alessandro Simovic
2018-07-05 14:17:55 +02:00
committed by Beat Küng
parent 9a4b26cd32
commit c684275018
9 changed files with 144 additions and 318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cmake/configs/nuttx_px4fmu-v4_default.cmake
-5
View File
@@ -115,11 +115,6 @@ set(config_module_list
#
modules/dataman
#
# RC Loss Alarm
#
modules/rc_loss_alarm
#
# OBC challenge
#
src/modules/events/CMakeLists.txt
+4 -4
View File
@@ -37,15 +37,15 @@ px4_add_module(
STACK_MAIN 2200
COMPILE_FLAGS
SRCS
rc_loss_alarm.cpp
send_event.cpp
set_leds.cpp
status_display.cpp
subscriber_handler.cpp
temperature_calibration/accel.cpp
temperature_calibration/baro.cpp
temperature_calibration/gyro.cpp
temperature_calibration/task.cpp
subscriber_handler.cpp
status_display.cpp
set_leds.cpp
DEPENDS
modules__uORB
)
src/modules/events/rc_loss_alarm.cpp
+103
View File
@@ -0,0 +1,103 @@
/****************************************************************************
*
* Copyright (c) 2018 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 PX4 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 rc_loss_alarm.cpp
*
*/
#include "rc_loss_alarm.h"
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <stdint.h>
#include <tunes/tune_definition.h>
#include <uORB/topics/tune_control.h>
RC_Loss_Alarm::RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler)
: _subscriber_handler(subscriber_handler)
{
}
bool RC_Loss_Alarm::check_for_updates()
{
if (_subscriber_handler.vehicle_status_updated()) {
orb_copy(ORB_ID(vehicle_status), _subscriber_handler.get_vehicle_status_sub(), &_vehicle_status);
return true;
}
return false;
}
void RC_Loss_Alarm::process()
{
if (!check_for_updates()) {
return;
}
if (!_was_armed &&
_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
_was_armed = true; // Once true, impossible to go back to false
}
if (!_had_rc && !_vehicle_status.rc_signal_lost) {
_had_rc = true;
}
if (_was_armed && _had_rc && _vehicle_status.rc_signal_lost &&
_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
play_tune();
}
}
void RC_Loss_Alarm::play_tune()
{
struct tune_control_s tune_control = {};
if (_tune_control_pub == nullptr) {
_tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
} else {
tune_control.tune_id = static_cast<int>(TuneID::ERROR_TUNE);
tune_control.strength = tune_control_s::STRENGTH_MAX;
tune_control.tune_override = 1;
tune_control.timestamp = hrt_absolute_time();
orb_publish(ORB_ID(tune_control), _tune_control_pub, &tune_control);
}
}
src/modules/rc_loss_alarm/rc_loss_alarm.h → src/modules/events/rc_loss_alarm.h
+28 -30
View File
@@ -31,47 +31,45 @@
*
****************************************************************************/
/**
* @file rc_loss_alarm.h
* Tone alarm in the event of RC loss
*
*/
#pragma once
#include <px4_module.h>
#include <px4_workqueue.h>
#include <tunes/tune_definition.h>
#include "subscriber_handler.h"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#define UPDATE_RATE (1000000) /* microseconds, 1 Hz */
class RC_Loss_Alarm: public ModuleBase<RC_Loss_Alarm>
class RC_Loss_Alarm
{
public:
RC_Loss_Alarm() = default;
~RC_Loss_Alarm();
RC_Loss_Alarm(const RC_Loss_Alarm &other) = delete;
RC_Loss_Alarm(const RC_Loss_Alarm &&other) = delete;
RC_Loss_Alarm &operator= (const RC_Loss_Alarm &other) = delete;
RC_Loss_Alarm &operator= (const RC_Loss_Alarm &&other) = delete;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** regularily called to handle state updates */
void process();
private:
static struct work_s _work;
int _vehicle_status_sub = -1;
static orb_advert_t _tune_control_pub;
static bool _was_armed;
static bool _had_rc; // Don't trigger alarm for systems without RC
/**
* check for topic updates
* @return true if one or more topics got updated
*/
bool check_for_updates();
static void cycle_trampoline(void *arg);
void cycle();
static void pub_tune();
static void stop_tune();
static int reset_module();
/** Publish tune control to sound alarm */
void play_tune();
/** Publish tune control to stop any ongoing tune, including alarm */
void stop_tune();
int _vehicle_status_sub = -1;
struct vehicle_status_s _vehicle_status = {};
bool _was_armed = false;
bool _had_rc = false; // Don't trigger alarm for systems without RC
orb_advert_t _tune_control_pub = nullptr;
const events::SubscriberHandler &_subscriber_handler;
};
src/modules/events/send_event.cpp
+3 -2
View File
@@ -64,7 +64,7 @@ int SendEvent::task_spawn(int argc, char *argv[])
}
SendEvent::SendEvent()
: _status_display(_subscriber_handler)
: _status_display(_subscriber_handler), _rc_loss_alarm(_subscriber_handler)
{
}
@@ -117,6 +117,7 @@ void SendEvent::cycle()
process_commands();
_status_display.process();
_rc_loss_alarm.process();
work_queue(LPWORK, &_work, (worker_t)&SendEvent::cycle_trampoline, this,
USEC2TICK(SEND_EVENT_INTERVAL_US));
@@ -200,7 +201,7 @@ int SendEvent::print_usage(const char *reason)
R"DESCR_STR(
### Description
Background process running periodically on the LP work queue to perform housekeeping tasks.
It is currently only responsible for temperature calibration.
It is currently only responsible for temperature calibration and tone alarm on RC Loss.
The tasks can be started via CLI or uORB topics (vehicle_command from MAVLink, etc.).
)DESCR_STR");
src/modules/events/send_event.h
+2
View File
@@ -35,6 +35,7 @@
#include "subscriber_handler.h"
#include "status_display.h"
#include "rc_loss_alarm.h"
#include <px4_workqueue.h>
#include <px4_module.h>
@@ -84,5 +85,6 @@ private:
static struct work_s _work;
events::SubscriberHandler _subscriber_handler;
status::StatusDisplay _status_display;
RC_Loss_Alarm _rc_loss_alarm;
orb_advert_t _command_ack_pub = nullptr;
};
src/modules/events/status_display.h
+4 -4
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2017 PX4 Development Team. All rights reserved.
* Copyright (c) 2017-2018 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
@@ -33,7 +33,7 @@
/**
* @file status_display.h
* Status Display decouple the LED and tune form the original commander
* Status Display decouples LED and tunes from commander
*
* @author Simone Guscetti <simone@px4.io>
*/
@@ -66,7 +66,7 @@ public:
protected:
/**
* check for topic updates
* @return true if one or more topic got updated
* @return true if one or more topics got updated
*/
bool check_for_updates();
@@ -78,7 +78,7 @@ protected:
/** publish LED control */
void publish();
// TODO: review if there is a better variant that allocate this in the memory
// TODO: review if there is a better variant that allocates this in the memory
struct battery_status_s _battery_status = {};
struct cpuload_s _cpu_load = {};
struct vehicle_status_s _vehicle_status = {};
src/modules/rc_loss_alarm/CMakeLists.txt
-42
View File
@@ -1,42 +0,0 @@
############################################################################
#
# Copyright (c) 2018 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 PX4 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.
#
############################################################################
px4_add_module(
MODULE modules__rc_loss_alarm
MAIN rc_loss_alarm
STACK_MAIN 512
COMPILE_FLAGS
SRCS
rc_loss_alarm.cpp
DEPENDS
)
src/modules/rc_loss_alarm/rc_loss_alarm.cpp
-231
View File
@@ -1,231 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 PX4 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.
*
****************************************************************************/
#include "rc_loss_alarm.h"
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <stdint.h>
#include <uORB/topics/tune_control.h>
// Init static members
work_s RC_Loss_Alarm::_work = {};
bool RC_Loss_Alarm::_was_armed = false;
bool RC_Loss_Alarm::_had_rc = false;
orb_advert_t RC_Loss_Alarm::_tune_control_pub = nullptr;
RC_Loss_Alarm::~RC_Loss_Alarm()
{
work_cancel(LPWORK, &_work);
orb_unsubscribe(_vehicle_status_sub);
orb_unadvertise(_tune_control_pub);
}
/** @see ModuleBase */
int RC_Loss_Alarm::task_spawn(int argc, char *argv[])
{
// Instantiate task
RC_Loss_Alarm *rc_loss_alarm = nullptr;
if (_object == nullptr) {
rc_loss_alarm = new RC_Loss_Alarm();
if (rc_loss_alarm == nullptr) {
PX4_ERR("failed to start flow");
return PX4_ERROR;
}
}
_object = rc_loss_alarm;
// schedule a cycle to start things
int ret = work_queue(LPWORK, &_work,
(worker_t)&RC_Loss_Alarm::cycle_trampoline, rc_loss_alarm, 0);
if (ret < 0) {
return ret;
}
// Since this task runs on the work-queue rather than in a separate thread,
// we specify the workqueue task_id
_task_id = task_id_is_work_queue;
// wait until task is up & running (the mode_* commands depend on it)
if (wait_until_running() < 0) {
return -1;
}
return PX4_OK;
}
/** @see ModuleBase */
int RC_Loss_Alarm::custom_command(int argc, char *argv[])
{
if (!strcmp(argv[0], "test")) {
RC_Loss_Alarm::pub_tune();
return PX4_OK;
} else if (!strcmp(argv[0], "reset")) {
return RC_Loss_Alarm::reset_module();
}
return print_usage("unknown command");
}
/** @see ModuleBase */
int RC_Loss_Alarm::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
RC Loss Alarm plays a loud error tune in the event that the drone loses RC
link after disarming. Ideal for finding a lost drone. The RC Loss Module
subscribes to the vehicle_status topic and checks the armed state as well as
the rc_signal_lost flag. The RC Loss alarm will only be triggered if
rc_signal_lost flag was false at least once at some point (i.e. if an RC is
connected).
### Example
The module is typically started with:
rc_loss_alarm start
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("rc_loss_alarm", "driver");
PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task");
PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop the task");
PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Display running status of task");
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Start alarm tune, can be stopped with 'reset'");
PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Resets internal state and stops an eventual alarm");
return PX4_OK;
}
void RC_Loss_Alarm::cycle_trampoline(void *arg)
{
RC_Loss_Alarm *dev = reinterpret_cast<RC_Loss_Alarm *>(arg);
dev->cycle();
}
void RC_Loss_Alarm::cycle()
{
// Subscribe if necessary
if(_vehicle_status_sub == -1){
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}
// Check armed status
bool updated = false;
orb_check(_vehicle_status_sub, &updated);
if(updated){
struct vehicle_status_s _vehicle_status = {};
orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
if (!_was_armed &&
_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED){
_was_armed = true; // Once true, impossible to go back to false
}
if (!_had_rc && !_vehicle_status.rc_signal_lost){
_had_rc = true;
}
if (_was_armed && _had_rc && _vehicle_status.rc_signal_lost &&
_vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED){
pub_tune();
}
}
// Schedule next cycle unless module is shutting down
if (!should_exit()) {
work_queue(LPWORK, &_work, (worker_t)&RC_Loss_Alarm::cycle_trampoline, this,
USEC2TICK(UPDATE_RATE));
}
}
void RC_Loss_Alarm::pub_tune()
{
struct tune_control_s tune_control = {};
tune_control.tune_id = static_cast<int>(TuneID::ERROR_TUNE);
tune_control.strength = tune_control_s::STRENGTH_MAX;
tune_control.tune_override = 1;
tune_control.timestamp = hrt_absolute_time();
if (_tune_control_pub == nullptr) {
_tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
}else{
orb_publish(ORB_ID(tune_control), _tune_control_pub, &tune_control);
}
}
void RC_Loss_Alarm::stop_tune()
{
struct tune_control_s tune_control = {};
tune_control.tune_id = static_cast<int>(TuneID::CUSTOM);
tune_control.frequency = 0;
tune_control.duration = 0;
tune_control.silence = 0;
tune_control.tune_override = true;
if (_tune_control_pub == nullptr) {
_tune_control_pub = orb_advertise(ORB_ID(tune_control), &tune_control);
}else{
orb_publish(ORB_ID(tune_control), _tune_control_pub, &tune_control);
}
}
int RC_Loss_Alarm::reset_module(){
RC_Loss_Alarm::_was_armed = false;
RC_Loss_Alarm::_had_rc = false;
RC_Loss_Alarm::stop_tune();
return PX4_OK;
}
// module 'main' command
extern "C" __EXPORT int rc_loss_alarm_main(int argc, char *argv[]);
int rc_loss_alarm_main(int argc, char *argv[])
{
return RC_Loss_Alarm::main(argc, argv);
}