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); } }
//////////////////////////////////////////////////////////////////////////////////// // Get distance between two points in cm // Get bearing from pos1 to pos2, returns an 1deg = 100 precision static void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing) { float dLat = *destinationLat2 - *currentLat1; // difference of latitude in 1/10 000 000 degrees float dLon = (float)(*destinationLon2 - *currentLon1) * GPS_scaleLonDown; *dist = sqrtf(sq(dLat) + sq(dLon)) * DISTANCE_BETWEEN_TWO_LONGITUDE_POINTS_AT_EQUATOR_IN_HUNDREDS_OF_KILOMETERS; *bearing = 9000.0f + atan2_approx(-dLat, dLon) * TAN_89_99_DEGREES; // Convert the output radians to 100xdeg if (*bearing < 0) *bearing += 36000; }
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) { #ifndef GPS // this local variable should be optimized out when GPS is not used. float magneticDeclination = 0.0f; #endif /* Compute pitch/roll angles */ attitude.values.roll = lrintf(atan2_approx(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf)); attitude.values.pitch = lrintf(((0.5f * M_PIf) - acos_approx(-rMat[2][0])) * (1800.0f / M_PIf)); attitude.values.yaw = lrintf((-atan2_approx(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + 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); } }
static void calculateVirtualPositionTarget_FW(float trackingPeriod) { float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X; float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y; float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); // Limit minimum forward velocity to 1 m/s float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f); // If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target #define TAN_15DEG 0.26795f bool needToCalculateCircularLoiter = isApproachingLastWaypoint() && (distanceToActualTarget <= (posControl.navConfig->fw_loiter_radius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { // We are closing in on a waypoint, calculate circular loiter float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f); float loiterTargetX = posControl.desiredState.pos.V.X + posControl.navConfig->fw_loiter_radius * cos_approx(loiterAngle); float loiterTargetY = posControl.desiredState.pos.V.Y + posControl.navConfig->fw_loiter_radius * sin_approx(loiterAngle); // We have temporary loiter target. Recalculate distance and position error posErrorX = loiterTargetX - posControl.actualState.pos.V.X; posErrorY = loiterTargetY - posControl.actualState.pos.V.Y; distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY)); } // Calculate virtual waypoint virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget); virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (trackingDistance / distanceToActualTarget); // Shift position according to pilot's ROLL input (up to max_manual_speed velocity) if (posControl.flags.isAdjustingPosition) { int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband); if (rcRollAdjustment) { float rcShiftY = rcRollAdjustment * posControl.navConfig->max_manual_speed / 500.0f * trackingPeriod; // Rotate this target shift from body frame to to earth frame and apply to position target virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw; virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw; posControl.flags.isAdjustingPosition = true; } } }