From c684275018465116211d6b1b99867716e3c1a967 Mon Sep 17 00:00:00 2001 From: Alessandro Simovic Date: Fri, 29 Jun 2018 11:32:05 +0200 Subject: [PATCH] moved rc_loss_alarm into the event module --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 5 - src/modules/events/CMakeLists.txt | 8 +- src/modules/events/rc_loss_alarm.cpp | 103 ++++++++ .../{rc_loss_alarm => events}/rc_loss_alarm.h | 58 +++-- src/modules/events/send_event.cpp | 5 +- src/modules/events/send_event.h | 2 + src/modules/events/status_display.h | 8 +- src/modules/rc_loss_alarm/CMakeLists.txt | 42 ---- src/modules/rc_loss_alarm/rc_loss_alarm.cpp | 231 ------------------ 9 files changed, 144 insertions(+), 318 deletions(-) create mode 100644 src/modules/events/rc_loss_alarm.cpp rename src/modules/{rc_loss_alarm => events}/rc_loss_alarm.h (65%) delete mode 100644 src/modules/rc_loss_alarm/CMakeLists.txt delete mode 100644 src/modules/rc_loss_alarm/rc_loss_alarm.cpp diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 5c0362af3c..c665bf3f22 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -115,11 +115,6 @@ set(config_module_list # modules/dataman - # - # RC Loss Alarm - # - modules/rc_loss_alarm - # # OBC challenge # diff --git a/src/modules/events/CMakeLists.txt b/src/modules/events/CMakeLists.txt index 49a3fb6db2..86607d9bb6 100644 --- a/src/modules/events/CMakeLists.txt +++ b/src/modules/events/CMakeLists.txt @@ -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 ) - diff --git a/src/modules/events/rc_loss_alarm.cpp b/src/modules/events/rc_loss_alarm.cpp new file mode 100644 index 0000000000..e2e9ce1561 --- /dev/null +++ b/src/modules/events/rc_loss_alarm.cpp @@ -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 + +#include +#include + +#include + +#include + +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(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); + } +} diff --git a/src/modules/rc_loss_alarm/rc_loss_alarm.h b/src/modules/events/rc_loss_alarm.h similarity index 65% rename from src/modules/rc_loss_alarm/rc_loss_alarm.h rename to src/modules/events/rc_loss_alarm.h index 0a4ae90961..5449c16441 100644 --- a/src/modules/rc_loss_alarm/rc_loss_alarm.h +++ b/src/modules/events/rc_loss_alarm.h @@ -31,47 +31,45 @@ * ****************************************************************************/ +/** + * @file rc_loss_alarm.h + * Tone alarm in the event of RC loss + * + */ +#pragma once -#include -#include - -#include +#include "subscriber_handler.h" #include #include -#define UPDATE_RATE (1000000) /* microseconds, 1 Hz */ - -class RC_Loss_Alarm: public ModuleBase +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[]); - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); + RC_Loss_Alarm(const events::SubscriberHandler &subscriber_handler); - /** @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(); + + /** Publish tune control to sound alarm */ + void play_tune(); - static void cycle_trampoline(void *arg); - void cycle(); - static void pub_tune(); - static void stop_tune(); - static int reset_module(); + /** 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; }; diff --git a/src/modules/events/send_event.cpp b/src/modules/events/send_event.cpp index 3c90565e41..4a21d01307 100644 --- a/src/modules/events/send_event.cpp +++ b/src/modules/events/send_event.cpp @@ -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"); diff --git a/src/modules/events/send_event.h b/src/modules/events/send_event.h index 46d0f14ce8..5a0ffc5e00 100644 --- a/src/modules/events/send_event.h +++ b/src/modules/events/send_event.h @@ -35,6 +35,7 @@ #include "subscriber_handler.h" #include "status_display.h" +#include "rc_loss_alarm.h" #include #include @@ -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; }; diff --git a/src/modules/events/status_display.h b/src/modules/events/status_display.h index a0ed5160ce..2119ff6d89 100644 --- a/src/modules/events/status_display.h +++ b/src/modules/events/status_display.h @@ -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 */ @@ -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 = {}; diff --git a/src/modules/rc_loss_alarm/CMakeLists.txt b/src/modules/rc_loss_alarm/CMakeLists.txt deleted file mode 100644 index 28b1d806e3..0000000000 --- a/src/modules/rc_loss_alarm/CMakeLists.txt +++ /dev/null @@ -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 -) diff --git a/src/modules/rc_loss_alarm/rc_loss_alarm.cpp b/src/modules/rc_loss_alarm/rc_loss_alarm.cpp deleted file mode 100644 index 1ef26a3abd..0000000000 --- a/src/modules/rc_loss_alarm/rc_loss_alarm.cpp +++ /dev/null @@ -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 - -#include -#include - -#include - -// 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(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(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(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); -}