void resetMulticopterPositionController(void)
{
int axis;
for (axis = 0; axis < 2; axis++) {
navPidReset(&posControl.pids.vel[axis]);
posControl.rcAdjustment[axis] = 0;
filterResetPt1(&mcPosControllerAccFilterStateX, 0.0f);
filterResetPt1(&mcPosControllerAccFilterStateY, 0.0f);
lastAccelTargetX = 0.0f;
lastAccelTargetY = 0.0f;
}
}
void resetMulticopterAltitudeController()
{
navPidReset(&posControl.pids.vel[Z]);
filterResetPt1(&altholdThrottleFilterState, 0.0f);
posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z; // Gradually transition from current climb
posControl.rcAdjustment[THROTTLE] = 0;
}
void resetMulticopterAltitudeController(void)
{
navPidReset(&posControl.pids.vel[Z]);
navPidReset(&posControl.pids.surface);
filterResetPt1(&altholdThrottleFilterState, 0.0f);
posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z; // Gradually transition from current climb
posControl.rcAdjustment[THROTTLE] = 0;
/* Prevent jump if activated with zero throttle - start with -50% throttle adjustment. That's obviously too much, but it will prevent jumping */
if (prepareForTakeoffOnReset) {
posControl.pids.vel[Z].integrator = -500.0f;
prepareForTakeoffOnReset = false;
}
}
