mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 08:15:26 +03:00
fixedWingPitchToThrottleCorrection definition bugfix
This commit is contained in:
parent
cc47513004
commit
dab9f95c69
3 changed files with 4 additions and 4 deletions
|
@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
|
|||
// pitch in degrees
|
||||
// output in Watt
|
||||
static float estimatePitchPower(float pitch) {
|
||||
int16_t altitudeChangeThrottle = fixedWingPitchToThrottleCorrection(DEGREES_TO_DECIDEGREES(pitch));
|
||||
int16_t altitudeChangeThrottle = (int16_t)pitch * navConfig()->fw.pitch_to_throttle;
|
||||
altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
|
||||
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;
|
||||
|
|
|
@ -463,7 +463,7 @@ bool loadNonVolatileWaypointList(void);
|
|||
bool saveNonVolatileWaypointList(void);
|
||||
|
||||
float getFinalRTHAltitude(void);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
|
||||
|
||||
/* Geodetic functions */
|
||||
typedef enum {
|
||||
|
|
|
@ -479,9 +479,9 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
|
|||
static pt1Filter_t pitchToThrFilterState;
|
||||
|
||||
// Apply low-pass filter to pitch angle to smooth throttle correction
|
||||
filteredPitch = pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
|
||||
int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
|
||||
|
||||
if (abs(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
|
||||
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
|
||||
// Unfiltered throttle correction outside of pitch deadband
|
||||
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue