/*�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]; }