Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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_t 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_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) {
83 
84  if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
85  WaypointX(_af)=WaypointX(_td)-1;
86  }
87 
88  float td_af_x = WaypointX(_af) - WaypointX(_td);
89  float td_af_y = WaypointY(_af) - WaypointY(_td);
90  float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
91  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tanf(app_angle));
92 
93  // set wapoint TOD (top of decent)
94  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
95  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
96  WaypointAlt(_tod) = WaypointAlt(_af);
97 
98  // calculate ground speed on final (target_speed - head wind)
100  float wind_norm = sqrt(wind->x*wind->x + wind->y*wind->y);
101  float wind_on_final = wind_norm * (((td_af_x*wind->y)/(td_af*wind_norm)) +
102  ((td_af_y*wind->x)/(td_af*wind_norm)));
103  Bound(wind_on_final,-MAX_WIND_ON_FINAL,MAX_WIND_ON_FINAL);
104  gs_on_final = target_speed - wind_on_final;
105 
106  // calculate position of SD (start decent)
107  float t_sd_intercept = (gs_on_final * tanf(app_angle)) / app_intercept_rate; //time
108  sd_intercept = gs_on_final * t_sd_intercept; // distance
109  sd_tod = 0.5 * sd_intercept;
110 
111  // set waypoint SD (start decent)
112  WaypointX(_sd) = WaypointX(_tod) + td_af_x / td_af * sd_tod;
113  WaypointY(_sd) = WaypointY(_tod) + td_af_y / td_af * sd_tod;
114  WaypointAlt(_sd) = WaypointAlt(_af);
115 
116  // calculate td_sd
117  float td_sd_x = WaypointX(_sd) - WaypointX(_td);
118  float td_sd_y = WaypointY(_sd) - WaypointY(_td);
119  float td_sd = sqrt( td_sd_x*td_sd_x + td_sd_y*td_sd_y);
120 
121  // calculate sd_tod in x,y
122  sd_tod_x = WaypointX(_tod) - WaypointX(_sd);
123  sd_tod_y = WaypointY(_tod) - WaypointY(_sd);
124 
125  // set Waypoint AF at least befor SD
126  if ((td_sd + app_distance_af_sd) > td_af) {
127  WaypointX(_af) = WaypointX(_sd) + td_af_x / td_af * app_distance_af_sd;
128  WaypointY(_af) = WaypointY(_sd) + td_af_y / td_af * app_distance_af_sd;
129  }
130  return FALSE;
131 } /* end of gls_copute_TOD */
132 
133 
134 bool_t gls_start(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
135 
136  init = TRUE;
137 
138  //struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
139  //float wind_additional = sqrt(wind->x*wind->x + wind->y*wind->y); // should be gusts only!
140  //Bound(wind_additional, 0, 0.5);
141  //target_speed = STALL_AIRSPEED * 1.3 + wind_additional; FIXME
142  target_speed = APP_TARGET_SPEED; // ok for now!
143 
148  Bound(app_distance_af_sd,0,200);
149 
150  // calculate Top Of Decent
151  gls_compute_TOD(_af, _sd, _tod, _td);
152 
153  return FALSE;
154 } /* end of gls_init() */
155 
156 
157 bool_t gls_run(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
158 
159 
160  // set target speed for approach on final
161  if (init){
162 #if USE_AIRSPEED
164 #endif
165  init = FALSE;
166  }
167 
168  // calculate distance tod_td
169  float final_x = WaypointX(_td) - WaypointX(_tod);
170  float final_y = WaypointY(_td) - WaypointY(_tod);
171  float final2 = Max(final_x * final_x + final_y * final_y, 1.);
172 
173  struct EnuCoor_f* pos_enu = stateGetPositionEnu_f();
174  float hspeed = *stateGetHorizontalSpeedNorm_f();
175 
176  float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
177  (pos_enu->y - WaypointY(_tod)) * final_y) / final2;
178  Bound(nav_final_progress,-1,1);
179  // float nav_final_length = sqrt(final2);
180 
181  // calculate requiered decent rate on glideslope
182  float pre_climb_glideslope = hspeed * (-tanf(app_angle));
183 
184  // calculate glideslope
185  float start_alt = WaypointAlt(_tod);
186  float diff_alt = WaypointAlt(_td) - start_alt;
187  float alt_glideslope = start_alt + nav_final_progress * diff_alt;
188 
189  // calculate intercept
190  float nav_intercept_progress = ((pos_enu->x - WaypointX(_sd)) * 2*sd_tod_x +
191  (pos_enu->y - WaypointY(_sd)) * 2*sd_tod_y) /
193  Bound(nav_intercept_progress,-1,1);
194  float tmp = nav_intercept_progress * sd_intercept / gs_on_final;
195  float alt_intercept = WaypointAlt(_tod) - (0.5 * app_intercept_rate * tmp * tmp);
196  float pre_climb_intercept = -nav_intercept_progress * hspeed * (tanf(app_angle));
197 
198  //########################################################
199 
200  // handle the different vertical approach segments
201 
202  float pre_climb = 0.;
203  float alt = 0.;
204 
205  // distance
206  float f_af = sqrt((pos_enu->x - WaypointX(_af))*(pos_enu->x - WaypointX(_af))+
207  (pos_enu->y - WaypointY(_af))*(pos_enu->y - WaypointY(_af)));
208 
209  if(f_af<app_distance_af_sd){ // approach fix (AF) to start descent (SD)
210  alt = WaypointAlt(_af);
211  pre_climb = 0.;
212  }
213  else if((f_af>app_distance_af_sd) && (f_af<(app_distance_af_sd+sd_intercept))){
214  // start descent (SD) to intercept
215  alt = alt_intercept;
216  pre_climb = pre_climb_intercept;
217  }
218  else{ //glideslope (intercept to touch down)
219  alt = alt_glideslope;
220  pre_climb = pre_climb_glideslope;
221  }
222  // Bound(pre_climb, -5, 0.);
223 
224 
225  //######################### autopilot modes
226 
227  NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
228  NavVerticalAutoThrottleMode(0); // throttle mode
229  NavSegment(_af, _td); // horizontal mode (stay on localiser)
230 
231  return TRUE;
232 } // end of gls()
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
#define FALSE
Definition: imu_chimu.h:141
#define Max(x, y)
float x
in meters
#define TRUE
Definition: imu_chimu.h:144
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:1192
vector in East North Up coordinates Units: meters