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_UNIT_TESTED void imuUpdateEulerAngles(void)
{
/* Compute pitch/roll angles */
attitude.values.roll = RADIANS_TO_DECIDEGREES(atan2_approx(rMat[2][1], rMat[2][2]));
attitude.values.pitch = RADIANS_TO_DECIDEGREES((0.5f * M_PIf) - acos_approx(-rMat[2][0]));
attitude.values.yaw = RADIANS_TO_DECIDEGREES(-atan2_approx(rMat[1][0], rMat[0][0])) + magneticDeclination;
if (attitude.values.yaw < 0)
attitude.values.yaw += 3600;
/* Update small angle state */
if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE);
} else {
DISABLE_STATE(SMALL_ANGLE);
}
}
