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
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
113  struct Int32Vect3 accel_meas_body;
115  if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81))
116 #else
117  if (launch != 1)
118 #endif
119  {
121  }
122  }
123  // Launch was detected: Motor Delay Counter
125  {
126  // Turn on Motor
128  launch = 1;
129  }
130  }
131  else
132  {
134  }
135 }
136 
137 //###############################################################################################
138 // Code that runs in 4Hz Nav
139 
140 bool_t nav_catapult_init(void)
141 {
142 
145 
146  return FALSE;
147 }
148 
149 
150 
151 bool_t nav_catapult(uint8_t _to, uint8_t _climb)
152 {
153  float alt = WaypointAlt(_climb);
154 
155  nav_catapult_armed = 1;
156 
157  // No Roll, Climb Pitch, No motor Phase
159  {
160  NavAttitude(RadOfDeg(0));
162  NavVerticalThrottleMode(9600*(0));
163 
164 
165 
166  // Store take-off waypoint
167  WaypointX(_to) = GetPosX();
168  WaypointY(_to) = GetPosY();
169  WaypointAlt(_to) = GetPosAlt();
170 
173 
174  }
175  // No Roll, Climb Pitch, Full Power
177  {
178  NavAttitude(RadOfDeg(0));
181  }
182  // Normal Climb: Heading Locked by Waypoint Target
183  else if (nav_catapult_launch == 0xffff)
184  {
185  NavVerticalAltitudeMode(alt, 0); // vertical mode (folow glideslope)
186  NavVerticalAutoThrottleMode(0); // throttle mode
187  NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
188  }
189  else
190  {
191  // Store Heading, move Climb
192  nav_catapult_launch = 0xffff;
193 
194  float dir_x = estimator_x - nav_catapult_x;
195  float dir_y = estimator_y - nav_catapult_y;
196 
197  float dir_L = sqrt(dir_x * dir_x + dir_y * dir_y);
198 
199  WaypointX(_climb) = nav_catapult_x + (dir_x / dir_L) * 300;
200  WaypointY(_climb) = nav_catapult_y + (dir_y / dir_L) * 300;
201 
203  }
204 
205 
206 return TRUE;
207 
208 } // end of gls()
209 
211 {
212  WaypointX(_td) = GetPosX();
213  WaypointY(_td) = GetPosY();
214  WaypointAlt(_td) = GetPosAlt();
215  return FALSE;
216 }
217 
218 
219 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool_t launch
Definition: sim_ap.c:41
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
#define GetPosY()
Definition: estimator.h:96
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
float estimator_y
north position in meters
Definition: estimator.c:43
#define FALSE
Definition: imu_chimu.h:141
#define GetPosX()
Definition: estimator.h:95
#define GetPosAlt()
Definition: estimator.h:97
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