////////////////////////////////////////////////////////////////////////////////////
// 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]));
}
}
////////////////////////////////////////////////////////////////////////////////////
// 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]));
}
}
/***********************
函数功能：根据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]));
}
}
/***********************
函数功能：偏离航线计算纠正角，纠正角会叠加在目标航向上
输入参数：
***********************/
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;
}
}