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

Change FW altitude controller to operate energy balance instead of raw altitude

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-08-06 20:13:13 +10:00
parent f5a394d917
commit cda5940838
3 changed files with 22 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/flight/imu.h
View file

@ -22,6 +22,7 @@
#include "config/parameter_group.h" #include "config/parameter_group.h"
#define GRAVITY_CMSS 980.665f #define GRAVITY_CMSS 980.665f
#define GRAVITY_MSS 9.80665f
extern int16_t throttleAngleCorrection; extern int16_t throttleAngleCorrection;
extern int16_t smallAngle; extern int16_t smallAngle;

6
src/main/navigation/navigation.c
View file

@ -2605,9 +2605,9 @@ void navigationUsePIDs(void)
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f); (float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f, navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f, (float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f); (float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 9.80665f);
} }
void navigationInit(void) void navigationInit(void)

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

@ -103,11 +103,25 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle // velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Update energies
const float demSPE = (posControl.desiredState.pos.V.Z / 100.0f) * GRAVITY_MSS;
const float demSKE = 0.0f;
const float estSPE = (posControl.actualState.pos.V.Z / 100.0f) * GRAVITY_MSS;
const float estSKE = 0.0f;
const float speedWeight = 0.0f; // no speed sensing for now
const float wSKE = speedWeight;
const float wSPE = 1.0f - speedWeight;
const float demSEB = demSPE * wSPE - demSKE * wSKE;
const float estSEB = estSPE * wSPE - estSKE * wSKE;
// Here we use negative values for dive for better clarity // Here we use negative values for dive for better clarity
int maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle); int maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
int minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle); int minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0); float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0);
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros)); targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
// Calculate climb angle ( >0 - climb, <0 - dive) // Calculate climb angle ( >0 - climb, <0 - dive)
@ -115,7 +129,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
posControl.rcAdjustment[PITCH] = targetPitchAngle; posControl.rcAdjustment[PITCH] = targetPitchAngle;
} }
void applyFixedWingAltitudeController(timeUs_t currentTimeUs) void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
{ {
static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ) static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
@ -530,7 +544,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
// Don't apply anything if ground speed is too low (<3m/s) // Don't apply anything if ground speed is too low (<3m/s)
if (posControl.actualState.velXY > 300) { if (posControl.actualState.velXY > 300) {
if (navStateFlags & NAV_CTL_ALT) if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeController(currentTimeUs); applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (navStateFlags & NAV_CTL_POS) if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs); applyFixedWingPositionController(currentTimeUs);
@ -541,7 +555,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
} }
#else #else
if (navStateFlags & NAV_CTL_ALT) if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeController(currentTimeUs); applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (navStateFlags & NAV_CTL_POS) if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs); applyFixedWingPositionController(currentTimeUs);