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

reposition (goto): add check if goto location is outside the geofence

Browse Source 
Before this check we were free to fly outside of a geofence with a goto
command. With this check we ignore the command if the location of the goto
is outside the fence. If this is the case we send back a mavlink warning to
the ground station.
This commit is contained in:
daniele 2020-03-25 16:45:13 +01:00 committed by Daniel Agar
parent 6b97361c92
commit d53dccc319
1 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -228,7 +228,31 @@ Navigator::run()
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {
if (true) {
bool reposition_valid = true;
if (have_geofence_position_data &&
((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
(_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_WARN))) {
if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
vehicle_global_position_s test_reposition_validity {};
test_reposition_validity.lat = cmd.param5;
test_reposition_validity.lon = cmd.param6;
if (PX4_ISFINITE(cmd.param7)) {
test_reposition_validity.alt = cmd.param7;
} else {
test_reposition_validity.alt = get_global_position()->alt;
}
reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos,
home_position_valid());
}
}
if (reposition_valid) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
@ -299,6 +323,9 @@ Navigator::run()
rep->current.timestamp = hrt_absolute_time();
rep->next.valid = false;
} else {
mavlink_log_critical(&_mavlink_log_pub, "Reposition is outside geofence");
}
// CMD_DO_REPOSITION is acknowledged by commander