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

fixedWingPitchToThrottleCorrection definition bugfix

Browse source
This commit is contained in:
Airwide 2020-11-12 03:02:03 +01:00
parent cc47513004
commit dab9f95c69
3 changed files with 4 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/flight/rth_estimator.c
View file

@ -77,7 +77,7 @@ static int8_t RTHInitialAltitudeChangePitchAngle(float altitudeChange) {
// pitch in degrees // pitch in degrees
// output in Watt // output in Watt
static float estimatePitchPower(float pitch) { 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); altitudeChangeThrottle = constrain(altitudeChangeThrottle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue()); const float altitudeChangeThrToCruiseThrRatio = (float)(altitudeChangeThrottle - getThrottleIdleValue()) / (navConfig()->fw.cruise_throttle - getThrottleIdleValue());
return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100; return (float)heatLossesCompensatedPower(batteryMetersConfig()->idle_power + batteryMetersConfig()->cruise_power * altitudeChangeThrToCruiseThrRatio) / 100;

2
src/main/navigation/navigation.h
View file

@ -463,7 +463,7 @@ bool loadNonVolatileWaypointList(void);
bool saveNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void);
float getFinalRTHAltitude(void); float getFinalRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch); int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
/* Geodetic functions */ /* Geodetic functions */
typedef enum { typedef enum {

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

@ -479,9 +479,9 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
static pt1Filter_t pitchToThrFilterState; static pt1Filter_t pitchToThrFilterState;
// Apply low-pass filter to pitch angle to smooth throttle correction // 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 // Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle; return DECIDEGREES_TO_DEGREES(pitch) * navConfig()->fw.pitch_to_throttle;
} }