From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:45:15 +0200 Subject: move flight termination and geofence flags from setpoint triplet to mission result --- src/modules/commander/commander.cpp | 18 ++++++------- src/modules/navigator/datalinkloss.cpp | 31 ++++++++++++---------- src/modules/navigator/gpsfailure.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 16 +++++------ src/modules/navigator/rcloss.cpp | 3 ++- src/modules/uORB/topics/mission_result.h | 8 +++--- .../uORB/topics/position_setpoint_triplet.h | 2 -- 7 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ddfbd65a1..f05f970e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); - - /* Check for geofence violation */ - if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of navigator request or geofence"); - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { @@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + /* Check for geofence violation */ + if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of navigator request or geofence"); + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 4e3d25840..66f1c8c73 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item() } case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -180,23 +181,25 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (!_param_skipcommshold.get()) { - /* if number of data link losses limit is not reached fly to comms hold wp */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; - } else { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } - } else { - /* comms hold wp not active, fly to airfield home directly */ - warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index f0bbbcf1c..cd55f60b0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item() switch (_gpsf_state) { case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 81ceaaca2..c173ecd50 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -395,11 +395,9 @@ Navigator::task_main() if (have_geofence_position_data) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = true; - if (_pos_sp_triplet.geofence_violated != true) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = true; + publish_mission_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -407,11 +405,9 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = false; - if (_pos_sp_triplet.geofence_violated != false) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = false; + publish_mission_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 54f1813f5..651e31184 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -127,7 +127,8 @@ RCLoss::set_rcl_item() } case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 65ddfb4ad..c7d25d1f0 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -55,9 +55,11 @@ struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e2b1525a2..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,8 +97,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ - bool geofence_violated; /**< true if the geofence is violated */ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** -- cgit v1.2.3