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()
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 */ }
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_t nav_anemotaxis( uint8_t c, uint8_t c1, uint8_t c2, uint8_t plume ) { if (chemo_sensor) { last_plume_was_here(); waypoints[plume].x = stateGetPositionEnu_f()->x; waypoints[plume].y = stateGetPositionEnu_f()->y; // DownlinkSendWp(plume); } struct FloatVect2* wind = stateGetHorizontalWindspeed_f(); float wind_dir = atan2(wind->x, wind->y) + M_PI; /** Not null even if wind_east=wind_north=0 */ float upwind_x = cos(wind_dir); float upwind_y = sin(wind_dir); switch (status) { case UTURN: NavCircleWaypoint(c, DEFAULT_CIRCLE_RADIUS*sign); if (NavQdrCloseTo(DegOfRad(M_PI_2-wind_dir))) { float crosswind_x = - upwind_y; float crosswind_y = upwind_x; waypoints[c1].x = waypoints[c].x + DEFAULT_CIRCLE_RADIUS*upwind_x; waypoints[c1].y = waypoints[c].y + DEFAULT_CIRCLE_RADIUS*upwind_y; float width = Max(2*ScalarProduct(upwind_x, upwind_y, stateGetPositionEnu_f()->x-last_plume.x, stateGetPositionEnu_f()->y-last_plume.y), DEFAULT_CIRCLE_RADIUS); waypoints[c2].x = waypoints[c1].x - width*crosswind_x*sign; waypoints[c2].y = waypoints[c1].y - width*crosswind_y*sign; // DownlinkSendWp(c1); // DownlinkSendWp(c2); status = CROSSWIND; nav_init_stage(); } break; case CROSSWIND: NavSegment(c1, c2); if (NavApproaching(c2, CARROT)) { waypoints[c].x = waypoints[c2].x + DEFAULT_CIRCLE_RADIUS*upwind_x; waypoints[c].y = waypoints[c2].y + DEFAULT_CIRCLE_RADIUS*upwind_y; // DownlinkSendWp(c); sign = -sign; status = UTURN; nav_init_stage(); } if (chemo_sensor) { waypoints[c].x = stateGetPositionEnu_f()->x + DEFAULT_CIRCLE_RADIUS*upwind_x; waypoints[c].y = stateGetPositionEnu_f()->y + DEFAULT_CIRCLE_RADIUS*upwind_y; // DownlinkSendWp(c); sign = -sign; status = UTURN; nav_init_stage(); } break; } chemo_sensor = 0; return TRUE; }