Exemplo n.º 1
0
////////////////////////////////////////////////////////////////////////////////////
// Calculate nav_lat and nav_lon from the x and y error and the speed
//
static void GPS_calc_poshold()
{
    int32_t p, i, d;
    int32_t output;
    int32_t target_speed;
    uint8_t axis;

    for (axis = 0; axis < 2; axis++) {
        target_speed = AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID);       // calculate desired speed from lon error
        rate_error[axis] = target_speed - actual_speed[axis];   // calc the speed error

        p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
        i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis] + error[axis], &dTnav, &poshold_ratePID);
        d = AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
        d = constrain(d, -2000, 2000);
        // get rid of noise
#if defined(GPS_LOW_SPEED_D_FILTER)
        if (abs(actual_speed[axis]) < 50)
            d = 0;
#endif
        output = p + i + d;

        nav[axis] = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX);
        AC_PID_set_integrator(&pid_nav[axis], AC_PID_get_integrator(&pid_poshold_rate[axis]));
    }
}
Exemplo n.º 2
0
////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(int max_speed)
{
    float trig[2];
    float temp;
    uint8_t axis;

    // push us towards the original track
    GPS_update_crosstrack();

    // nav_bearing includes crosstrack
    temp = (9000l - nav_bearing) * RADX100;
    trig[_X] = cosf(temp);
    trig[_Y] = sinf(temp);

    for (axis = 0; axis < 2; axis++) {
        rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis];
        rate_error[axis] = constrain(rate_error[axis], -1000, 1000);
        // P + I + D
        nav[axis] = AC_PID_get_p(&pid_nav[axis], rate_error[axis], &navPID)
            + AC_PID_get_i(&pid_nav[axis], rate_error[axis], &dTnav, &navPID)
            + AC_PID_get_d(&pid_nav[axis], rate_error[axis], &dTnav, &navPID);
        nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
        AC_PID_set_integrator(&pid_poshold_rate[axis], AC_PID_get_integrator(&pid_nav[axis]));
    }
}
Exemplo n.º 3
0
/***********************
函数功能:根据X,Y的误差和速度计算导航经纬度
输入参数:神马都没有
***********************/
static void GPS_calc_poshold(void)
{
    int32_t p, i, d;
    int32_t output;
    int32_t target_speed;
	static int32_t d2[2],d1[2],dsum;
    int axis;

    for (axis = 0; axis < 2; axis++) {	//0是纬度,1是经度
		if(GPS_Speed_Mode)			     //GPS姿态模式下  使用摇杆控制飞行速度
		{
			target_speed = GPS_target_speed[axis] ;
		}
		else
	        target_speed = constrain(AC_PID_get_p(&pi_poshold[axis], error[axis], &posholdPID),-300,300);       //根据误差计算期望转速  最大值 150cm/s

        rate_error[axis] = constrain(target_speed - actual_speed[axis],-300,300);   // calc the speed error

				//临时
//	debug_A7108[1]=target_speed;

//        p = AC_PID_get_p(&pid_poshold_rate[axis], error[axis], &poshold_ratePID);		//	 rate_error[axis]
//        i = AC_PID_get_i(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);	   //imax=2000;20*100
    //    d = 100 * AC_PID_get_d(&pid_poshold_rate[axis], error[axis], &dTnav, &poshold_ratePID);
	//	
 //       d = constrain(d, -2.0*abs(error[axis]), 2.0*abs(error[axis]));
 //		if(abs(error[axis])<200)   d = constrain(d, -200, 200);
//		else		d = constrain(d, -800, 800);
        p = AC_PID_get_p(&pid_poshold_rate[axis], rate_error[axis], &poshold_ratePID);
        i = AC_PID_get_i(&pid_poshold_rate[axis], rate_error[axis], &dTnav, &poshold_ratePID);	   //  + error[axis] imax=2000;20*100
 
		d = -100 * poshold_ratePID.kD * actual_speed[axis];

		d = constrain(d, -500, 500);

		dsum = d + d1[axis] + d2[axis];
		d2[axis] = d1[axis];
		d1[axis] = d;

        // get rid of noise							 
//#if defined(GPS_LOW_SPEED_D_FILTER)
//        if (abs(error[axis]) < 50)
//            d = 0;
//#endif
        output = p + i + dsum/3;	 // dsum/3		  // output/10就再三角变换就是ANGLE了

        nav[axis] = constrain(output, -NAV_BANK_MAX, NAV_BANK_MAX);
        AC_PID_set_integrator(&pid_nav[axis], AC_PID_get_integrator(&pid_poshold_rate[axis]));
    }
}