Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 08:45:31 +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) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
{ {
// Calculate base throttle correction from pitch moving average
const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth; const int16_t movingAverageCycles = navConfig()->fw.pitch_to_throttle_smooth;
static int16_t averagePitch = 0; static int16_t averagePitch = 0;
averagePitch = (averagePitch * movingAverageCycles + pitch - averagePitch) / movingAverageCycles; 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) || pitch < (averagePitch - navConfig()->fw.pitch_to_throttle_thresh)) {
if (pitch > (averagePitch + navConfig()->fw.pitch_to_throttle_thresh)) { // Unfiltered throttle correction outside of pitch deadband
return baseThrottleCorrection + DECIDEGREES_TO_DEGREES(pitch - averagePitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(pitch) * 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;
} }
else { else {
return baseThrottleCorrection; // Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(averagePitch) * navConfig()->fw.pitch_to_throttle;
} }
} }