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

Refactor code to use common function that take a base frequency

Browse source
This commit is contained in:
Harry Kalogirou 2020-05-01 14:56:41 +03:00
parent 52daaa789e
commit ac39fe1cc7
1 changed files with 15 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -51,6 +51,9 @@
#include "rx/rx.h"
// Base frequencies for smoothing pitch and roll
#define NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ 2.0f
#define NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ 10.0f
// If we are going slower than NAV_FW_MIN_VEL_SPEED_BOOST - boost throttle to fight against the wind
#define NAV_FW_THROTTLE_SPEED_BOOST_GAIN 1.5f
@ -67,6 +70,14 @@ static bool isAutoThrottleManuallyIncreased = false;
static int32_t navHeadingError;
static int8_t loiterDirYaw = 1;
// Calculates the cutoff frequency for smoothing out roll/pitch commands
// control_smoothness valid range from 0 to 9
// resulting cutoff_freq ranging from baseFreq downwards to ~0.11Hz
static float getSmoothnessCutoffFreq(float baseFreq)
{
uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
return 0.001f * baseFreq * (float)(smoothness*smoothness*smoothness) + 0.1f;
}
/*-----------------------------------------------------------
* Altitude controller
@ -137,11 +148,8 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
// Apply low-pass filter to prevent rapid correction cutoff_freq = 0.002 * (10.0-x)^3 + epsilon
// Pitch cutoff set to 1/5 the roll cutoff
uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
float cutoff_freq = 0.002f * (float)(smoothness*smoothness*smoothness) + 0.1f;
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, cutoff_freq, US2S(deltaMicros));
// Apply low-pass filter to prevent rapid correction
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
// Reconstrain pitch angle ( >0 - climb, <0 - dive)
targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@ -371,10 +379,8 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
DEGREES_TO_CENTIDEGREES(navConfig()->fw.max_bank_angle),
pidFlags);
// Apply low-pass filter to prevent rapid correction cutoff_freq = 0.01 * (10.0-x)^3 + epsilon
uint16_t smoothness = 10 - navConfig()->fw.control_smoothness;
float cutoff_freq = 0.01f * (float)(smoothness*smoothness*smoothness) + 0.1f;
rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, cutoff_freq, US2S(deltaMicros));
// Apply low-pass filter to prevent rapid correction
rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);