Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

feat(navigator): respect geofence in course mode

Browse Source
This commit is contained in:
mahima-yoga
2026-05-18 11:07:07 +02:00
parent 54241d9bf6
commit bc459a8814
1 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+16 -2
View File
@@ -433,9 +433,23 @@ void Navigator::run()
// which can lead to dangerous and unexpected behaviors (see loiter.cpp, there is an if(armed) in there too)
if (_navigation_mode == &_course) {
// In course mode, update altitude directly
// In course mode, update altitude directly (after geofence check)
float new_alt = PX4_ISFINITE(cmd.param1) ? cmd.param1 : get_global_position()->alt;
_course.set_altitude(new_alt);
vehicle_global_position_s position_setpoint{};
position_setpoint.lat = get_global_position()->lat;
position_setpoint.lon = get_global_position()->lon;
position_setpoint.alt = new_alt;
if (geofence_allows_position(position_setpoint)) {
_course.set_altitude(new_alt);
} else {
mavlink_log_critical(&_mavlink_log_pub, "Altitude change is outside geofence\t");
events::send(events::ID("navigator_course_change_altitude_outside_geofence"), {events::Log::Error, events::LogInternal::Info},
"Altitude change is outside geofence");
}
// DO_CHANGE_ALTITUDE is acknowledged by commander
} else {