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

SURFACE: Different gains, proper climb rate calculation

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2016-04-04 18:04:28 +10:00
parent 5e45c692f7
commit f9a6d4394c
2 changed files with 5 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/flight/navigation_rewrite.c
View file

@ -1616,7 +1616,7 @@ void updateAltitudeTargetFromClimbRate(float climbRate, navUpdateAltitudeFromRat
else {
if (posControl.flags.isTerrainFollowEnabled) {
if (posControl.actualState.surface >= 0.0f && posControl.flags.hasValidSurfaceSensor && (mode == CLIMB_RATE_UPDATE_SURFACE_TARGET)) {
posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / posControl.pids.pos[Z].param.kP), 1.0f, INAV_SURFACE_MAX_DISTANCE);
posControl.desiredState.surface = constrainf(posControl.actualState.surface + (climbRate / (posControl.pids.pos[Z].param.kP * posControl.pids.surface.param.kP)), 1.0f, INAV_SURFACE_MAX_DISTANCE);
}
}
else {
@ -2180,8 +2180,8 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile)
(float)posControl.pidProfile->D8[PIDVEL] / 100.0f);
// Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 1.0f,
0.25f,
navPidInit(&posControl.pids.surface, 2.0f,
1.0f,
0.0f);
// Initialize fixed wing PID controllers

3
src/main/flight/navigation_rewrite_multicopter.c
View file

@ -68,7 +68,8 @@ static void updateSurfaceTrackingAltitudeSetpoint(uint32_t deltaMicros)
/* If we have a surface offset target and a valid surface offset reading - recalculate altitude target */
if (posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface >= 0) {
if (posControl.actualState.surface >= 0 && posControl.flags.hasValidSurfaceSensor) {
float targetAltitudeError = navPidApply2(posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), &posControl.pids.surface, -25.0f, +25.0f, false);
// We better overshoot a little bit than undershoot
float targetAltitudeError = navPidApply2(posControl.desiredState.surface, posControl.actualState.surface, US2S(deltaMicros), &posControl.pids.surface, -5.0f, +35.0f, false);
posControl.desiredState.pos.V.Z = posControl.actualState.pos.V.Z + targetAltitudeError;
}
else {