From 84e11a0cac53f753f65b0bea4659e1f2d9c0b35e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 5 Sep 2012 11:37:17 +0200 Subject: Fixed correct RC loss detection, AR.Drone is now shutting down motors after 1 s of RC loss. Added debug topic. --- apps/commander/commander.c | 10 ++++++++++ apps/commander/state_machine_helper.c | 13 +++++++++---- apps/commander/state_machine_helper.h | 6 ++++++ 3 files changed, 25 insertions(+), 4 deletions(-) (limited to 'apps/commander') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 80d2c58f8..4135abf60 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -1174,6 +1175,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* Start RC state check */ + bool prev_lost = current_status.rc_signal_lost; if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { @@ -1238,10 +1240,18 @@ int commander_thread_main(int argc, char *argv[]) /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ current_status.rc_signal_cutting_off = true; current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp; + /* if the RC signal is gone for a full second, consider it lost */ if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true; } + /* Check if this is the first loss or first gain*/ + if ((!prev_lost && current_status.rc_signal_lost) || + prev_lost && !current_status.rc_signal_lost) { + /* publish rc lost */ + publish_armed_status(¤t_status); + } + /* End mode switch */ /* END RC state check */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 4e2166a3a..51ed95114 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + publish_armed_status(current_status); ret = OK; } if (invalid_state) { @@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); } +void publish_armed_status(const struct vehicle_status_s *current_status) { + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + armed.failsafe = current_status->rc_signal_lost; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +} + /* * Private functions, update the state machine diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 4908c799a..c4d1b78a5 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat */ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +/** + * Publish the armed state depending on the current system state + * + * @param current_status the current system status + */ +void publish_armed_status(const struct vehicle_status_s *current_status); -- cgit v1.2.3