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;
}
void resetFixedWingHeadingController(void)
{
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
}
void resetMulticopterHeadingController(void)
{
magHold = CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw);
}
static void applyMulticopterHeadingController(void)
{
magHold = CENTIDEGREES_TO_DEGREES(posControl.desiredState.yaw);
}
void resetMulticopterHeadingController(void)
{
updateMagHoldHeading(CENTIDEGREES_TO_DEGREES(posControl.actualState.yaw));
}
