Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules 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 "state.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 0.75 // seconds
70 #endif
71 
73 
74 #ifndef NAV_CATAPULT_HEADING_DELAY
75 #define NAV_CATAPULT_HEADING_DELAY 3.0 // seconds
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  {
105  if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
107  }
108 
109  // Launch detection Filter
110  if (nav_catapult_launch < 5)
111  {
112  // Five consecutive measurements > 1.5
113 #ifndef SITL
114  struct Int32Vect3 accel_meas_body;
116  if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81))
117 #else
118  if (launch != 1)
119 #endif
120  {
122  }
123  }
124  // Launch was detected: Motor Delay Counter
125  else if (nav_catapult_launch >= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
126  {
127  // Turn on Motor
129  launch = 1;
130  }
131  }
132  else
133  {
135  }
136 }
137 
138 //###############################################################################################
139 // Code that runs in 4Hz Nav
140 
141 bool_t nav_catapult_init(void)
142 {
143 
146 
147  return FALSE;
148 }
149 
150 
151 
152 bool_t nav_catapult(uint8_t _to, uint8_t _climb)
153 {
154  float alt = WaypointAlt(_climb);
155 
156  nav_catapult_armed = 1;
157 
158  // No Roll, Climb Pitch, No motor Phase
159  if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
160  {
161  NavAttitude(RadOfDeg(0));
163  NavVerticalThrottleMode(9600*(0));
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
176  else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
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 = stateGetPositionEnu_f()->x - nav_catapult_x;
195  float dir_y = stateGetPositionEnu_f()->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 }
209 
211 {
212  WaypointX(_td) = GetPosX();
213  WaypointY(_td) = GetPosY();
214  WaypointAlt(_td) = GetPosAlt();
215  return FALSE;
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:40
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:672
float y
in meters
#define FALSE
Definition: imu_chimu.h:141
Fixed wing horizontal control.
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
float x
in meters
#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
API to get/set the generic vehicle states.
Fixedwing autopilot modes.