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