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

Initial cut for NAV yaw adjustments with separate PID controller on FW

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-12-06 14:37:50 +01:00
parent 412a5b456a
commit e7387df0f8
6 changed files with 56 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -61,6 +61,7 @@
static bool isPitchAdjustmentValid = false;
static bool isRollAdjustmentValid = false;
static bool isYawAdjustmentValid = false;
static float throttleSpeedAdjustment = 0;
static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
@ -210,7 +211,9 @@ void resetFixedWingPositionController(void)
navPidReset(&posControl.pids.fw_nav);
posControl.rcAdjustment[ROLL] = 0;
posControl.rcAdjustment[YAW] = 0;
isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
}
@ -333,6 +336,17 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
/*
* Yaw adjustment
* It is working in relative mode and we aim to keep error at zero
*/
float yawAdjustment = navPidApply2(&posControl.pids.fw_heading, 0, navHeadingError, US2S(deltaMicros),
-500,
500,
pidFlags);
posControl.rcAdjustment[YAW] = yawAdjustment;
}
void applyFixedWingPositionController(timeUs_t currentTimeUs)
@ -376,10 +390,12 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
}
isRollAdjustmentValid = true;
isYawAdjustmentValid = true;
}
else {
// No valid pos sensor data, don't adjust pitch automatically, rcCommand[ROLL] is passed through to PID controller
isRollAdjustmentValid = false;
isYawAdjustmentValid = false;
}
}
@ -448,6 +464,10 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]);
}
if (isYawAdjustmentValid && (navStateFlags & NAV_CTL_POS)) {
rcCommand[YAW] = posControl.rcAdjustment[YAW];
}
if (isPitchAdjustmentValid && (navStateFlags & NAV_CTL_ALT)) {
// PITCH >0 dive, <0 climb
int16_t pitchCorrection = constrain(posControl.rcAdjustment[PITCH], -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle), DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle));