dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FlightTaskOrbit: don't start Orbit if radius is not in range

This commit is contained in:
Matthias Grob 2022-03-17 14:55:29 +01:00
parent 0595efbd9b
commit d0794c1189
1 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
View File

@ -69,10 +69,16 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
}
float new_velocity = signFromBool(new_is_clockwise) * new_absolute_velocity;
_started_clockwise = new_is_clockwise;
_sanitizeParams(new_radius, new_velocity);
_orbit_radius = new_radius;
_orbit_velocity = new_velocity;
if (math::isInRange(new_radius, _radius_min, _radius_max)) {
_started_clockwise = new_is_clockwise;
_sanitizeParams(new_radius, new_velocity);
_orbit_radius = new_radius;
_orbit_velocity = new_velocity;
} else {
ret = false;
}
// commanded heading behaviour
if (PX4_ISFINITE(command.param3)) {