Beispiel #1
0
////////////////////////////////////////////////////////////////////////////////////
// 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;
  }
}
Beispiel #2
0
/***********************
函数功能:偏离航线计算纠正角,纠正角会叠加在目标航向上
输入参数:
***********************/
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;
    }
}