bool adjustMulticopterAltitudeFromRCInput(void)
{
int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altHoldThrottleRCZero;
if (ABS(rcThrottleAdjustment) > posControl.rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement
float rcClimbRate;
// Make sure we can satisfy max_manual_climb_rate in both up and down directions
if (rcThrottleAdjustment > 0) {
// Scaling from altHoldThrottleRCZero to maxthrottle
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.escAndServoConfig->maxthrottle - altHoldThrottleRCZero);
}
else {
// Scaling from minthrottle to altHoldThrottleRCZero
rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle);
}
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_UPDATE_SURFACE_TARGET);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateAltitudeTargetFromClimbRate(0, CLIMB_RATE_UPDATE_SURFACE_TARGET);
}
return false;
}
}
/*-----------------------------------------------------------
* 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;
}
}
}
bool adjustMulticopterAltitudeFromRCInput(void)
{
int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altholdInitialRCThrottle;
if (ABS(rcThrottleAdjustment) > posControl.rcControlsConfig->alt_hold_deadband) {
// set velocity proportional to stick movement
float rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / 500;
updateAltitudeTargetFromClimbRate(rcClimbRate);
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingAltitude) {
updateAltitudeTargetFromClimbRate(0);
}
return false;
}
}
bool adjustFixedWingAltitudeFromRCInput(void)
{
int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], posControl.rcControlsConfig->alt_hold_deadband);
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * posControl.navConfig->max_manual_climb_rate / (500.0f - posControl.rcControlsConfig->alt_hold_deadband);
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET);
return true;
}
else {
return false;
}
}
/* Calculate global altitude setpoint based on surface setpoint */
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) {
// 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 {
// TODO: We are possible above valid range, we now descend down to attempt to get back within range
//updateAltitudeTargetFromClimbRate(-0.10f * posControl.navConfig->emerg_descent_rate, CLIMB_RATE_KEEP_SURFACE_TARGET);
updateAltitudeTargetFromClimbRate(-20.0f, CLIMB_RATE_KEEP_SURFACE_TARGET);
}
}
#if defined(NAV_BLACKBOX)
navTargetPosition[Z] = constrain(lrintf(posControl.desiredState.pos.V.Z), -32678, 32767);
#endif
}
