Paparazzi UAS  v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_flip.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Freek van Tienen <freek.v.tienen@gmail.com>
3  * 2015 Ewoud Smeur
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 
32 #include "guidance_flip.h"
33 
34 #include "autopilot.h"
35 #include "guidance_h.h"
37 #include "stabilization/stabilization_attitude.h"
38 
39 #ifndef STOP_ROLL_CMD_ANGLE
40 #define STOP_ROLL_CMD_ANGLE 25.0
41 #endif
42 #ifndef FIRST_THRUST_DURATION
43 #define FIRST_THRUST_DURATION 0.3
44 #endif
45 #ifndef FINAL_THRUST_LEVEL
46 #define FINAL_THRUST_LEVEL 6000
47 #endif
48 
55 
57 {
58  flip_counter = 0;
59  flip_state = 0;
60  flip_rollout = false;
63 }
64 
66 {
67  uint32_t timer;
68  int32_t phi;
69  static uint32_t timer_save = 0;
70 
71  timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
73 
74  switch (flip_state) {
75  case 0:
76  flip_cmd_earth.x = 0;
77  flip_cmd_earth.y = 0;
79  heading_save);
81  stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
82  timer_save = 0;
83 
84  if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
85  flip_state++;
86  }
87  break;
88 
89  case 1:
90  stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
91  stabilization_cmd[COMMAND_PITCH] = 0;
92  stabilization_cmd[COMMAND_YAW] = 0;
93  stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
94 
95  if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
96  flip_state++;
97  }
98  break;
99 
100  case 2:
101  stabilization_cmd[COMMAND_ROLL] = 0;
102  stabilization_cmd[COMMAND_PITCH] = 0;
103  stabilization_cmd[COMMAND_YAW] = 0;
104  stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
105 
106  if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
107  timer_save = timer;
108  flip_state++;
109  }
110  break;
111 
112  case 3:
113  flip_cmd_earth.x = 0;
114  flip_cmd_earth.y = 0;
116  heading_save);
118 
119  stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
120 
121  if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
122  flip_state++;
123  }
124  break;
125 
126  default:
130  flip_rollout = false;
131  flip_counter = 0;
132  timer_save = 0;
133  flip_state = 0;
134 
135  stabilization_cmd[COMMAND_ROLL] = 0;
136  stabilization_cmd[COMMAND_PITCH] = 0;
137  stabilization_cmd[COMMAND_YAW] = 0;
138  stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
139  break;
140  }
141 }
void guidance_flip_enter(void)
Definition: guidance_flip.c:56
bool flip_rollout
Definition: guidance_flip.c:51
int32_t heading_save
Definition: guidance_flip.c:52
Open Loop guidance for making a flip.
int32_t stabilization_attitude_get_heading_i(void)
Read an attitude setpoint from the RC.
#define FIRST_THRUST_DURATION
Definition: guidance_flip.c:43
float psi
in radians
void stabilization_attitude_run(bool in_flight)
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:156
#define STOP_ROLL_CMD_ANGLE
Definition: guidance_flip.c:40
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
#define BFP_OF_REAL(_vr, _frac)
uint8_t autopilot_mode_old
Definition: guidance_flip.c:53
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:252
uint8_t autopilot_mode_auto2
unsigned long uint32_t
Definition: types.h:18
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
#define FINAL_THRUST_LEVEL
Definition: guidance_flip.c:46
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
Core autopilot interface common to all firmwares.
int32_t phi
in rad with INT32_ANGLE_FRAC
unsigned char uint8_t
Definition: types.h:14
void guidance_flip_run(void)
Definition: guidance_flip.c:65
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:28
uint32_t flip_counter
Definition: guidance_flip.c:49
struct Int32Vect2 flip_cmd_earth
Definition: guidance_flip.c:54
Horizontal guidance for rotorcrafts.
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
uint8_t flip_state
Definition: guidance_flip.c:50
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:179