rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-04-12 04:02:20 +02:00
Code Issues Releases Activity

LP_616 safety check preventing possible execution of atan2f(0,0)

Browse Source
This commit is contained in:
julian_lilov 2021-10-14 17:14:46 +03:00
parent b2fb571999
commit 0ddce8ce96
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
flight/modules/PathPlanner/pathplanner.c
View File

@ -678,6 +678,11 @@ static uint8_t conditionPointingTowardsNext()
PositionStateData positionState; PositionStateData positionState;
PositionStateGet(&positionState); PositionStateGet(&positionState);
// check if current position exactly matches nextWaipoint (in 2D space)
if ((fabsf(nextWaypoint.Position.North - positionState.North) < 1e-6f) && (fabsf(nextWaypoint.Position.East - positionState.East) < 1e-6f)) {
return true;
}
float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East)); float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East));
VelocityStateData velocity; VelocityStateData velocity;