Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-18 05:45:28 +03:00
Code Issues Wiki Activity

Loiter direction (#4036)

Browse source 
* Loiter direction
https://github.com/iNavFlight/inav/issues/3002
This commit is contained in:
hali9 2019-01-08 08:26:06 +01:00 • committed by Konstantin Sharlaimov
parent 61654d8ce8
commit 69970a2b95
8 changed files with 40 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -43,6 +43,7 @@
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "navigation/navigation.h"
@ -63,6 +64,7 @@ static bool isRollAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static int8_t loiterDirYaw = 1;
/*-----------------------------------------------------------
@ -213,6 +215,18 @@ void resetFixedWingPositionController(void)
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
}
static int8_t loiterDirection(void) {
int8_t dir = 1; //NAV_LOITER_RIGHT
if (pidProfile()->loiter_direction == NAV_LOITER_LEFT) dir = -1;
if (pidProfile()->loiter_direction == NAV_LOITER_YAW) {
if (rcCommand[YAW] < -250) loiterDirYaw = 1; //RIGHT //yaw is contrariwise
if (rcCommand[YAW] > 250) loiterDirYaw = -1; //LEFT //see annexCode in fc_core.c
dir = loiterDirYaw;
}
if (IS_RC_MODE_ACTIVE(BOXLOITERDIRCHN)) dir *= -1;
return dir;
}
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
float posErrorX = posControl.desiredState.pos.x - navGetCurrentActualPositionAndVelocity()->pos.x;
@ -233,7 +247,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
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);
@ -292,7 +306,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// If forced turn direction flag is enabled we fix the sign of the direction
if (forceTurnDirection) {
navHeadingError = ABS(navHeadingError);
navHeadingError = loiterDirection() * ABS(navHeadingError);
}
// Slow error monitoring (2Hz rate)
@ -308,7 +322,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// Only allow PID integrator to shrink if error is decreasing over time
const pidControllerFlags_e pidFlags = PID_DTERM_FROM_ERROR | (errorIsDecreasing ? PID_SHRINK_INTEGRATOR : 0);
// Input error in (deg*100), output pitch angle (deg*100)
// Input error in (deg*100), output roll angle (deg*100)
float rollAdjustment = navPidApply2(&posControl.pids.fw_nav, posControl.actualState.yaw + navHeadingError, posControl.actualState.yaw, US2S(deltaMicros),
-DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),