/*�Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */
bool snav_on_time(float nominal_radius)
{
nominal_radius = fabs(nominal_radius);
float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x);
float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr));
float remaining_time = snav_desired_tow - gps.tow / 1000.;
/* Use the nominal airspeed if the estimated one is not realistic */
float airspeed = stateGetAirspeed_f();
if (airspeed < NOMINAL_AIRSPEED / 2. ||
airspeed > 2.* NOMINAL_AIRSPEED) {
airspeed = NOMINAL_AIRSPEED;
}
/* Recompute ground speeds every 10 s */
if (ground_speed_timer == 0) {
ground_speed_timer = 40; /* every 10s, called at 40Hz */
compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y,
stateGetHorizontalWindspeed_f()->x); // Wind in NED frame
}
ground_speed_timer--;
/* Time to complete the circle at nominal_radius */
float nominal_time = 0.;
float a;
float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */
/* Going one step too far */
for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) {
float qdr = current_qdr + Sign(a_radius) * a;
ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2);
nominal_time += ANGLE_STEP * nominal_radius / ground_speed;
}
/* Removing what exceeds remaining_angle */
nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed;
/* Radius size to finish in one single circle */
float radius = remaining_time / nominal_time * nominal_radius;
if (radius > 2. * nominal_radius) {
radius = nominal_radius;
}
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(wp_cd.a, 0.);
radius *= Sign(a_radius);
wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y;
nav_circle_XY(wp_ca.x, wp_ca.y, radius);
/* Stay in this mode until the end of time */
return (remaining_time > 0);
}
static inline float ground_speed_of_course(float x) {
uint8_t i = Norm2Pi(M_PI_2-x)/ANGLE_STEP;
return ground_speeds[i];
}
