Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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  */
23 
47 #include "generated/airframe.h"
48 #include "estimator.h"
50 #include "subsystems/nav.h"
51 #include "generated/flight_plan.h"
52 
53 
54 
56 float app_angle;
58 
59 bool_t init = TRUE;
60 
61 #ifndef APP_TARGET_SPEED
62 #define APP_TARGET_SPEED V_CTL_AUTO_AIRSPEED_SETPOINT;
63 #endif
64 
65 #ifndef APP_ANGLE
66 #define APP_ANGLE RadOfDeg(5);
67 #endif
68 
69 #ifndef APP_INTERCEPT_AF_TOD
70 #define APP_INTERCEPT_AF_TOD 100
71 #endif
72 
73 
74 static inline bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td) {
75 
76  if ((WaypointX(_af)==WaypointX(_td))&&(WaypointY(_af)==WaypointY(_td))){
77  WaypointX(_af)=WaypointX(_td)-1;
78  }
79 
80  float td_af_x = WaypointX(_af) - WaypointX(_td);
81  float td_af_y = WaypointY(_af) - WaypointY(_td);
82  float td_af = sqrt( td_af_x*td_af_x + td_af_y*td_af_y);
83  float td_tod = (WaypointAlt(_af) - WaypointAlt(_td)) / (tan(app_angle));
84 
85  WaypointX(_tod) = WaypointX(_td) + td_af_x / td_af * td_tod;
86  WaypointY(_tod) = WaypointY(_td) + td_af_y / td_af * td_tod;
87  WaypointAlt(_tod) = WaypointAlt(_af);
88 
89  if (td_tod > td_af) {
90  WaypointX(_af) = WaypointX(_tod) + td_af_x / td_af * app_intercept_af_tod;
91  WaypointY(_af) = WaypointY(_tod) + td_af_y / td_af * app_intercept_af_tod;
92  }
93  return FALSE;
94 } /* end of gls_copute_TOD */
95 
96 
97 //###############################################################################################
98 
99 bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td) {
100 
101  init = TRUE;
102 
103 #if USE_AIRSPEED
104  //float wind_additional = sqrt(wind_east*wind_east + wind_north*wind_north); // should be gusts only!
105  //Bound(wind_additional, 0, 0.5);
106  //target_speed = FL_ENVE_V_S * 1.3 + wind_additional; FIXME
107  target_speed = APP_TARGET_SPEED; // ok for now!
108 #endif
109 
112  Bound(app_intercept_af_tod,0,200);
113 
114 
115  gls_compute_TOD(_af, _tod, _td); // calculate Top Of Decent
116 
117  return FALSE;
118 } /* end of gls_init() */
119 
120 
121 //###############################################################################################
122 
123 
124 bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td) {
125 
126 
127  if (init){
128 
129 #if USE_AIRSPEED
130  v_ctl_auto_airspeed_setpoint = target_speed; // set target speed for approach
131 #endif
132  init = FALSE;
133  }
134 
135 
136  float final_x = WaypointX(_td) - WaypointX(_tod);
137  float final_y = WaypointY(_td) - WaypointY(_tod);
138  float final2 = Max(final_x * final_x + final_y * final_y, 1.);
139 
140  float nav_final_progress = ((estimator_x - WaypointX(_tod)) * final_x + (estimator_y - WaypointY(_tod)) * final_y) / final2;
141  Bound(nav_final_progress,-1,1);
142  float nav_final_length = sqrt(final2);
143 
144  float pre_climb = -(WaypointAlt(_tod) - WaypointAlt(_td)) / (nav_final_length / estimator_hspeed_mod);
145  Bound(pre_climb, -5, 0.);
146 
147  float start_alt = WaypointAlt(_tod);
148  float diff_alt = WaypointAlt(_td) - start_alt;
149  float alt = start_alt + nav_final_progress * diff_alt;
150  Bound(alt, WaypointAlt(_td), start_alt +(pre_climb/(-v_ctl_altitude_pgain))) // to prevent climbing before intercept
151 
152 
153 
154 
155  if(nav_final_progress < -0.5) { // for smooth intercept
156 
157  NavVerticalAltitudeMode(WaypointAlt(_tod), 0); // vertical mode (fly straigt and intercept glideslope)
158 
159  NavVerticalAutoThrottleMode(0); // throttle mode
160 
161  NavSegment(_af, _td); // horizontal mode (stay on localiser)
162  }
163 
164  else {
165 
166  NavVerticalAltitudeMode(alt, pre_climb); // vertical mode (folow glideslope)
167 
168  NavVerticalAutoThrottleMode(0); // throttle mode
169 
170  NavSegment(_af, _td); // horizontal mode (stay on localiser)
171  }
172 
173 
174 return TRUE;
175 
176 } // end of gls()
float estimator_hspeed_mod
module of horizontal ground speed in m/s
Definition: estimator.c:64
gps landing system
float estimator_y
north position in meters
Definition: estimator.c:43
float app_angle
Definition: gls.c:56
static bool_t gls_compute_TOD(uint8_t _af, uint8_t _tod, uint8_t _td)
Definition: gls.c:74
float v_ctl_auto_airspeed_setpoint
in meters per second
Definition: energy_ctrl.c:124
#define FALSE
Definition: imu_chimu.h:141
float target_speed
Definition: gls.c:55
#define Max(x, y)
float estimator_x
east position in meters
Definition: estimator.c:42
float v_ctl_altitude_pgain
Definition: energy_ctrl.c:93
#define TRUE
Definition: imu_chimu.h:144
float app_intercept_af_tod
Definition: gls.c:57
unsigned char uint8_t
Definition: types.h:14
bool_t gls(uint8_t _af, uint8_t _tod, uint8_t _td)
Definition: gls.c:124
#define APP_TARGET_SPEED
Definition: gls.c:62
bool_t gls_init(uint8_t _af, uint8_t _tod, uint8_t _td)
Definition: gls.c:99
State estimation, fusioning sensors.
#define APP_ANGLE
Definition: gls.c:66
#define APP_INTERCEPT_AF_TOD
Definition: gls.c:70
bool_t init
Definition: gls.c:59