Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Added baseThrottleCorrection calculated from moving average pitch

Browse source
This commit is contained in:
Airwide 2020-09-27 23:20:42 +02:00
parent 517fcd0bf6
commit d6d6127310
1 changed files with 19 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -463,17 +463,20 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch)
{
if (pitch > navConfig()->fw.pitch_to_throttle_thresh) {
// Positive pitch above threshold
return DECIDEGREES_TO_DEGREES(pitch - navConfig()->fw.pitch_to_throttle_thresh) * navConfig()->fw.pitch_to_throttle;
// Calculate base throttle correction from pitch moving average
const int16_t movingAverageCycles = 128; //Number of main loop cycles for average calculation
static int16_t 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 < -navConfig()->fw.pitch_to_throttle_thresh) {
// Negative pitch below threshold
return DECIDEGREES_TO_DEGREES(pitch + 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;
}
else {
// Inside pitch_to_throttle_thresh deadband. Make no throttle correction.
return 0;
return baseThrottleCorrection;
}
}