diff --git a/src/main/config/config.c b/src/main/config/config.c index 9baa2b4d83..3afa8fa1a2 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -163,7 +163,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->I8[PIDALT] = 0; // not used pidProfile->D8[PIDALT] = 0; // not used pidProfile->P8[PIDPOS] = 15; // NAV_POS_XY_P * 100 - pidProfile->I8[PIDPOS] = 0; // not used + pidProfile->I8[PIDPOS] = 100; // posDecelerationTime * 100 pidProfile->D8[PIDPOS] = 0; // not used pidProfile->P8[PIDPOSR] = 90; // NAV_VEL_XY_P * 100 pidProfile->I8[PIDPOSR] = 15; // NAV_VEL_XY_I * 100 diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 65f9da6954..07aa4c8091 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -1909,7 +1909,10 @@ void navigationUsePIDs(pidProfile_t *initialPidProfile) posControl.pidProfile = initialPidProfile; - // Initialize position hold PI-controller + // Brake time parameter + posControl.posDecelerationTime = (float)posControl.pidProfile->I8[PIDPOS] / 100.0f; + + // Initialize position hold P-controller for (axis = 0; axis < 2; axis++) { navPInit(&posControl.pids.pos[axis], (float)posControl.pidProfile->P8[PIDPOS] / 100.0f); diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index 44e88ace8a..a193d399ef 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/src/main/flight/navigation_rewrite_multicopter.c @@ -474,8 +474,8 @@ void applyMulticopterEmergencyLandingController(uint32_t currentTime) *-----------------------------------------------------------*/ void calculateMulticopterInitialHoldPosition(t_fp_vector * pos) { - float stoppingDistanceX = posControl.actualState.vel.V.X / posControl.pids.pos[X].param.kP; - float stoppingDistanceY = posControl.actualState.vel.V.Y / posControl.pids.pos[Y].param.kP; + float stoppingDistanceX = posControl.actualState.vel.V.X * posControl.posDecelerationTime; + float stoppingDistanceY = posControl.actualState.vel.V.Y * posControl.posDecelerationTime; pos->V.X = posControl.actualState.pos.V.X + stoppingDistanceX; pos->V.Y = posControl.actualState.pos.V.Y + stoppingDistanceY; diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index 9e8f5b7db8..747f51e80f 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/src/main/flight/navigation_rewrite_private.h @@ -223,8 +223,9 @@ typedef struct { bool enabled; navigationFlags_t flags; - /* Navigation PID controllers */ + /* Navigation PID controllers + pre-computed flight parameters */ navigationPIDControllers_t pids; + float posDecelerationTime; /* Local system state, both actual (estimated) and desired (target setpoint)*/ navigationEstimatedState_t actualState;