Paparazzi UAS  v5.15_devel-59-g8c145b7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_catapult.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012, Christophe De Wagter
3  * Copyright (C) 2016, Gautier Hattenberger <gautier.hattenberger@enac.fr>
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, see
19  * <http://www.gnu.org/licenses/>.
20  *
21  */
22 
37 
38 #include "state.h"
39 #include "generated/airframe.h"
41 #include "autopilot.h"
43 
44 #include "mcu_periph/uart.h"
45 #include "pprzlink/messages.h"
47 
48 
49 // Default values for take-off procedure
50 
51 #ifndef NAV_CATAPULT_ACCELERATION_THRESHOLD
52 #define NAV_CATAPULT_ACCELERATION_THRESHOLD 1.5 // in g
53 #endif
54 
55 #ifndef NAV_CATAPULT_ACCELERATION_DETECTION
56 #define NAV_CATAPULT_ACCELERATION_DETECTION 5 // number of valid measurements for launch detection
57 #endif
58 
59 #ifndef NAV_CATAPULT_MOTOR_DELAY
60 #define NAV_CATAPULT_MOTOR_DELAY 0.75 // in seconds
61 #endif
62 
63 #ifndef NAV_CATAPULT_HEADING_DELAY
64 #define NAV_CATAPULT_HEADING_DELAY 3.0 // in seconds
65 #endif
66 
67 #ifndef NAV_CATAPULT_INITIAL_PITCH
68 #define NAV_CATAPULT_INITIAL_PITCH RadOfDeg(10) // in radians
69 #endif
70 
71 #ifndef NAV_CATAPULT_INITIAL_THROTTLE
72 #define NAV_CATAPULT_INITIAL_THROTTLE 1.0 // [0, 1]
73 #endif
74 
75 #ifndef NAV_CATAPULT_CLIMB_DISTANCE
76 #define NAV_CATAPULT_CLIMB_DISTANCE 300. // distance of the climb point ahead of catapult
77 #endif
78 
79 #ifndef NAV_CATAPULT_TIMEOUT
80 #define NAV_CATAPULT_TIMEOUT 30. // disarm timeout (in seconds)
81 #endif
82 
84 
85 
87 {
88 
90  nav_catapult.timer = 0;
96 
97 }
98 
99 //#############################################################
100 // Code that Runs in a Fast Module
101 
103 {
105  nav_catapult.timer = 0;
106  // nothing more to do
107  return;
108  }
109 
110  // increase timer
112 
113  // wait for acceleration
115 
116  // launch detection filter
118  // several consecutive measurements above threshold
119 #ifndef SITL
120  struct FloatVect3 *accel_ned = (struct FloatVect3 *)stateGetAccelNed_f();
121  struct FloatRMat *ned_to_body = stateGetNedToBodyRMat_f();
122  struct FloatVect3 accel_body;
123  float_rmat_transp_vmult(&accel_body, ned_to_body, accel_ned);
124  if (accel_body.x < nav_catapult.accel_threshold * 9.81) {
125  // accel is low, reset timer
126  nav_catapult.timer = 0;
127  return;
128  }
129 #else
130  if (autopilot.launch != true) {
131  // wait for simulated launch
132  nav_catapult.timer = 0;
133  return;
134  }
135 #endif
136  }
137  // launch was detected: Motor Delay Counter
138  else if (nav_catapult.timer >= nav_catapult.motor_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
139  // turn on motor
141  autopilot.launch = true;
142  // go to next stage
144  }
145  }
146 
147  // reaching timeout and function still running
148  // shuting it down
149  if (nav_catapult.timer > NAV_CATAPULT_TIMEOUT * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
151  nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
152  }
153 }
154 
155 
156 //############################################################
157 // Code that runs in 4Hz Nav
158 
160 {
161  switch (nav_catapult.status) {
162  case NAV_CATAPULT_UNINIT:
163  // start high freq function if not done
164  if (nav_catapult_nav_catapult_highrate_module_status != MODULES_RUN) {
165  nav_catapult_nav_catapult_highrate_module_status = MODULES_START;
166  }
167  // arm catapult
169  break;
170  case NAV_CATAPULT_ARMED:
171  // store initial position
174  nav_catapult.pos.z = stateGetPositionUtm_f()->alt; // useful ?
176  break;
178  // no throttle, zero attitude
179  NavAttitude(RadOfDeg(0.f));
182  // wait for acceleration from high speed function
183  break;
185  // fixed attitude and motor
186  NavAttitude(RadOfDeg(0.f));
189  if (nav_catapult.timer >= nav_catapult.heading_delay * NAV_CATAPULT_HIGHRATE_MODULE_FREQ) {
190  // store heading, move climb waypoint
191  float dir_x = stateGetPositionEnu_f()->x - nav_catapult.pos.x;
192  float dir_y = stateGetPositionEnu_f()->y - nav_catapult.pos.y;
193  float dir_L = sqrtf(dir_x * dir_x + dir_y * dir_y);
194  WaypointX(_climb) = nav_catapult.pos.x + (dir_x / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
195  WaypointY(_climb) = nav_catapult.pos.y + (dir_y / dir_L) * NAV_CATAPULT_CLIMB_DISTANCE;
196  DownlinkSendWpNr(_climb);
197  // next step
199  }
200  break;
202  // normal climb: heading locked by waypoint target
203  NavVerticalAltitudeMode(WaypointAlt(_climb), 0.f); // vertical mode (folow glideslope)
204  NavVerticalAutoThrottleMode(0.f); // throttle mode
205  NavGotoWaypoint(_climb); // horizontal mode (stay on localiser)
206  if (nav_approaching_xy(WaypointX(_climb), WaypointY(_climb), nav_catapult.pos.x, nav_catapult.pos.y, 0.f)) {
207  // reaching climb waypoint, end procedure
209  }
210  break;
211  case NAV_CATAPULT_DISARM:
212  // end procedure
214  nav_catapult_nav_catapult_highrate_module_status = MODULES_STOP;
215  return false;
216  default:
217  return false;
218  }
219 
220  // procedure still running
221  return true;
222 
223 }
224 
bool launch
request launch
Definition: autopilot.h:71
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
#define WaypointAlt(_wp)
waypoint altitude in m above MSL
Definition: common_nav.h:48
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:50
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
Fixed wing horizontal control.
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define WaypointX(_wp)
Definition: common_nav.h:45
float x
in meters
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
#define WaypointY(_wp)
Definition: common_nav.h:46
Core autopilot interface common to all firmwares.
static struct UtmCoor_f * stateGetPositionUtm_f(void)
Get position in UTM coordinates (float).
Definition: state.h:692
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
rotation matrix
#define MAX_PPRZ
Definition: paparazzi.h:8
float y
in meters