Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
takeoff_detect.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5 
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
30 #include "std.h"
32 #include "autopilot.h"
33 #include "state.h"
34 #include "generated/modules.h" // for status and function's period
35 
37 #ifndef TAKEOFF_DETECT_LAUNCH_PITCH
38 #define TAKEOFF_DETECT_LAUNCH_PITCH RadOfDeg(30.)
39 #endif
40 
42 #ifndef TAKEOFF_DETECT_ABORT_PITCH
43 #define TAKEOFF_DETECT_ABORT_PITCH RadOfDeg(-20.)
44 #endif
45 
47 #ifndef TAKEOFF_DETECT_TIMER
48 #define TAKEOFF_DETECT_TIMER 2.
49 #endif
50 
52 #ifndef TAKEOFF_DETECT_DISABLE_TIMER
53 #define TAKEOFF_DETECT_DISABLE_TIMER 4.
54 #endif
55 
61 };
62 
67 };
68 
70 
71 // Init
73 {
74  // variable init is done in start function
75 }
76 
77 // Start
79 {
80  takeoff_detect.state = TO_DETECT_ARMED; // always start periodic with ARMED state
81  takeoff_detect.timer = 0; // and reset timer
82 }
83 
84 // Periodic
86 {
87  // Run detection state machine here
88  switch (takeoff_detect.state) {
89  case TO_DETECT_ARMED:
90  // test for "nose up" + AP in AUTO2, optionally AUTO1 not default since risky if one does not know what it does
92  {
93 #ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
95 #else
97 #endif
98  {
100  }
101  }
102  else {
103  // else reset timer
104  takeoff_detect.timer = 0;
105  }
106  // if timer is finished, start launching
107  if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_TIMER)) {
108  autopilot.launch = true;
110  takeoff_detect.timer = 0;
111  }
112  break;
113 
114  case TO_DETECT_LAUNCHING:
115  // abort if pitch goes below threshold while launching
117  {
118 #ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
120 #else
122 #endif
123  {
124  // back to ARMED state
125  autopilot.launch = false;
127  }
128  }
129  // increment timer and disable detection after some time
131  if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_DISABLE_TIMER)) {
133  }
134  break;
135 
136  case TO_DETECT_DISABLED:
137  // stop periodic call
138  takeoff_detect_takeoff_detect_periodic_status = MODULES_STOP;
139  break;
140 
141  default:
142  // No kidding ?!
144  break;
145  }
146 }
147 
148 
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:222
struct pprz_autopilot autopilot
Global autopilot structure.
Definition: autopilot.c:49
Core autopilot interface common to all firmwares.
bool launch
request launch
Definition: autopilot.h:71
#define AP_MODE_AUTO2
#define AP_MODE_AUTO1
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
API to get/set the generic vehicle states.
void takeoff_detect_periodic(void)
Periodic call.
enum takeoff_detect_state state
#define TAKEOFF_DETECT_LAUNCH_PITCH
Default pitch angle to trigger launch.
#define TAKEOFF_DETECT_DISABLE_TIMER
Disable timer in seconds.
static struct takeoff_detect_struct takeoff_detect
void takeoff_detect_start(void)
Start function called once before periodic.
#define TAKEOFF_DETECT_TIMER
Detection timer in seconds.
#define TAKEOFF_DETECT_ABORT_PITCH
Default pitch angle to cancel launch.
takeoff_detect_state
Takeoff detection states.
@ TO_DETECT_LAUNCHING
@ TO_DETECT_DISABLED
@ TO_DETECT_ARMED
void takeoff_detect_init(void)
Init function.
Takeoff detection structure.
Automatic takeoff assistance for fixed-wing.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78