/*-----------------------------------------------------------
* Multicopter emergency landing
*-----------------------------------------------------------*/
static void applyMulticopterEmergencyLandingController(uint32_t currentTime)
{
static uint32_t previousTimeUpdate;
static uint32_t previousTimePositionUpdate;
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
/* Attempt to stabilise */
rcCommand[ROLL] = 0;
rcCommand[PITCH] = 0;
rcCommand[YAW] = 0;
if (posControl.flags.hasValidAltitudeSensor) {
/* We have an altitude reference, apply AH-based landing controller */
// 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 = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterAltitudeController();
return;
}
if (posControl.flags.verticalPositionDataNew) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateAltitudeTargetFromClimbRate(-1.0f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_RESET_SURFACE_TARGET);
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
resetMulticopterAltitudeController();
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionDataConsumed = 1;
}
// Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
}
else {
/* Sensors has gone haywire, attempt to land regardless */
failsafeConfig_t * failsafeConfig = getActiveFailsafeConfig();
if (failsafeConfig) {
rcCommand[THROTTLE] = failsafeConfig->failsafe_throttle;
}
else {
rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
}
}
}
static void applyMulticopterAltitudeController(uint32_t currentTime)
{
static uint32_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static uint32_t previousTimeUpdate; // Occurs @ looptime rate
uint32_t deltaMicros = currentTime - previousTimeUpdate;
previousTimeUpdate = currentTime;
// 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 = currentTime;
previousTimePositionUpdate = currentTime;
resetMulticopterAltitudeController();
return;
}
// If we have an update on vertical position data - update velocity and accel targets
if (posControl.flags.verticalPositionNewData) {
uint32_t deltaMicrosPositionUpdate = currentTime - previousTimePositionUpdate;
previousTimePositionUpdate = currentTime;
// Check if last correction was too log ago - ignore this update
if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
updateSurfaceTrackingAltitudeSetpoint_MC();
updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate);
updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate);
}
else {
// due to some glitch position update has not occurred in time, reset altitude controller
resetMulticopterAltitudeController();
}
// Indicate that information is no longer usable
posControl.flags.verticalPositionNewData = 0;
}
// Update throttle controller
rcCommand[THROTTLE] = constrain((int16_t)posControl.navConfig->mc_hover_throttle + posControl.rcAdjustment[THROTTLE], posControl.escAndServoConfig->minthrottle, posControl.escAndServoConfig->maxthrottle);
// Save processed throttle for future use
rcCommandAdjustedThrottle = rcCommand[THROTTLE];
}
