Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-21 23:35:30 +03:00
Code Issues Wiki Activity

[maths.c] Add two news functions

Browse source
This commit is contained in:
JuliooCesarMDM 2022-01-11 21:39:45 -03:00
parent ac6650e25f
commit 930c6cf859
16 changed files with 41 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/navigation/navigation_fixedwing.c
View file

@ -266,7 +266,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
float posErrorY = posControl.desiredState.pos.y - navGetCurrentActualPositionAndVelocity()->pos.y;
float distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
float distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
@ -290,7 +290,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;
posErrorY = loiterTargetY - navGetCurrentActualPositionAndVelocity()->pos.y;
distanceToActualTarget = fast_fsqrtf(sq(posErrorX) + sq(posErrorY));
distanceToActualTarget = calc_length_pythagorean_2D(posErrorX, posErrorY);
}
// Calculate virtual waypoint