Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-22 15:55:40 +03:00
Code Issues Wiki Activity

Adding loiter radius to programming

Browse source 
Adding a loiter radius override to the programming. This change allows pilots to change their loiter radius on the fly. The minimum loiter radius will always be that set in Advanced Tuning, for safety. The maximum radius is 100000cm.

As part of this change, I increased the min and max constraints of the basic maths conditions. The were set to int16, however operands can be int32. Before the change, this caused clipping of the radius when performing basic calculus on it.
This commit is contained in:
Darren Lines 2021-09-22 22:32:56 +01:00
parent b51cb7739a
commit b66caae07a
4 changed files with 40 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,6 +49,8 @@
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
#include "programming/logic_condition.h"
#include "rx/rx.h"
#include "sensors/battery.h"
@ -271,10 +273,16 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
uint32_t navLoiterRadius = navConfig()->fw.loiter_radius;
#ifdef USE_PROGRAMMING_FRAMEWORK
navLoiterRadius = getLoiterRadius(navConfig()->fw.loiter_radius);
#endif
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
@ -283,8 +291,8 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(loiterDirection() * 45.0f);
float loiterTargetX = posControl.desiredState.pos.x + navConfig()->fw.loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navConfig()->fw.loiter_radius * sin_approx(loiterAngle);
float loiterTargetX = posControl.desiredState.pos.x + navLoiterRadius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.y + navLoiterRadius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - navGetCurrentActualPositionAndVelocity()->pos.x;