Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gls.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2012, Tobias M√ľnch
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
47 #include "generated/airframe.h"
48 #include "state.h"
50 #include "subsystems/nav.h"
51 #include "generated/flight_plan.h"
52 
54 float app_angle;
57 
58 bool_t init = TRUE;
59 
60 #ifndef APP_TARGET_SPEED
61 #define APP_TARGET_SPEED NOMINAL_AIRSPEED
62 #endif
63 #define MAX_WIND_ON_FINAL 0.8*APP_TARGET_SPEED
64 
65 #ifndef APP_ANGLE
66 #define APP_ANGLE RadOfDeg(5)
67 #endif
68 
69 #ifndef APP_INTERCEPT_RATE
70 #define APP_INTERCEPT_RATE 0.625 // 4s from start decent until intercept with pre_climb = -2.5 m/s
71 #endif
72 
73 #ifndef APP_DISTANCE_AF_SD
74 #define APP_DISTANCE_AF_SD 100
75 #endif
76 
78 float sd_tod;
80 float sd_tod_x;
81 float sd_tod_y;
82 
83 static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td) {
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 = sqrt( 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 = sqrt(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 = sqrt( 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_t gls_init(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
136 
137  init = TRUE;
138 
139 #if USE_AIRSPEED
140  //struct FloatVect2* wind = stateGetHorizontalWindspeed_f();
141  //float wind_additional = sqrt(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 #endif
146 
151  Bound(app_distance_af_sd,0,200);
152 
153  // calculate Top Of Decent
154  gls_compute_TOD(_af, _sd, _tod, _td);
155 
156  return FALSE;
157 } /* end of gls_init() */
158 
159 
160 bool_t gls(uint8_t _af,uint8_t _sd, uint8_t _tod, uint8_t _td) {
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 = sqrt(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) /
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 = sqrt((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  }
216  else if((f_af>app_distance_af_sd) && (f_af<(app_distance_af_sd+sd_intercept))){
217  // start descent (SD) to intercept
218  alt = alt_intercept;
219  pre_climb = pre_climb_intercept;
220  }
221  else{ //glideslope (intercept to touch down)
222  alt = alt_glideslope;
223  pre_climb = pre_climb_glideslope;
224  }
225  // Bound(pre_climb, -5, 0.);
226 
227 
228  //######################### autopilot modes
229 
230  NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
231  NavVerticalAutoThrottleMode(0); // throttle mode
232  NavSegment(_af, _td); // horizontal mode (stay on localiser)
233 
234  return TRUE;
235 } // end of gls()
static bool_t gls_compute_TOD(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
Definition: gls.c:83
gps landing system
bool_t gls_init(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
Definition: gls.c:135
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854
bool_t gls(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
Definition: gls.c:160
float sd_tod
Definition: gls.c:78
float app_angle
Definition: gls.c:54
float gs_on_final
Definition: gls.c:77
float app_intercept_rate
Definition: gls.c:55
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
float sd_tod_x
Definition: gls.c:80
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
#define FALSE
Definition: imu_chimu.h:141
float sd_intercept
Definition: gls.c:79
float app_distance_af_sd
Definition: gls.c:56
float target_speed
Definition: gls.c:53
#define Max(x, y)
float x
in meters
#define APP_INTERCEPT_RATE
Definition: gls.c:70
#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
#define APP_TARGET_SPEED
Definition: gls.c:61
#define MAX_WIND_ON_FINAL
Definition: gls.c:63
#define APP_DISTANCE_AF_SD
Definition: gls.c:74
vector in East North Up coordinates Units: meters
float sd_tod_y
Definition: gls.c:81
#define APP_ANGLE
Definition: gls.c:66
bool_t init
Definition: gls.c:58