Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 #if defined(COMMAND_ROLL) && defined(COMMAND_PITCH) && defined(COMMAND_YAW)
68 
69  uint32_t timer;
70  int32_t phi;
71  static uint32_t timer_save = 0;
72 
73  timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
75 
76  switch (flip_state) {
77  case 0:
78  flip_cmd_earth.x = 0;
79  flip_cmd_earth.y = 0;
81  heading_save);
83  stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
84  timer_save = 0;
85 
86  if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
87  flip_state++;
88  }
89  break;
90 
91  case 1:
92  stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
93  stabilization_cmd[COMMAND_PITCH] = 0;
94  stabilization_cmd[COMMAND_YAW] = 0;
95  stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
96 
97  if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
98  flip_state++;
99  }
100  break;
101 
102  case 2:
103  stabilization_cmd[COMMAND_ROLL] = 0;
104  stabilization_cmd[COMMAND_PITCH] = 0;
105  stabilization_cmd[COMMAND_YAW] = 0;
106  stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
107 
108  if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
109  timer_save = timer;
110  flip_state++;
111  }
112  break;
113 
114  case 3:
115  flip_cmd_earth.x = 0;
116  flip_cmd_earth.y = 0;
118  heading_save);
120 
121  stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
122 
123  if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
124  flip_state++;
125  }
126  break;
127 
128  default:
132  flip_rollout = false;
133  flip_counter = 0;
134  timer_save = 0;
135  flip_state = 0;
136 
137  stabilization_cmd[COMMAND_ROLL] = 0;
138  stabilization_cmd[COMMAND_PITCH] = 0;
139  stabilization_cmd[COMMAND_YAW] = 0;
140  stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
141  break;
142  }
143 #else
146 #endif
147 }
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:193
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:335
Core autopilot interface common to all firmwares.
float psi
in radians
int32_t phi
in rad with INT32_ANGLE_FRAC
#define ANGLE_BFP_OF_REAL(_af)
#define BFP_OF_REAL(_vr, _frac)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
#define FINAL_THRUST_LEVEL
Definition: guidance_flip.c:46
uint8_t flip_state
Definition: guidance_flip.c:50
void guidance_flip_run(void)
Definition: guidance_flip.c:65
uint32_t flip_counter
Definition: guidance_flip.c:49
int32_t heading_save
Definition: guidance_flip.c:52
uint8_t autopilot_mode_old
Definition: guidance_flip.c:53
#define STOP_ROLL_CMD_ANGLE
Definition: guidance_flip.c:40
bool flip_rollout
Definition: guidance_flip.c:51
void guidance_flip_enter(void)
Definition: guidance_flip.c:56
#define FIRST_THRUST_DURATION
Definition: guidance_flip.c:43
struct Int32Vect2 flip_cmd_earth
Definition: guidance_flip.c:54
Open Loop guidance for making a flip.
uint8_t autopilot_mode_auto2
Horizontal guidance for rotorcrafts.
void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
void stabilization_attitude_run(bool in_flight)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:34
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
int32_t stabilization_attitude_get_heading_i(void)
Read an attitude setpoint from the RC.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98