/* For a landing UPWIND.
Computes Top Of Descent waypoint from Touch Down and Approach Fix
waypoints, using glide airspeed, glide vertical speed and wind */
static inline bool_t compute_TOD(uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
{
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / glide_vspeed * (glide_airspeed - sqrtf(
wind->x * wind->x + wind->y * wind->y));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
return FALSE;
}
bool_t nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = GetPosX();
WaypointY(_td) = GetPosY();
WaypointAlt(_td) = GetPosAlt();
return FALSE;
}
bool_t nav_select_touch_down(uint8_t _td)
{
WaypointX(_td) = stateGetPositionEnu_f()->x;
WaypointY(_td) = stateGetPositionEnu_f()->y;
WaypointAlt(_td) = stateGetPositionUtm_f()->alt;
return FALSE;
}
bool_t disc_survey( uint8_t center, float radius) {
float wind_dir = atan2(wind_north, wind_east) + M_PI;
/** Not null even if wind_east=wind_north=0 */
float upwind_x = cos(wind_dir);
float upwind_y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (status) {
case UTURN:
nav_circle_XY(c.x, c.y, grid*sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) {
c1.x = estimator_x;
c1.y = estimator_y;
float d = ScalarProduct(upwind_x, upwind_y, estimator_x-WaypointX(center), estimator_y-WaypointY(center));
if (d > radius) {
status = DOWNWIND;
} else {
float w = sqrt(radius*radius - d*d) - 1.5*grid;
float crosswind_x = - upwind_y;
float crosswind_y = upwind_x;
c2.x = WaypointX(center)+d*upwind_x-w*sign*crosswind_x;
c2.y = WaypointY(center)+d*upwind_y-w*sign*crosswind_y;
status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
c2.x = WaypointX(center) - upwind_x * radius;
c2.y = WaypointY(center) - upwind_y * radius;
status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(c1.x, c1.y, c2.x, c2.y);
if (nav_approaching_xy(c2.x, c2.y, c1.x, c1.y, CARROT)) {
c.x = c2.x + grid*upwind_x;
c.y = c2.y + grid*upwind_y;
sign = -sign;
status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center), 0.); /* No preclimb */
return TRUE;
}
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td) {
if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
WaypointX(_af)=WaypointX(_td)-1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tan(app_angle));
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
if (td_tod > td_af) {
WaypointX(_af) = WaypointX(_tod) + td_af_x / td_af * app_intercept_af_tod;
WaypointY(_af) = WaypointY(_tod) + td_af_y / td_af * app_intercept_af_tod;
}
return FALSE;
} /* end of gls_copute_TOD */
bool nav_bungee_takeoff_setup(uint8_t bungee_wp)
{
// Store bungee point (from WP id, altitude should be ground alt)
// FIXME use current alt instead ?
VECT3_ASSIGN(bungee_point, WaypointX(bungee_wp), WaypointY(bungee_wp), WaypointAlt(bungee_wp));
// Compute other points
compute_points_from_bungee();
// Enable Launch Status and turn kill throttle on
CTakeoffStatus = Launch;
kill_throttle = 1;
return false;
}
bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
if (init){
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
#endif
init = FALSE;
}
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress,-1,1);
float nav_final_length = sqrt(final2);
float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
Bound(pre_climb, -5, 0.);
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt = start_alt + nav_final_progress * diff_alt;
Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(v_ctl_altitude_pgain))) // to prevent climbing before intercept
if(nav_final_progress < -0.5) { // for smooth intercept
NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
else {
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
}
return TRUE;
} // end of gls()
static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
if ((WaypointX(_af) == WaypointX(_td)) && (WaypointY(_af) == WaypointY(_td))) {
WaypointX(_af) = WaypointX(_td) - 1;
}
float td_af_x = WaypointX(_af) - WaypointX(_td);
float td_af_y = WaypointY(_af) - WaypointY(_td);
float td_af = sqrt(td_af_x * td_af_x + td_af_y * td_af_y);
float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle));
// set wapoint TOD (top of decent)
WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
WaypointAlt(_tod) = WaypointAlt(_af);
// calculate ground speed on final (target_speed - head wind)
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_norm = sqrt(wind->x * wind->x + wind->y * wind->y);
float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) +
((td_af_y * wind->x) / (td_af * wind_norm)));
Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL);
gs_on_final = target_speed - wind_on_final;
// calculate position of SD (start decent)
float t_sd_intercept = (gs_on_final * tanf(app_angle)) / app_intercept_rate; //time
sd_intercept = gs_on_final * t_sd_intercept; // distance
sd_tod = 0.5 * sd_intercept;
// set waypoint SD (start decent)
WaypointX(_sd) = WaypointX(_tod) + td_af_x / td_af * sd_tod;
WaypointY(_sd) = WaypointY(_tod) + td_af_y / td_af * sd_tod;
WaypointAlt(_sd) = WaypointAlt(_af);
// calculate td_sd
float td_sd_x = WaypointX(_sd) - WaypointX(_td);
float td_sd_y = WaypointY(_sd) - WaypointY(_td);
float td_sd = sqrt(td_sd_x * td_sd_x + td_sd_y * td_sd_y);
// calculate sd_tod in x,y
sd_tod_x = WaypointX(_tod) - WaypointX(_sd);
sd_tod_y = WaypointY(_tod) - WaypointY(_sd);
// set Waypoint AF at least befor SD
if ((td_sd + app_distance_af_sd) > td_af) {
WaypointX(_af) = WaypointX(_sd) + td_af_x / td_af * app_distance_af_sd;
WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd;
}
return FALSE;
} /* end of gls_copute_TOD */
bool_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
{
// set target speed for approach on final
if (init) {
#if USE_AIRSPEED
v_ctl_auto_airspeed_setpoint = target_speed;
#endif
init = FALSE;
}
// calculate distance tod_td
float final_x = WaypointX(_td) - WaypointX(_tod);
float final_y = WaypointY(_td) - WaypointY(_tod);
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float hspeed = *stateGetHorizontalSpeedNorm_f();
float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
(pos_enu->y - WaypointY(_tod)) * final_y) / final2;
Bound(nav_final_progress, -1, 1);
// float nav_final_length = sqrt(final2);
// calculate requiered decent rate on glideslope
float pre_climb_glideslope = hspeed * (-tanf(app_angle));
// calculate glideslope
float start_alt = WaypointAlt(_tod);
float diff_alt = WaypointAlt(_td) - start_alt;
float alt_glideslope = start_alt + nav_final_progress * diff_alt;
// calculate intercept
float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x +
(pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) /
Max((sd_intercept * sd_intercept), 1.);
Bound(nav_intercept_progress, -1, 1);
float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
//########################################################
// handle the different vertical approach segments
float pre_climb = 0.;
float alt = 0.;
// distance
float f_af = sqrt((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) +
(pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af)));
if (f_af < app_distance_af_sd) { // approach fix (AF) to start descent (SD)
alt = WaypointAlt(_af);
pre_climb = 0.;
} else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) {
// start descent (SD) to intercept
alt = alt_intercept;
pre_climb = pre_climb_intercept;
} else { //glideslope (intercept to touch down)
alt = alt_glideslope;
pre_climb = pre_climb_glideslope;
}
// Bound(pre_climb, -5, 0.);
//######################### autopilot modes
NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0); // throttle mode
NavSegment(_af, _td); // horizontal mode (stay on localiser)
return TRUE;
} // end of gls()
bool_t nav_line_run(uint8_t l1, uint8_t l2, float radius) {
radius = fabs(radius);
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = WaypointX(l1) - WaypointX(l2);
float l2_l1_y = WaypointY(l1) - WaypointY(l2);
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { WaypointX(l1) + radius * u_y,
WaypointY(l1) + radius * -u_x,
alt };
struct point l2_c2 = { WaypointX(l1) + 1.732*radius * u_x,
WaypointY(l1) + 1.732*radius * u_y,
alt };
struct point l2_c3 = { WaypointX(l1) + radius * -u_y,
WaypointY(l1) + radius * u_x,
alt };
struct point l1_c1 = { WaypointX(l2) + radius * -u_y,
WaypointY(l2) + radius * u_x,
alt };
struct point l1_c2 = { WaypointX(l2) +1.732*radius * -u_x,
WaypointY(l2) + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { WaypointX(l2) + radius * u_y,
WaypointY(l2) + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
/* Vertical target */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (line_status) {
case LR12: /* From wp l2 to wp l1 */
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
line_status = LQC21;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21: /* From wp l1 to wp l2 */
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
line_status = LQC12;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
break;
default: /* Should not occur !!! End the pattern */
return FALSE;
}
return TRUE; /* This pattern never ends */
}
/* D is the current position */
bool_t snav_init(uint8_t a, float desired_course_rad, float radius) {
wp_a = a;
radius = fabs(radius);
float da_x = WaypointX(wp_a) - estimator_x;
float da_y = WaypointY(wp_a) - estimator_y;
/* D_CD orthogonal to current course, CD on the side of A */
float u_x = cos(M_PI_2 - estimator_hspeed_dir);
float u_y = sin(M_PI_2 - estimator_hspeed_dir);
d_radius = - Sign(u_x*da_y - u_y*da_x) * radius;
wp_cd.x = estimator_x + d_radius * u_y;
wp_cd.y = estimator_y - d_radius * u_x;
wp_cd.a = WaypointAlt(wp_a);
/* A_CA orthogonal to desired course, CA on the side of D */
float desired_u_x = cos(M_PI_2 - desired_course_rad);
float desired_u_y = sin(M_PI_2 - desired_course_rad);
a_radius = Sign(desired_u_x*da_y - desired_u_y*da_x) * radius;
u_a_ca_x = desired_u_y;
u_a_ca_y = - desired_u_x;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
wp_ca.a = WaypointAlt(wp_a);
/* Unit vector along CD-CA */
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
float cd_ca = sqrt(u_x*u_x+u_y*u_y);
/* If it is too close in reverse direction, set CA on the other side */
if (a_radius * d_radius < 0 && cd_ca < 2 * radius) {
a_radius = -a_radius;
wp_ca.x = WaypointX(wp_a) + a_radius * u_a_ca_x;
wp_ca.y = WaypointY(wp_a) + a_radius * u_a_ca_y;
u_x = wp_ca.x - wp_cd.x;
u_y = wp_ca.y - wp_cd.y;
cd_ca = sqrt(u_x*u_x+u_y*u_y);
}
u_x /= cd_ca;
u_y /= cd_ca;
if (a_radius * d_radius > 0) {
/* Both arcs are in the same direction */
/* CD_TD orthogonal to CD_CA */
wp_td.x = wp_cd.x - d_radius * u_y;
wp_td.y = wp_cd.y + d_radius * u_x;
/* CA_TA also orthogonal to CD_CA */
wp_ta.x = wp_ca.x - a_radius * u_y;
wp_ta.y = wp_ca.y + a_radius * u_x;
} else {
/* Arcs are in reverse directions: trigonemetric puzzle :-) */
float alpha = atan2(u_y, u_x) + acos(d_radius/(cd_ca/2));
wp_td.x = wp_cd.x + d_radius * cos(alpha);
wp_td.y = wp_cd.y + d_radius * sin(alpha);
wp_ta.x = wp_ca.x + a_radius * cos(alpha);
wp_ta.y = wp_ca.y + a_radius * sin(alpha);
}
qdr_td = M_PI_2 - atan2(wp_td.y-wp_cd.y, wp_td.x-wp_cd.x);
qdr_a = M_PI_2 - atan2(WaypointY(wp_a)-wp_ca.y, WaypointX(wp_a)-wp_ca.x);
wp_td.a = wp_cd.a;
wp_ta.a = wp_ca.a;
ground_speed_timer = 0;
return FALSE;
}
void nav_survey_rectangle(uint8_t wp1, uint8_t wp2) {
static float survey_radius;
nav_survey_active = TRUE;
nav_survey_west = Min(WaypointX(wp1), WaypointX(wp2));
nav_survey_east = Max(WaypointX(wp1), WaypointX(wp2));
nav_survey_south = Min(WaypointY(wp1), WaypointY(wp2));
nav_survey_north = Max(WaypointY(wp1), WaypointY(wp2));
/* Update the current segment from corners' coordinates*/
if (SurveyGoingNorth()) {
survey_to.y = nav_survey_north;
survey_from.y = nav_survey_south;
} else if (SurveyGoingSouth()) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else if (SurveyGoingEast()) {
survey_to.x = nav_survey_east;
survey_from.x = nav_survey_west;
} else if (SurveyGoingWest()) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
}
if (! survey_uturn) { /* S-N, N-S, W-E or E-W straight route */
if ((estimator_y < nav_survey_north && SurveyGoingNorth()) ||
(estimator_y > nav_survey_south && SurveyGoingSouth()) ||
(estimator_x < nav_survey_east && SurveyGoingEast()) ||
(estimator_x > nav_survey_west && SurveyGoingWest())) {
/* Continue ... */
nav_route_xy(survey_from.x, survey_from.y, survey_to.x, survey_to.y);
} else {
if (survey_orientation == NS) {
/* North or South limit reached, prepare U-turn and next leg */
float x0 = survey_from.x; /* Current longitude */
if (x0+nav_survey_shift < nav_survey_west || x0+nav_survey_shift > nav_survey_east) {
x0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
x0 = x0 + nav_survey_shift; /* Longitude of next leg */
survey_from.x = survey_to.x = x0;
/* Swap South and North extremities */
float tmp = survey_from.y;
survey_from.y = survey_to.y;
survey_to.y = tmp;
/** Do half a circle around WP 0 */
waypoints[0].x = x0 - nav_survey_shift/2.;
waypoints[0].y = survey_from.y;
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingNorth()) {
survey_radius = -survey_radius;
}
} else { /* (survey_orientation == WE) */
/* East or West limit reached, prepare U-turn and next leg */
/* There is a y0 declared in math.h (for ARM) !!! */
float my_y0 = survey_from.y; /* Current latitude */
if (my_y0+nav_survey_shift < nav_survey_south || my_y0+nav_survey_shift > nav_survey_north) {
my_y0 += nav_survey_shift / 2;
nav_survey_shift = -nav_survey_shift;
}
my_y0 = my_y0 + nav_survey_shift; /* Longitude of next leg */
survey_from.y = survey_to.y = my_y0;
/* Swap West and East extremities */
float tmp = survey_from.x;
survey_from.x = survey_to.x;
survey_to.x = tmp;
/** Do half a circle around WP 0 */
waypoints[0].x = survey_from.x;
waypoints[0].y = my_y0 - nav_survey_shift/2.;
/* Computes the right direction for the circle */
survey_radius = nav_survey_shift / 2.;
if (SurveyGoingWest()) {
survey_radius = -survey_radius;
}
}
nav_in_segment = FALSE;
survey_uturn = TRUE;
LINE_STOP_FUNCTION;
}
} else { /* U-turn */
if ((SurveyGoingNorth() && NavCourseCloseTo(0)) ||
(SurveyGoingSouth() && NavCourseCloseTo(180)) ||
(SurveyGoingEast() && NavCourseCloseTo(90)) ||
(SurveyGoingWest() && NavCourseCloseTo(270))) {
/* U-turn finished, back on a segment */
survey_uturn = FALSE;
nav_in_circle = FALSE;
LINE_START_FUNCTION;
} else {
NavCircleWaypoint(0, survey_radius);
}
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(wp1), 0.); /* No preclimb */
}
bool_t VerticalRaster(uint8_t l1, uint8_t l2, float radius, float AltSweep) {
radius = fabs(radius);
float alt = waypoints[l1].a;
waypoints[l2].a = alt;
float l2_l1_x = waypoints[l1].x - waypoints[l2].x;
float l2_l1_y = waypoints[l1].y - waypoints[l2].y;
float d = sqrt(l2_l1_x*l2_l1_x+l2_l1_y*l2_l1_y);
/* Unit vector from l1 to l2 */
float u_x = l2_l1_x / d;
float u_y = l2_l1_y / d;
/* The half circle centers and the other leg */
struct point l2_c1 = { waypoints[l1].x + radius * u_y,
waypoints[l1].y + radius * -u_x,
alt };
struct point l2_c2 = { waypoints[l1].x + 1.732*radius * u_x,
waypoints[l1].y + 1.732*radius * u_y,
alt };
struct point l2_c3 = { waypoints[l1].x + radius * -u_y,
waypoints[l1].y + radius * u_x,
alt };
struct point l1_c1 = { waypoints[l2].x + radius * -u_y,
waypoints[l2].y + radius * u_x,
alt };
struct point l1_c2 = { waypoints[l2].x +1.732*radius * -u_x,
waypoints[l2].y + 1.732*radius * -u_y,
alt };
struct point l1_c3 = { waypoints[l2].x + radius * u_y,
waypoints[l2].y + radius * -u_x,
alt };
float qdr_out_2_1 = M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_2 = -M_PI/3. - atan2(u_y, u_x);
float qdr_out_2_3 = M_PI - atan2(u_y, u_x);
/* Vertical target */
NavVerticalAutoThrottleMode(0); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(l1), 0.);
switch (line_status) {
case LR12: /* From wp l2 to wp l1 */
NavSegment(l2, l1);
if (NavApproachingFrom(l1, l2, CARROT)) {
line_status = LQC21;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC21:
nav_circle_XY(l2_c1.x, l2_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1)-10)) {
line_status = LTC2;
nav_init_stage();
}
break;
case LTC2:
nav_circle_XY(l2_c2.x, l2_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2)+10) && estimator_z >= (waypoints[l1].a-10)) {
line_status = LQC22;
nav_init_stage();
}
break;
case LQC22:
nav_circle_XY(l2_c3.x, l2_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3)-10)) {
line_status = LR21;
nav_init_stage();
}
break;
case LR21: /* From wp l1 to wp l2 */
NavSegment(l1, l2);
if (NavApproachingFrom(l2, l1, CARROT)) {
line_status = LQC12;
waypoints[l1].a = waypoints[l1].a+AltSweep;
nav_init_stage();
}
break;
case LQC12:
nav_circle_XY(l1_c1.x, l1_c1.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_1 + M_PI)-10)) {
line_status = LTC1;
nav_init_stage();
}
break;
case LTC1:
nav_circle_XY(l1_c2.x, l1_c2.y, -radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_2 + M_PI)+10) && estimator_z >= (waypoints[l1].a-5)) {
line_status = LQC11;
nav_init_stage();
}
break;
case LQC11:
nav_circle_XY(l1_c3.x, l1_c3.y, radius);
if (NavQdrCloseTo(DegOfRad(qdr_out_2_3 + M_PI)-10)) {
line_status = LR12;
nav_init_stage();
}
}
return TRUE; /* This pattern never ends */
}
bool nav_survey_disc_run(uint8_t center_wp, float radius)
{
struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
float wind_dir = atan2(wind->x, wind->y) + M_PI;
/** Not null even if wind_east=wind_north=0 */
struct FloatVect2 upwind;
upwind.x = cos(wind_dir);
upwind.y = sin(wind_dir);
float grid = nav_survey_shift / 2;
switch (disc_survey.status) {
case UTURN:
nav_circle_XY(disc_survey.c.x, disc_survey.c.y, grid * disc_survey.sign);
if (NavQdrCloseTo(DegOfRad(M_PI_2 - wind_dir))) {
disc_survey.c1.x = stateGetPositionEnu_f()->x;
disc_survey.c1.y = stateGetPositionEnu_f()->y;
struct FloatVect2 dist;
VECT2_DIFF(dist, disc_survey.c1, waypoints[center_wp]);
float d = VECT2_DOT_PRODUCT(upwind, dist);
if (d > radius) {
disc_survey.status = DOWNWIND;
} else {
float w = sqrtf(radius * radius - d * d) - 1.5 * grid;
struct FloatVect2 crosswind;
crosswind.x = -upwind.y;
crosswind.y = upwind.x;
disc_survey.c2.x = waypoints[center_wp].x + d * upwind.x - w * disc_survey.sign * crosswind.x;
disc_survey.c2.y = waypoints[center_wp].y + d * upwind.y - w * disc_survey.sign * crosswind.y;
disc_survey.status = SEGMENT;
}
nav_init_stage();
}
break;
case DOWNWIND:
disc_survey.c2.x = waypoints[center_wp].x - upwind.x * radius;
disc_survey.c2.y = waypoints[center_wp].y - upwind.y * radius;
disc_survey.status = SEGMENT;
/* No break; */
case SEGMENT:
nav_route_xy(disc_survey.c1.x, disc_survey.c1.y, disc_survey.c2.x, disc_survey.c2.y);
if (nav_approaching_xy(disc_survey.c2.x, disc_survey.c2.y, disc_survey.c1.x, disc_survey.c1.y, CARROT)) {
disc_survey.c.x = disc_survey.c2.x + grid * upwind.x;
disc_survey.c.y = disc_survey.c2.y + grid * upwind.y;
disc_survey.sign = -disc_survey.sign;
disc_survey.status = UTURN;
nav_init_stage();
}
break;
default:
break;
}
NavVerticalAutoThrottleMode(0.); /* No pitch */
NavVerticalAltitudeMode(WaypointAlt(center_wp), 0.); /* No preclimb */
return true;
}
bool nav_catapult_run(uint8_t _climb)
{
switch (nav_catapult.status) {
case NAV_CATAPULT_UNINIT:
// start high freq function if not done
if (nav_catapult_nav_catapult_highrate_module_status != MODULES_RUN) {
nav_catapult_nav_catapult_highrate_module_status = MODULES_START;
}
// arm catapult
nav_catapult.status = NAV_CATAPULT_ARMED;
break;
case NAV_CATAPULT_ARMED:
// store initial position
nav_catapult.pos.x = stateGetPositionEnu_f()->x;
nav_catapult.pos.y = stateGetPositionEnu_f()->y;
nav_catapult.pos.z = stateGetPositionUtm_f()->alt; // useful ?
nav_catapult.status = NAV_CATAPULT_WAIT_ACCEL;
break;
case NAV_CATAPULT_WAIT_ACCEL:
// no throttle, zero attitude
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(0.f);
// wait for acceleration from high speed function
break;
case NAV_CATAPULT_MOTOR_ON:
// fixed attitude and motor
NavAttitude(RadOfDeg(0.f));
NavVerticalAutoThrottleMode(nav_catapult.initial_pitch);
NavVerticalThrottleMode(MAX_PPRZ * nav_catapult.initial_throttle);
if (nav_catapult.timer >= nav_catapult.heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
// store heading, move climb waypoint
float dir_x = stateGetPositionEnu_f()->x - nav_catapult.pos.x;
float dir_y = stateGetPositionEnu_f()->y - nav_catapult.pos.y;
float dir_L = sqrtf(dir_x * dir_x + dir_y * dir_y);
WaypointX(_climb) = nav_catapult.pos.x + (dir_x / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
WaypointY(_climb) = nav_catapult.pos.y + (dir_y / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
DownlinkSendWpNr(_climb);
// next step
nav_catapult.status = NAV_CATAPULT_MOTOR_CLIMB;
}
break;
case NAV_CATAPULT_MOTOR_CLIMB:
// normal climb: heading locked by waypoint target
NavVerticalAltitudeMode(WaypointAlt(_climb), 0.f); // vertical mode (folow glideslope)
NavVerticalAutoThrottleMode(0.f); // throttle mode
NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
if (nav_approaching_xy(WaypointX(_climb), WaypointY(_climb), nav_catapult.pos.x, nav_catapult.pos.y, 0.f)) {
// reaching climb waypoint, end procedure
nav_catapult.status = NAV_CATAPULT_DISARM;
}
break;
case NAV_CATAPULT_DISARM:
// end procedure
nav_catapult.status = NAV_CATAPULT_UNINIT;
nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
return false;
default:
return false;
}
// procedure still running
return true;
}
