From a2ba34d1aedfc84b8ece97ebdaa9d17781ad07aa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 27 Sep 2015 19:49:01 -0400 Subject: [PATCH] geofence violation actions --- msg/geofence_result.msg | 7 ++ src/modules/commander/commander.cpp | 146 ++++++++++++++++++----- src/modules/navigator/geofence.cpp | 28 ++--- src/modules/navigator/geofence.h | 14 ++- src/modules/navigator/geofence_params.c | 12 +- src/modules/navigator/navigator_main.cpp | 6 +- 6 files changed, 149 insertions(+), 64 deletions(-) diff --git a/msg/geofence_result.msg b/msg/geofence_result.msg index 7fc21c2af8..165d92b989 100644 --- a/msg/geofence_result.msg +++ b/msg/geofence_result.msg @@ -1 +1,8 @@ +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination + bool geofence_violated # true if the geofence is violated +uint8 geofence_action # action to take when geofence is violated \ No newline at end of file diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 62a187ff93..3395b8c069 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -195,13 +195,7 @@ static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; static unsigned _last_mission_instance = 0; -static uint64_t last_manual_input = 0; -static switch_pos_t last_offboard_switch = 0; -static switch_pos_t last_return_switch = 0; -static switch_pos_t last_mode_switch = 0; -static switch_pos_t last_acro_switch = 0; -static switch_pos_t last_posctl_switch = 0; -static switch_pos_t last_loiter_switch = 0; +static manual_control_setpoint_s _last_sp_man; struct vtol_vehicle_status_s vtol_status; @@ -922,6 +916,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_rc_in_off = param_find("COM_RC_IN_MODE"); param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); + param_t _param_geofence_action = param_find("GF_ACTION"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1260,6 +1255,8 @@ int commander_thread_main(int argc, char *argv[]) float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; + uint8_t geofence_action = 0; + /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; @@ -1346,6 +1343,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); + param_get(_param_geofence_action, &geofence_action); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1835,24 +1833,106 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result); } - /* Check for geofence violation */ - if (armed.armed && (geofence_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 */ + // Geofence actions + if (armed.armed && (geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE)) { + + static bool geofence_loiter_on = false; + static bool geofence_rtl_on = false; + + static uint8_t geofence_main_state_before_violation = vehicle_status_s::MAIN_STATE_MAX; + + // check for geofence violation + if (geofence_result.geofence_violated) { + static hrt_abstime last_geofence_violation = 0; + const hrt_abstime geofence_violation_action_interval = 10000000; // 10 seconds + if (hrt_elapsed_time(&last_geofence_violation) > geofence_violation_action_interval) { + + last_geofence_violation = hrt_absolute_time(); + + switch (geofence_result.geofence_action) { + case (geofence_result_s::GF_ACTION_NONE) : { + // do nothing + break; + } + case (geofence_result_s::GF_ACTION_WARN) : { + // do nothing, mavlink critical messages are sent by navigator + break; + } + case (geofence_result_s::GF_ACTION_LOITER) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_LOITER)) { + geofence_loiter_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_RTL) : { + if (TRANSITION_CHANGED == main_state_transition(&status, vehicle_status_s::MAIN_STATE_AUTO_RTL)) { + geofence_rtl_on = true; + } + break; + } + case (geofence_result_s::GF_ACTION_TERMINATE) : { + warnx("Flight termination because of geofence"); + mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + armed.force_failsafe = true; + status_changed = true; + break; + } + } + } + } + + // reset if no longer in LOITER or if manually switched to LOITER + geofence_loiter_on = geofence_loiter_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_LOITER) + && (sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + // reset if no longer in RTL or if manually switched to RTL + geofence_rtl_on = geofence_rtl_on + && (status.main_state == vehicle_status_s::MAIN_STATE_AUTO_RTL) + && (sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_OFF); + + if (!geofence_loiter_on && !geofence_rtl_on) { + // store the last good main_state when not in a geofence triggered action (LOITER or RTL) + geofence_main_state_before_violation = status.main_state; + } + + // revert geofence failsafe transition if sticks are moved and we were previously in MANUAL or ASSIST + if ((geofence_loiter_on || geofence_rtl_on) && + (geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_MANUAL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ALTCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_POSCTL || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_ACRO || + geofence_main_state_before_violation == vehicle_status_s::MAIN_STATE_STAB)) { + + // transition to previous state if sticks are increased + const float min_stick_change = 0.2; + if ((_last_sp_man.timestamp != sp_man.timestamp) && + ((fabsf(sp_man.x) - fabsf(_last_sp_man.x) > min_stick_change) || + (fabsf(sp_man.y) - fabsf(_last_sp_man.y) > min_stick_change) || + (fabsf(sp_man.z) - fabsf(_last_sp_man.z) > min_stick_change) || + (fabsf(sp_man.r) - fabsf(_last_sp_man.r) > min_stick_change))) { + + main_state_transition(&status, geofence_main_state_before_violation); + } + } + } + + + /* Check for mission flight termination */ + if (armed.armed && mission_result.flight_termination) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; if (!flight_termination_printed) { - warnx("Flight termination because of navigator request or geofence"); - mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination"); + warnx("Flight termination because of navigator request"); flight_termination_printed = true; } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { mavlink_log_critical(mavlink_fd, "Flight termination active"); } - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + } /* Only evaluate mission state if home is set, * this prevents false positives for the mission @@ -1937,10 +2017,15 @@ int commander_thread_main(int argc, char *argv[]) * for being in manual mode only applies to manual arming actions. * the system can be armed in auto if armed via the GCS. */ + if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) && (status.main_state != vehicle_status_s::MAIN_STATE_STAB)) { print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + } else if (!status.condition_home_position_valid && + geofence_action == geofence_result_s::GF_ACTION_RTL) { + print_reject_arm("NOT ARMING: Geofence RTL requires valid home"); + } else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -2107,7 +2192,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If we are not in a manual (RC stick controlled mode) + * If we are not in a manual (RC stick controlled mode) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && @@ -2422,31 +2507,32 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - /* if offboard is set allready by a mavlink command, abort */ + /* if offboard is set already by a mavlink command, abort */ if (status.offboard_control_set_by_command) { return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD); } /* manual setpoint has not updated, do not re-evaluate it */ - if ((last_manual_input == sp_man->timestamp) || - ((last_offboard_switch == sp_man->offboard_switch) && - (last_return_switch == sp_man->return_switch) && - (last_mode_switch == sp_man->mode_switch) && - (last_acro_switch == sp_man->acro_switch) && - (last_posctl_switch == sp_man->posctl_switch) && - (last_loiter_switch == sp_man->loiter_switch))) { + if ((_last_sp_man.timestamp == sp_man->timestamp) || + ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && + (_last_sp_man.return_switch == sp_man->return_switch) && + (_last_sp_man.mode_switch == sp_man->mode_switch) && + (_last_sp_man.acro_switch == sp_man->acro_switch) && + (_last_sp_man.posctl_switch == sp_man->posctl_switch) && + (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + + // update these fields for the geofence system + _last_sp_man.timestamp = sp_man->timestamp; + _last_sp_man.x = sp_man->x; + _last_sp_man.y = sp_man->y; + _last_sp_man.z = sp_man->z; + _last_sp_man.r = sp_man->r; /* no timestamp change or no switch change -> nothing changed */ return TRANSITION_NOT_CHANGED; } - last_manual_input = sp_man->timestamp; - last_offboard_switch = sp_man->offboard_switch; - last_return_switch = sp_man->return_switch; - last_mode_switch = sp_man->mode_switch; - last_acro_switch = sp_man->acro_switch; - last_posctl_switch = sp_man->posctl_switch; - last_loiter_switch = sp_man->loiter_switch; + _last_sp_man = *sp_man; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) { diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 7c6bb281ac..2baa0a7581 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -53,14 +53,8 @@ #include #include -#define GEOFENCE_OFF 0 -#define GEOFENCE_FILE_ONLY 1 -#define GEOFENCE_MAX_DISTANCES_ONLY 2 -#define GEOFENCE_FILE_AND_MAX_DISTANCES 3 - #define GEOFENCE_RANGE_WARNING_LIMIT 3000000 - /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR # undef ERROR @@ -76,8 +70,8 @@ Geofence::Geofence() : _last_vertical_range_warning(0), _altitude_min(0), _altitude_max(0), - _verticesCount(0), - _param_geofence_mode(this, "MODE"), + _vertices_count(0), + _param_action(this, "ACTION"), _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), @@ -138,7 +132,6 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, bool Geofence::inside(double lat, double lon, float altitude) { - if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) { int32_t max_horizontal_distance = _param_max_hor_distance.get(); int32_t max_vertical_distance = _param_max_ver_distance.get(); @@ -152,7 +145,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) { if (hrt_elapsed_time(&_last_vertical_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max vertical distance by %.1f m", (double)(dist_z - max_vertical_distance)); _last_vertical_range_warning = hrt_absolute_time(); } @@ -162,7 +155,7 @@ bool Geofence::inside(double lat, double lon, float altitude) if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) { if (hrt_elapsed_time(&_last_horizontal_range_warning) > GEOFENCE_RANGE_WARNING_LIMIT) { - mavlink_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", + mavlink_and_console_log_critical(_mavlinkFd, "Geofence exceeded max horizontal distance by %.1f m", (double)(dist_xy - max_horizontal_distance)); _last_horizontal_range_warning = hrt_absolute_time(); } @@ -171,7 +164,6 @@ bool Geofence::inside(double lat, double lon, float altitude) } } } - } bool inside_fence = inside_polygon(lat, lon, altitude); @@ -194,12 +186,6 @@ bool Geofence::inside(double lat, double lon, float altitude) bool Geofence::inside_polygon(double lat, double lon, float altitude) { - /* Return true if geofence is disabled or only checking max distances */ - if ((_param_geofence_mode.get() == GEOFENCE_OFF) - || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) { - return true; - } - if (valid()) { if (!isEmpty()) { @@ -219,7 +205,7 @@ bool Geofence::inside_polygon(double lat, double lon, float altitude) struct fence_vertex_s temp_vertex_j; /* Red until fence is finished */ - for (unsigned i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) { + for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) { if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { break; } @@ -259,7 +245,7 @@ Geofence::valid() } // Otherwise - if ((_verticesCount < 4) || (_verticesCount > fence_s::GEOFENCE_MAX_VERTICES)) { + if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) { warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1); return false; } @@ -415,7 +401,7 @@ Geofence::loadFromFile(const char *filename) /* Check if import was successful */ if (gotVertical && pointCounter > 0) { - _verticesCount = pointCounter; + _vertices_count = pointCounter; warnx("Geofence: imported successfully"); mavlink_log_info(_mavlinkFd, "Geofence imported"); rc = OK; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index c9518872f1..a19e13ca2f 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -97,12 +97,14 @@ public: int loadFromFile(const char *filename); - bool isEmpty() {return _verticesCount == 0;} + bool isEmpty() {return _vertices_count == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } int getSource() { return _param_source.get(); } + int getGeofenceAction() { return _param_action.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } private: @@ -114,20 +116,20 @@ private: hrt_abstime _last_horizontal_range_warning; hrt_abstime _last_vertical_range_warning; - float _altitude_min; - float _altitude_max; + float _altitude_min; + float _altitude_max; - unsigned _verticesCount; + uint8_t _vertices_count; /* Params */ - control::BlockParamInt _param_geofence_mode; + control::BlockParamInt _param_action; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; control::BlockParamInt _param_counter_threshold; control::BlockParamInt _param_max_hor_distance; control::BlockParamInt _param_max_ver_distance; - uint8_t _outside_counter; + uint8_t _outside_counter; int _mavlinkFd; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 63eec7213d..070fde6174 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -44,15 +44,15 @@ */ /** - * Geofence mode. + * Geofence violation action. * - * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both + * 0 = none, 1 = warning (default), 2 = loiter, 3 = return to launch, 4 = fight termination * * @min 0 - * @max 3 + * @max 4 * @group Geofence */ -PARAM_DEFINE_INT32(GF_MODE, 0); +PARAM_DEFINE_INT32(GF_ACTION, 1); /** * Geofence altitude mode @@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(GF_COUNT, -1); /** * Max horizontal distance in meters. * - * Set to > 0 to activate RTL if horizontal distance to home exceeds this value. + * Set to > 0 to activate a geofence action if horizontal distance to home exceeds this value. * * @group Geofence */ @@ -102,7 +102,7 @@ PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1); /** * Max vertical distance in meters. * - * Set to > 0 to activate RTL if vertical distance to home exceeds this value. + * Set to > 0 to activate a geofence action if vertical distance to home exceeds this value. * * @group Geofence */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bf8974cd8c..92f3e2fecb 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -405,10 +405,14 @@ Navigator::task_main() /* Check geofence violation */ static hrt_abstime last_geofence_check = 0; - if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { + if (have_geofence_position_data && + (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) && + (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter[0], _home_pos, _home_position_set); last_geofence_check = hrt_absolute_time(); have_geofence_position_data = false; + + _geofence_result.geofence_action = _geofence.getGeofenceAction(); if (!inside) { /* inform other apps via the mission result */ _geofence_result.geofence_violated = true;