fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence violation actions

Browse Source
This commit is contained in:
Daniel Agar
2015-09-27 19:49:01 -04:00
committed by Lorenz Meier
parent 6fd1daf279
commit a2ba34d1ae
6 changed files with 149 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+116 -30
View File
@@ -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) {