Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
nav_catapult.c
Go to the documentation of this file.
1 /*
2  *
3  * Copyright (C) 2012, Christophe De Wagter
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 
39 #include "generated/airframe.h"
40 #include "estimator.h"
41 #include "ap_downlink.h"
43 #include "subsystems/nav.h"
44 #include "generated/flight_plan.h"
47 
48 // Imu is required
49 #include "subsystems/imu.h"
50 
51 #ifndef DOWNLINK_DEVICE
52 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
53 #endif
54 #include "mcu_periph/uart.h"
55 #include "messages.h"
57 
58 
59 static bool_t nav_catapult_armed = FALSE;
61 
62 #ifndef NAV_CATAPULT_ACCELERATION_THRESHOLD
63 #define NAV_CATAPULT_ACCELERATION_THRESHOLD 1.5;
64 #endif
65 
67 
68 #ifndef NAV_CATAPULT_MOTOR_DELAY
69 #define NAV_CATAPULT_MOTOR_DELAY 45 // Main Control Loops
70 #endif
71 
73 
74 #ifndef NAV_CATAPULT_HEADING_DELAY
75 #define NAV_CATAPULT_HEADING_DELAY 180
76 #endif
77 
79 
80 #ifndef NAV_CATAPULT_INITIAL_PITCH
81 #define NAV_CATAPULT_INITIAL_PITCH RadOfDeg(10)
82 #endif
83 
85 
86 #ifndef NAV_CATAPULT_INITIAL_THROTTLE
87 #define NAV_CATAPULT_INITIAL_THROTTLE 1.0
88 #endif
89 
91 
93 
94 static float nav_catapult_x = 0;
95 static float nav_catapult_y = 0;
96 
97 //###############################################################################################
98 // Code that Runs in a Fast Module
99 
101 {
102  // Only run when
103  if (nav_catapult_armed)
104  {
107 
108  // Launch detection Filter
109  if (nav_catapult_launch < 5)
110  {
111  // Five consecutive measurements > 1.5
112 #ifndef SITL
114 #else
115  if (launch != 1)
116 #endif
117  {
119  }
120  }
121  // Launch was detected: Motor Delay Counter
123  {
124  // Turn on Motor
126  launch = 1;
127  }
128  }
129  else
130  {
132  }
133 }
134 
135 //###############################################################################################
136 // Code that runs in 4Hz Nav
137 
138 bool_t nav_catapult_init(void)
139 {
140 
143 
144  return FALSE;
145 }
146 
147 
148 
149 bool_t nav_catapult(uint8_t _to, uint8_t _climb)
150 {
151  float alt = WaypointAlt(_climb);
152 
153  nav_catapult_armed = 1;
154 
155  // No Roll, Climb Pitch, No motor Phase
157  {
158  NavAttitude(RadOfDeg(0));
160  NavVerticalThrottleMode(9600*(0));
161 
162 
163 
164  // Store take-off waypoint
165  WaypointX(_to) = GetPosX();
166  WaypointY(_to) = GetPosY();
167  WaypointAlt(_to) = GetPosAlt();
168 
171 
172  }
173  // No Roll, Climb Pitch, Full Power
175  {
176  NavAttitude(RadOfDeg(0));
179  }
180  // Normal Climb: Heading Locked by Waypoint Target
181  else if (nav_catapult_launch == 0xffff)
182  {
183  NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope)
184  NavVerticalAutoThrottleMode(0); // throttle mode
185  NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
186  }
187  else
188  {
189  // Store Heading, move Climb
190  nav_catapult_launch = 0xffff;
191 
192  float dir_x = estimator_x - nav_catapult_x;
193  float dir_y = estimator_y - nav_catapult_y;
194 
195  float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
196 
197  WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
198  WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
199 
201  }
202 
203 
204 return TRUE;
205 
206 } // end of gls()
207 
209 {
210  WaypointX(_td) = GetPosX();
211  WaypointY(_td) = GetPosY();
212  WaypointAlt(_td) = GetPosAlt();
213  return FALSE;
214 }
215 
216 
217 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool_t launch
Definition: sim_ap.c:41
#define GetPosY()
Definition: estimator.h:92
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
float estimator_y
north position in meters
Definition: estimator.c:43
#define FALSE
Definition: imu_chimu.h:141
#define GetPosX()
Definition: estimator.h:91
#define GetPosAlt()
Definition: estimator.h:93
float estimator_x
east position in meters
Definition: estimator.c:42
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
#define TRUE
Definition: imu_chimu.h:144
unsigned char uint8_t
Definition: types.h:14
State estimation, fusioning sensors.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50