forked from Archive/PX4-Autopilot
geofence violation actions
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
6fd1daf279
commit
a2ba34d1ae
|
|
@ -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
|
||||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -53,14 +53,8 @@
|
|||
#include <geo/geo.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue