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

Full/unfiltered throttle correction outside of pitch deadband

Browse source
This commit is contained in:
Airwide 2020-11-11 10:36:37 +01:00
parent 8b1f7e3b13
commit 2767297970
1 changed files with 5 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -463,22 +463,18 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
{
// Calculate base throttle correction from pitch moving average
const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth;
static int16_t averagePitch = 0;
averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles;
const int16_t baseThrottleCorrection = DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle;
// Calculate final throttle correction
if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh)) {
return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle;
}
else if (pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) {
return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch + navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle;
if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh) || pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) {
// Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
}
else {
return baseThrottleCorrection;
// Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle;
}
}