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_skid_landing.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2016, Michal Podhradsky, Thomas Fisher
4  *
5  * AggieAir, Utah State University
6  *
7  * This file is part of paparazzi.
8  *
9  * paparazzi is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2, or (at your option)
12  * any later version.
13  *
14  * paparazzi is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with paparazzi; see the file COPYING. If not, write to
21  * the Free Software Foundation, 59 Temple Place - Suite 330,
22  * Boston, MA 02111-1307, USA.
23  *
24  */
25 
57 #include "generated/airframe.h"
58 #include "state.h"
60 #include "autopilot.h"
63 
65 {
67 };
71 static float land_radius;
72 static struct FloatVect2 land_circle;
73 static float land_app_alt;
74 static float land_circle_quadrant;
75 static float approach_quadrant;
76 static float final_land_altitude;
78 
79 #ifndef SKID_LANDING_AF_HEIGHT //> AF height [m]
80 #define SKID_LANDING_AF_HEIGHT 50
81 #endif
82 #ifndef SKID_LANDING_FINAL_HEIGHT //> final height [m]
83 #define SKID_LANDING_FINAL_HEIGHT 5
84 #endif
85 #ifndef SKID_LANDING_FINAL_STAGE_TIME //> final stage time [s]
86 #define SKID_LANDING_FINAL_STAGE_TIME 5
87 #endif
88 
89 static inline float distance_equation(struct FloatVect2 p1,struct FloatVect2 p2)
90 {
91  return sqrtf((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y));
92 }
93 
94 void nav_skid_landing_setup(uint8_t afwp, uint8_t tdwp, float radius)
95 {
96  aw_waypoint = afwp;
97  td_waypoint = tdwp;
99  land_radius = radius;
102  final_land_count = 1;
103 
104  float x_0 = waypoints[td_waypoint].x - waypoints[aw_waypoint].x;
105  float y_0 = waypoints[td_waypoint].y - waypoints[aw_waypoint].y;
106 
107  /* Unit vector from AF to TD */
108  float d = sqrtf(x_0 * x_0 + y_0 * y_0);
109  float x_1 = x_0 / d;
110  float y_1 = y_0 / d;
111 
114 
117 
118  if (land_radius > 0) {
119  approach_quadrant = land_circle_quadrant - RadOfDeg(90);
121  } else {
122  approach_quadrant = land_circle_quadrant + RadOfDeg(90);
124  }
125 }
126 
128 {
129  switch (skid_landing_status) {
130  case CircleDown:
131  NavVerticalAutoThrottleMode(0); // No pitch
134 
135  if (stateGetPositionUtm_f()->alt < waypoints[aw_waypoint].a + 5) {
137  nav_init_stage();
138  }
139 
140  break;
141 
142  case LandingWait:
143  NavVerticalAutoThrottleMode(0); // No pitch
146 
147  if (NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(approach_quadrant))) {
149  nav_init_stage();
150  }
151  break;
152 
153  case Approach:
154  NavVerticalAutoThrottleMode(0); // No pitch
157 
158  if (NavQdrCloseTo(DegOfRad(land_circle_quadrant))) {
160  nav_init_stage();
161  }
162  break;
163 
164  case Final:
166  < v_ctl_landing_alt_throttle_kill) {
168  }
170  if (!autopilot_throttle_killed()) {
173  }
174  break;
175 
176  default:
177  break;
178  }
179  return TRUE;
180 }
181 
183 {
184  struct FloatVect2 p_from;
185  struct FloatVect2 p_to;
186  struct FloatVect2 now;
187 
188  float start_alt = waypoints[From_WP].a;
189  float diff_alt = waypoints[To_WP].a - start_alt;
190  p_from.x = waypoints[From_WP].x;
191  p_from.y = waypoints[From_WP].y;
192  p_to.x = waypoints[To_WP].x;
193  p_to.y = waypoints[To_WP].y;
194  now.x = stateGetPositionEnu_f()->x;
195  now.y = stateGetPositionEnu_f()->y;
196 
197  float end_dist = distance_equation(p_from,p_to);
198  float here_dist = distance_equation(p_from,now);
199  float end_here_dist = distance_equation(p_to,now);
200  float alt = start_alt + (here_dist/end_dist) * diff_alt;
201 
202  if (end_dist < end_here_dist){
203  alt = start_alt;
204  }
206 
207  if ((stateGetPositionUtm_f()->alt-waypoints[To_WP].a)<0.0){
208  alt = waypoints[To_WP].a;
209  }
210  nav_altitude = alt;
211 }
float x
Definition: common_nav.h:40
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:230
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
Fixed wing horizontal control.
#define TRUE
Definition: std.h:4
float y
Definition: common_nav.h:41
float x
in meters
void autopilot_set_kill_throttle(bool kill)
set kill throttle
Definition: autopilot.c:219
uint8_t v_ctl_mode
Definition: energy_ctrl.c:74
Core autopilot interface common to all firmwares.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition: common_nav.c:38
float a
Definition: common_nav.h:42
float y
in meters
#define V_CTL_MODE_LANDING