Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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"
44 #include "generated/flight_plan.h"
47 
48 // Imu is required
49 #include "subsystems/imu.h"
50 
51 #include "mcu_periph/uart.h"
52 #include "messages.h"
54 
55 
56 static bool_t nav_catapult_armed = FALSE;
58 
59 #ifndef NAV_CATAPULT_ACCELERATION_THRESHOLD
60 #define NAV_CATAPULT_ACCELERATION_THRESHOLD 1.5
61 #endif
62 
64 
65 #ifndef NAV_CATAPULT_MOTOR_DELAY
66 #define NAV_CATAPULT_MOTOR_DELAY 0.75 // seconds
67 #endif
68 
70 
71 #ifndef NAV_CATAPULT_HEADING_DELAY
72 #define NAV_CATAPULT_HEADING_DELAY 3.0 // seconds
73 #endif
74 
76 
77 #ifndef NAV_CATAPULT_INITIAL_PITCH
78 #define NAV_CATAPULT_INITIAL_PITCH RadOfDeg(10)
79 #endif
80 
82 
83 #ifndef NAV_CATAPULT_INITIAL_THROTTLE
84 #define NAV_CATAPULT_INITIAL_THROTTLE 1.0
85 #endif
86 
88 
90 
91 static float nav_catapult_x = 0;
92 static float nav_catapult_y = 0;
93 
94 //###############################################################################################
95 // Code that Runs in a Fast Module
96 
98 {
99  // Only run when
100  if (nav_catapult_armed)
101  {
102  if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
104  }
105 
106  // Launch detection Filter
107  if (nav_catapult_launch < 5)
108  {
109  // Five consecutive measurements > 1.5
110 #ifndef SITL
111  struct Int32Vect3 accel_meas_body;
112  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
113  INT32_RMAT_TRANSP_VMULT(accel_meas_body, *body_to_imu_rmat, imu.accel);
114  if (ACCEL_FLOAT_OF_BFP(accel_meas_body.x) < (nav_catapult_acceleration_threshold * 9.81))
115 #else
116  if (launch != 1)
117 #endif
118  {
120  }
121  }
122  // Launch was detected: Motor Delay Counter
123  else if (nav_catapult_launch >= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
124  {
125  // Turn on Motor
127  launch = 1;
128  }
129  }
130  else
131  {
133  }
134 }
135 
136 //###############################################################################################
137 // Code that runs in 4Hz Nav
138 
139 bool_t nav_catapult_setup(void)
140 {
141 
144 
145  return FALSE;
146 }
147 
148 
149 
150 bool_t nav_catapult_run(uint8_t _to, uint8_t _climb)
151 {
152  float alt = WaypointAlt(_climb);
153 
154  nav_catapult_armed = 1;
155 
156  // No Roll, Climb Pitch, No motor Phase
157  if (nav_catapult_launch <= nav_catapult_motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
158  {
159  NavAttitude(RadOfDeg(0));
161  NavVerticalThrottleMode(9600*(0));
162 
163 
166 
167  // Store take-off waypoint
168  WaypointX(_to) = nav_catapult_x;
169  WaypointY(_to) = nav_catapult_y;
171 
172  }
173  // No Roll, Climb Pitch, Full Power
174  else if (nav_catapult_launch < nav_catapult_heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ)
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 = stateGetPositionEnu_f()->x - nav_catapult_x;
193  float dir_y = stateGetPositionEnu_f()->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 }
207 
209 {
213  return FALSE;
214 }
215 
unsigned short uint16_t
Definition: types.h:16
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
bool_t launch
Definition: sim_ap.c:40
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
#define INT32_RMAT_TRANSP_VMULT(_vb, _m_b2a, _va)
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
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:47
float x
in meters
#define ACCEL_FLOAT_OF_BFP(_ai)
Inertial Measurement Unit interface.
#define TRUE
Definition: imu_chimu.h:144
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:651
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
rotation matrix
float alt
in meters above WGS84 reference ellipsoid
Fixedwing autopilot modes.