fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

geofence: reposition to current location when violating geofence in hold mode

Browse Source 
This can only happen if the vehicle is executing a go to.
This commit is contained in:
daniele 2020-03-25 17:19:18 +01:00 committed by Daniel Agar
parent d53dccc319
commit 6ea60b5165
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/navigator/navigator_main.cpp
View File

@ -487,6 +487,22 @@ Navigator::run()
/* Issue a warning about the geofence violation once */
if (!_geofence_violation_warning_sent) {
mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
/* If we are already in loiter it is very likely that we are doing a reposition
* so we should block that by repositioning in the current location */
if (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN
&& get_vstatus()->nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.yaw = get_global_position()->yaw;
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
rep->current.alt = get_global_position()->alt;
rep->current.valid = true;
_pos_sp_triplet_updated = true;
}
_geofence_violation_warning_sent = true;
}