diff --git a/src/modules/rc_loss_alarm/rc_loss_alarm.cpp b/src/modules/rc_loss_alarm/rc_loss_alarm.cpp index 4bfa4e56bb..ef8cfd7c9e 100644 --- a/src/modules/rc_loss_alarm/rc_loss_alarm.cpp +++ b/src/modules/rc_loss_alarm/rc_loss_alarm.cpp @@ -45,6 +45,9 @@ // Init static members work_s RC_Loss_Alarm::_work = {}; +bool RC_Loss_Alarm::_was_armed = false; +orb_advert_t RC_Loss_Alarm::_tune_control_pub = nullptr; + RC_Loss_Alarm::RC_Loss_Alarm() { @@ -99,6 +102,14 @@ int RC_Loss_Alarm::task_spawn(int argc, char *argv[]) /** @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"); } @@ -123,6 +134,8 @@ rc_loss_alarm start 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; } @@ -147,6 +160,22 @@ void RC_Loss_Alarm::pub_tune() } } +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); + } +} + void RC_Loss_Alarm::cycle() { // Subscribe if necessary @@ -181,6 +210,12 @@ void RC_Loss_Alarm::cycle() } } +int RC_Loss_Alarm::reset_module(){ + RC_Loss_Alarm::_was_armed = 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[]) diff --git a/src/modules/rc_loss_alarm/rc_loss_alarm.h b/src/modules/rc_loss_alarm/rc_loss_alarm.h index 84eaf371ee..496e64bc99 100644 --- a/src/modules/rc_loss_alarm/rc_loss_alarm.h +++ b/src/modules/rc_loss_alarm/rc_loss_alarm.h @@ -62,10 +62,15 @@ private: static struct work_s _work; int _vehicle_status_sub = -1; struct vehicle_status_s _vehicle_status = {}; - orb_advert_t _tune_control_pub = nullptr; - bool _was_armed = false; + static orb_advert_t _tune_control_pub; + static bool _was_armed; static void cycle_trampoline(void *arg); void cycle(); - void pub_tune(); + static void pub_tune(); + static void stop_tune(); + static int reset_module(); + + // Do not allow class copies + RC_Loss_Alarm(const RC_Loss_Alarm &other); };