static void updatePositionAccelController_MC(uint32_t deltaMicros, float maxAccelLimit)
{
float velErrorX, velErrorY, newAccelX, newAccelY;
// Calculate velocity error
velErrorX = posControl.desiredState.vel.V.X - posControl.actualState.vel.V.X;
velErrorY = posControl.desiredState.vel.V.Y - posControl.actualState.vel.V.Y;
// Calculate XY-acceleration limit according to velocity error limit
float accelLimitX, accelLimitY;
float velErrorMagnitude = sqrtf(sq(velErrorX) + sq(velErrorY));
if (velErrorMagnitude > 0.1f) {
accelLimitX = maxAccelLimit / velErrorMagnitude * fabsf(velErrorX);
accelLimitY = maxAccelLimit / velErrorMagnitude * fabsf(velErrorY);
}
else {
accelLimitX = maxAccelLimit / 1.414213f;
accelLimitY = accelLimitX;
}
// Apply additional jerk limiting of 1700 cm/s^3 (~100 deg/s), almost any copter should be able to achieve this rate
// This will assure that we wont't saturate out LEVEL and RATE PID controller
float maxAccelChange = US2S(deltaMicros) * 1700.0f;
float accelLimitXMin = constrainf(lastAccelTargetX - maxAccelChange, -accelLimitX, +accelLimitX);
float accelLimitXMax = constrainf(lastAccelTargetX + maxAccelChange, -accelLimitX, +accelLimitX);
float accelLimitYMin = constrainf(lastAccelTargetY - maxAccelChange, -accelLimitY, +accelLimitY);
float accelLimitYMax = constrainf(lastAccelTargetY + maxAccelChange, -accelLimitY, +accelLimitY);
// TODO: Verify if we need jerk limiting after all
// Apply PID with output limiting and I-term anti-windup
// Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit
// Thus we don't need to do anything else with calculated acceleration
newAccelX = navPidApply2(posControl.desiredState.vel.V.X, posControl.actualState.vel.V.X, US2S(deltaMicros), &posControl.pids.vel[X], accelLimitXMin, accelLimitXMax, false);
newAccelY = navPidApply2(posControl.desiredState.vel.V.Y, posControl.actualState.vel.V.Y, US2S(deltaMicros), &posControl.pids.vel[Y], accelLimitYMin, accelLimitYMax, false);
// Save last acceleration target
lastAccelTargetX = newAccelX;
lastAccelTargetY = newAccelY;
// Apply LPF to jerk limited acceleration target
float accelN = filterApplyPt1(newAccelX, &mcPosControllerAccFilterStateX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
float accelE = filterApplyPt1(newAccelY, &mcPosControllerAccFilterStateY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
// Rotate acceleration target into forward-right frame (aircraft)
float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;
float accelRight = -accelN * posControl.actualState.sinYaw + accelE * posControl.actualState.cosYaw;
// Calculate banking angles
float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
int16_t maxBankAngle = DEGREES_TO_DECIDEGREES(posControl.navConfig->mc_max_bank_angle);
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
}
// Position to velocity controller for Z axis
static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
{
static pt1Filter_t velzFilterState;
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle from desired climb rate and forward velocity
// FIXME: Use airspeed here
float forwardVelocity = MAX(posControl.actualState.velXY, 300.0f); // Limit min velocity for PID controller at about 10 km/h
// Calculate max climb rate from current forward velocity and maximum pitch angle (climb angle is fairly small, approximate tan=sin)
float maxVelocityClimb = forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_climb_angle));
float maxVelocityDive = -forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_dive_angle));
posControl.desiredState.vel.V.Z = navPidApply2(posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), &posControl.pids.fw_alt, maxVelocityDive, maxVelocityClimb, false);
posControl.desiredState.vel.V.Z = pt1FilterApply4(&velzFilterState, posControl.desiredState.vel.V.Z, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
// Calculate climb angle ( >0 - climb, <0 - dive)
int16_t climbAngleDeciDeg = RADIANS_TO_DECIDEGREES(atan2_approx(posControl.desiredState.vel.V.Z, forwardVelocity));
climbAngleDeciDeg = constrain(climbAngleDeciDeg, -posControl.navConfig->fw_max_dive_angle * 10, posControl.navConfig->fw_max_climb_angle * 10);
posControl.rcAdjustment[PITCH] = climbAngleDeciDeg;
#if defined(NAV_BLACKBOX)
navDesiredVelocity[Z] = constrain(posControl.desiredState.vel.V.Z, -32678, 32767);
navTargetPosition[Z] = constrain(posControl.desiredState.pos.V.Z, -32678, 32767);
#endif
}
static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
{
// Calculate min and max throttle boundaries (to compensate for integral windup)
int16_t thrAdjustmentMin = (int16_t)posControl.escAndServoConfig->minthrottle - (int16_t)posControl.navConfig->mc_hover_throttle;
int16_t thrAdjustmentMax = (int16_t)posControl.escAndServoConfig->maxthrottle - (int16_t)posControl.navConfig->mc_hover_throttle;
posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &altholdThrottleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
}
static void updatePositionHeadingController_FW(uint32_t deltaMicros)
{
static bool forceTurnDirection = false;
// We have virtual position target, calculate heading error
int32_t virtualTargetBearing = calculateBearingToDestination(&virtualDesiredPosition);
// Calculate NAV heading error
int32_t headingError = wrap_18000(virtualTargetBearing - posControl.actualState.yaw);
// Forced turn direction
// If heading error is close to 180 deg we initiate forced turn and only disable it when heading error goes below 90 deg
if (ABS(headingError) > 17000) {
forceTurnDirection = true;
}
else if (ABS(headingError) < 9000 && forceTurnDirection) {
forceTurnDirection = false;
}
// If forced turn direction flag is enabled we fix the sign of the direction
if (forceTurnDirection) {
headingError = ABS(headingError);
}
// Input error in (deg*100), output pitch angle (deg*100)
float rollAdjustment = navPidApply2(posControl.actualState.yaw + headingError, posControl.actualState.yaw, US2S(deltaMicros), &posControl.pids.fw_nav,
-DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle),
DEGREES_TO_CENTIDEGREES(posControl.navConfig->fw_max_bank_angle),
true);
// Apply low-pass filter to prevent rapid correction
rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
// Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
// Update magHold heading lock in case pilot is using MAG mode (prevent MAGHOLD to fight navigation)
posControl.desiredState.yaw = wrap_36000(posControl.actualState.yaw + headingError);
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw));
// Add pitch compensation
//posControl.rcAdjustment[PITCH] = -CENTIDEGREES_TO_DECIDEGREES(ABS(rollAdjustment)) * 0.50f;
}
/* 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
}
