Paparazzi UAS  v5.15_devel-230-gc96ce27
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_gls.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012, Tobias Münch
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
46 #include "generated/airframe.h"
47 #include "state.h"
48 #include "modules/nav/nav_gls.h"
50 #include "generated/flight_plan.h"
51 
53 float app_angle;
56 
57 bool init = true;
58 
59 #ifndef APP_TARGET_SPEED
60 #define APP_TARGET_SPEED NOMINAL_AIRSPEED
61 #endif
62 #define MAX_WIND_ON_FINAL 0.8*APP_TARGET_SPEED
63 
64 #ifndef APP_ANGLE
65 #define APP_ANGLE RadOfDeg(5)
66 #endif
67 
68 #ifndef APP_INTERCEPT_RATE
69 #define APP_INTERCEPT_RATE 0.625 // 4s from start decent until intercept with pre_climb = -2.5 m/s
70 #endif
71 
72 #ifndef APP_DISTANCE_AF_SD
73 #define APP_DISTANCE_AF_SD 100
74 #endif
75 
77 float sd_tod;
79 float sd_tod_x;
80 float sd_tod_y;
81 
82 static inline bool gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
83 {
84 
85  if ((WaypointX(_af) == WaypointX(_td)) && (WaypointY(_af) == WaypointY(_td))) {
86  WaypointX(_af) = WaypointX(_td) - 1;
87  }
88 
89  float td_af_x = WaypointX(_af) - WaypointX(_td);
90  float td_af_y = WaypointY(_af) - WaypointY(_td);
91  float td_af = sqrtf(td_af_x * td_af_x + td_af_y * td_af_y);
92  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle));
93 
94  // set wapoint TOD (top of decent)
95  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
96  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
97  WaypointAlt(_tod) = WaypointAlt(_af);
98 
99  // calculate ground speed on final (target_speed - head wind)
100  struct FloatVect2 *wind = stateGetHorizontalWindspeed_f();
101  float wind_norm = sqrtf(wind->x * wind->x + wind->y * wind->y);
102  float wind_on_final = wind_norm * (((td_af_x * wind->y) / (td_af * wind_norm)) +
103  ((td_af_y * wind->x) / (td_af * wind_norm)));
104  Bound(wind_on_final, -MAX_WIND_ON_FINAL, MAX_WIND_ON_FINAL);
105  gs_on_final = target_speed - wind_on_final;
106 
107  // calculate position of SD (start decent)
108  float t_sd_intercept = (gs_on_final * tanf(app_angle)) / app_intercept_rate; //time
109  sd_intercept = gs_on_final * t_sd_intercept; // distance
110  sd_tod = 0.5 * sd_intercept;
111 
112  // set waypoint SD (start decent)
113  WaypointX(_sd) = WaypointX(_tod) + td_af_x / td_af * sd_tod;
114  WaypointY(_sd) = WaypointY(_tod) + td_af_y / td_af * sd_tod;
115  WaypointAlt(_sd) = WaypointAlt(_af);
116 
117  // calculate td_sd
118  float td_sd_x = WaypointX(_sd) - WaypointX(_td);
119  float td_sd_y = WaypointY(_sd) - WaypointY(_td);
120  float td_sd = sqrtf(td_sd_x * td_sd_x + td_sd_y * td_sd_y);
121 
122  // calculate sd_tod in x,y
123  sd_tod_x = WaypointX(_tod) - WaypointX(_sd);
124  sd_tod_y = WaypointY(_tod) - WaypointY(_sd);
125 
126  // set Waypoint AF at least befor SD
127  if ((td_sd + app_distance_af_sd) > td_af) {
128  WaypointX(_af) = WaypointX(_sd) + td_af_x / td_af * app_distance_af_sd;
129  WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd;
130  }
131  return false;
132 } /* end of gls_copute_TOD */
133 
134 
135 bool gls_start(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
136 {
137 
138  init = true;
139 
140  //struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
141  //float wind_additional = sqrtf(wind->x*wind->x + wind->y*wind->y); // should be gusts only!
142  //Bound(wind_additional, 0, 0.5);
143  //target_speed = STALL_AIRSPEED * 1.3 + wind_additional; FIXME
144  target_speed = APP_TARGET_SPEED; // ok for now!
145 
150  Bound(app_distance_af_sd, 0, 200);
151 
152  // calculate Top Of Decent
153  gls_compute_TOD(_af, _sd, _tod, _td);
154 
155  return false;
156 } /* end of gls_init() */
157 
158 
159 bool gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
160 {
161 
162 
163  // set target speed for approach on final
164  if (init) {
165 #if USE_AIRSPEED
167 #endif
168  init = false;
169  }
170 
171  // calculate distance tod_td
172  float final_x = WaypointX(_td) - WaypointX(_tod);
173  float final_y = WaypointY(_td) - WaypointY(_tod);
174  float final2 = Max(final_x * final_x + final_y * final_y, 1.);
175 
176  struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
177  float hspeed = stateGetHorizontalSpeedNorm_f();
178 
179  float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
180  (pos_enu->y - WaypointY(_tod)) * final_y) / final2;
181  Bound(nav_final_progress, -1, 1);
182  // float nav_final_length = sqrtf(final2);
183 
184  // calculate requiered decent rate on glideslope
185  float pre_climb_glideslope = hspeed * (-tanf(app_angle));
186 
187  // calculate glideslope
188  float start_alt = WaypointAlt(_tod);
189  float diff_alt = WaypointAlt(_td) - start_alt;
190  float alt_glideslope = start_alt + nav_final_progress * diff_alt;
191 
192  // calculate intercept
193  float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2 * sd_tod_x +
194  (pos_enu->y - WaypointY(_sd)) * 2 * sd_tod_y) /
195  Max((sd_intercept * sd_intercept), 1.);
196  Bound(nav_intercept_progress, -1, 1);
197  float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
198  float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
199  float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
200 
201  //########################################################
202 
203  // handle the different vertical approach segments
204 
205  float pre_climb = 0.;
206  float alt = 0.;
207 
208  // distance
209  float f_af = sqrtf((pos_enu->x - WaypointX(_af)) * (pos_enu->x - WaypointX(_af)) +
210  (pos_enu->y - WaypointY(_af)) * (pos_enu->y - WaypointY(_af)));
211 
212  if (f_af < app_distance_af_sd) { // approach fix (AF) to start descent (SD)
213  alt = WaypointAlt(_af);
214  pre_climb = 0.;
215  } else if ((f_af > app_distance_af_sd) && (f_af < (app_distance_af_sd + sd_intercept))) {
216  // start descent (SD) to intercept
217  alt = alt_intercept;
218  pre_climb = pre_climb_intercept;
219  } else { //glideslope (intercept to touch down)
220  alt = alt_glideslope;
221  pre_climb = pre_climb_glideslope;
222  }
223  // Bound(pre_climb, -5, 0.);
224 
225 
226  //######################### autopilot modes
227 
228  NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
229  NavVerticalAutoThrottleMode(0); // throttle mode
230  NavSegment(_af, _td); // horizontal mode (stay on localiser)
231 
232  return true;
233 } // end of gls()
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
vector in East North Up coordinates Units: meters
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:121
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
#define Max(x, y)
Definition: main_fbw.c:53
#define WaypointY(_wp)
Definition: common_nav.h:46
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
static struct FloatVect2 * stateGetHorizontalWindspeed_f(void)
Get horizontal windspeed (float).
Definition: state.h:1377
float y
in meters