////////////////////////////////////////////////////////////////////////////////////
// Calculating cross track error, this tries to keep the copter on a direct line
// when flying to a waypoint.
//
static void GPS_update_crosstrack(void) {
if (fabs(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sin(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
}else{
nav_bearing = target_bearing;
}
}
/***********************
函数功能：偏离航线计算纠正角，纠正角会叠加在目标航向上
输入参数：
***********************/
static void GPS_update_crosstrack(void) //偏离航线计算纠正角，纠正角会叠加在目标航向上
{
float crosstrack_output;
if (abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) // 偏差角度太大就不需要这种做法 ，大于45度纠正会出现纠航向大于90度
{
float temp = (target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * wp_distance; //算出偏离航线的距离，点到线的距离
crosstrack_output = AC_PID_get_p(&pid_nav_crosstrack, crosstrack_error, &nav_crosstrackPID)
+ AC_PID_get_i(&pid_nav_crosstrack, crosstrack_error, &dTnav, &nav_crosstrackPID)
+ AC_PID_get_d(&pid_nav_crosstrack, crosstrack_error, &dTnav, &nav_crosstrackPID);
nav_bearing = target_bearing + constrain(crosstrack_output, -4500, 4500); //限制纠正角为30度
nav_bearing = wrap_36000(nav_bearing);
}
else
{
nav_bearing = target_bearing;
}
}
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 the desired nav_lat and nav_lon for distance flying such as RTH
void GPS_calc_nav_rate(uint16_t max_speed)
{
float trig[2], rate_error;
float temp, tiltcomp, trgtspeed;
uint8_t axis;
int32_t crosstrack_error;
if ((abs(wrap_18000(target_bearing - original_target_bearing)) < 4500) && cfg.nav_ctrkgain != 0)// If we are too far off or too close we don't do track following
{
temp = (float)(target_bearing - original_target_bearing) * RADX100;
crosstrack_error = sinf(temp) * (float)wp_distance * cfg.nav_ctrkgain; // Meters we are off track line
nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000);
nav_bearing = wrap_36000(nav_bearing);
}
else nav_bearing = target_bearing;
temp = (float)(9000l - nav_bearing) * RADX100; // nav_bearing and maybe crosstrack
trig[GPS_X] = cosf(temp);
trig[GPS_Y] = sinf(temp);
for (axis = 0; axis < 2; axis++)
{
trgtspeed = (trig[axis] * (float)max_speed); // Target speed
rate_error = trgtspeed - MIX_speed[axis]; // Since my INS Stuff is shit, reduce ACC influence to 50% anyway better than leadfilter
rate_error = constrain(rate_error, -1000, 1000);
nav[axis] = get_P(rate_error, &navPID_PARAM) + // P + I + D
get_I(rate_error, &dTnav, &navPID[axis], &navPID_PARAM) +
get_D(rate_error, &dTnav, &navPID[axis], &navPID_PARAM);
if (cfg.nav_tiltcomp != 0) // Do the apm 2.9.1 magic tiltcompensation
{
tiltcomp = trgtspeed * trgtspeed * ((float)cfg.nav_tiltcomp * 0.0001f);
if(trgtspeed < 0) tiltcomp = -tiltcomp;
}
else tiltcomp = 0;
nav[axis] = constrain(nav[axis] + tiltcomp, -maxbank100, maxbank100);
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
