////////////////////////////////////////////////////////////////////////////////////
// Calculate the desired nav_lat and nav_lon for distance flying such as RTH
//
static void GPS_calc_nav_rate(uint16_t max_speed) {
float trig[2];
uint8_t axis;
float temp;
// push us towards the original track
GPS_update_crosstrack();
// nav_bearing includes crosstrack
temp = (9000l - nav_bearing) * RADX100;
trig[_X] = cos(temp);
trig[_Y] = sin(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] =
get_P(rate_error[axis], &navPID_PARAM)
+get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM)
+get_D(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM);
nav[axis] = constrain(nav[axis], -NAV_BANK_MAX, NAV_BANK_MAX);
poshold_ratePID[axis].integrator = navPID[axis].integrator;
}
}
////////////////////////////////////////////////////////////////////////////////////
// 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]));
}
}