Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
56
57#include "generated/airframe.h"
58#include "state.h"
60#include "autopilot.h"
63
71static float land_radius;
72static struct FloatVect2 land_circle;
73static float land_app_alt;
75static float approach_quadrant;
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
89static 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
94void 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;
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) {
121 } else {
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) {
138 }
139
140 break;
141
142 case LandingWait:
143 NavVerticalAutoThrottleMode(0); // No pitch
146
147 if (NavCircleCount() > 0.5 && NavQdrCloseTo(DegOfRad(approach_quadrant))) {
150 }
151 break;
152
153 case Approach:
154 NavVerticalAutoThrottleMode(0); // No pitch
157
158 if (NavQdrCloseTo(DegOfRad(land_circle_quadrant))) {
161 }
162 break;
163
164 case Final:
166 < v_ctl_landing_alt_throttle_kill) {
168 }
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}
void autopilot_set_kill_throttle(bool kill)
set kill throttle Kill always kills, also when motors_on is already false (fixedwing in flight).
Definition autopilot.c:312
bool autopilot_throttle_killed(void)
get kill status
Definition autopilot.c:324
Core autopilot interface common to all firmwares.
struct point waypoints[NB_WAYPOINT]
size == nb_waypoint, waypoint 0 is a dummy waypoint
Definition common_nav.c:44
uint8_t v_ctl_mode
Definition energy_ctrl.c:74
Fixed wing horizontal control.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition state.h:832
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
#define V_CTL_MODE_LANDING
void nav_init_stage(void)
needs to be implemented by fixedwing and rotorcraft seperately
Definition nav.c:92
float nav_altitude
Definition nav.c:307
void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y)
Computes the carrot position along the desired segment.
Definition nav.c:382
void nav_circle_XY(float x, float y, float radius)
Navigates around (x, y).
Definition nav.c:108
Fixedwing Navigation library.
#define NavQdrCloseTo(x)
True if x (in degrees) is close to the current QDR (less than 10 degrees).
Definition nav.h:161
#define NavCircleCount()
Definition nav.h:155
#define NavVerticalAltitudeMode(_alt, _pre_climb)
Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.
Definition nav.h:191
#define NavVerticalAutoThrottleMode(_pitch)
Set the climb control to auto-throttle with the specified pitch pre-command.
Definition nav.h:177
static float land_radius
static uint8_t final_land_count
bool nav_skid_landing_run(void)
static enum LandingStatus skid_landing_status
static float final_land_altitude
void nav_skid_landing_setup(uint8_t afwp, uint8_t tdwp, float radius)
static uint8_t td_waypoint
#define SKID_LANDING_FINAL_HEIGHT
static uint8_t aw_waypoint
LandingStatus
@ CircleDown
@ LandingWait
@ Approach
@ Final
void nav_skid_landing_glide(uint8_t From_WP, uint8_t To_WP)
static float distance_equation(struct FloatVect2 p1, struct FloatVect2 p2)
static float land_circle_quadrant
static struct FloatVect2 land_circle
static float approach_quadrant
static float land_app_alt
Landing on skidpads See video of the landing: https://www.youtube.com/watch?v=aYrB7s3oeX4 Standard la...
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.