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

[NAV] Cleanup pos/vel controllers

Browse source
This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2020-07-23 13:19:04 +02:00
parent f83b54beaa
commit d2f05b7e23
2 changed files with 51 additions and 115 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -158,19 +158,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
{
static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last time Z-controller was called is too far in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
resetFixedWingAltitudeController();
return;
}
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
if (posControl.flags.verticalPositionDataNew) {
@ -182,7 +170,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
resetFixedWingAltitudeController();
}
@ -398,19 +386,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
void applyFixedWingPositionController(timeUs_t currentTimeUs)
{
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
resetFixedWingPositionController();
return;
}
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
@ -429,6 +405,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
updatePositionHeadingController_FW(currentTimeUs, deltaMicrosPositionUpdate);
}
else {
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
resetFixedWingPositionController();
}
@ -448,19 +425,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
{
static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
previousTimeUpdate = currentTimeUs;
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
previousTimeUpdate = currentTimeUs;
previousTimePositionUpdate = currentTimeUs;
throttleSpeedAdjustment = 0;
return 0;
}
static timeUs_t previousTimePositionUpdate = 0; // Occurs @ GPS update rate
// Apply controller only if position source is valid
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
@ -480,6 +445,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
throttleSpeedAdjustment = constrainf(throttleSpeedAdjustment, 0.0f, 500.0f);
}
else {
// Position update has not occurred in time (first iteration or glitch), reset altitude controller
throttleSpeedAdjustment = 0;
}