Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_opticflow_hover.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Hann Woei Ho
3  * 2015 Freek van Tienen <freek.v.tienen@gmail.com>
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 
30 // Own Header
32 
33 #include "modules/core/abi.h"
34 
35 // Stabilization
38 #include "autopilot.h"
40 
42 #ifndef VISION_VELOCITY_ESTIMATE_ID
43 #define VISION_VELOCITY_ESTIMATE_ID ABI_BROADCAST
44 #endif
45 PRINT_CONFIG_VAR(VISION_VELOCITY_ESTIMATE_ID)
46 
47 #define CMD_OF_SAT 1500 // 40 deg = 2859.1851
48 
49 #ifndef VISION_PHI_PGAIN
50 #define VISION_PHI_PGAIN 400
51 #endif
52 PRINT_CONFIG_VAR(VISION_PHI_PGAIN)
53 
54 #ifndef VISION_PHI_IGAIN
55 #define VISION_PHI_IGAIN 20
56 #endif
57 PRINT_CONFIG_VAR(VISION_PHI_IGAIN)
58 
59 #ifndef VISION_THETA_PGAIN
60 #define VISION_THETA_PGAIN 400
61 #endif
62 PRINT_CONFIG_VAR(VISION_THETA_PGAIN)
63 
64 #ifndef VISION_THETA_IGAIN
65 #define VISION_THETA_IGAIN 20
66 #endif
67 PRINT_CONFIG_VAR(VISION_THETA_IGAIN)
68 
69 #ifndef VISION_DESIRED_VX
70 #define VISION_DESIRED_VX 0
71 #endif
72 PRINT_CONFIG_VAR(VISION_DESIRED_VX)
73 
74 #ifndef VISION_DESIRED_VY
75 #define VISION_DESIRED_VY 0
76 #endif
77 PRINT_CONFIG_VAR(VISION_DESIRED_VY)
78 
79 /* Check the control gains */
80 #if (VISION_PHI_PGAIN < 0) || \
81  (VISION_PHI_IGAIN < 0) || \
82  (VISION_THETA_PGAIN < 0) || \
83  (VISION_THETA_IGAIN < 0)
84 #error "ALL control gains have to be positive!!!"
85 #endif
86 
88 
89 /* Initialize the default gains and settings */
92  .phi_igain = VISION_PHI_IGAIN,
93  .theta_pgain = VISION_THETA_PGAIN,
94  .theta_igain = VISION_THETA_IGAIN,
95  .desired_vx = VISION_DESIRED_VX,
96  .desired_vy = VISION_DESIRED_VY
97 };
98 
99 
100 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
101  uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z);
106 {
107  // Subscribe to the VELOCITY_ESTIMATE ABI message
109 }
110 
116 {
117  /* Reset the integrated errors */
120 
121  /* Set rool/pitch to 0 degrees and psi to current heading */
122  opticflow_stab.cmd.phi = 0;
125 }
126 
131 {
132  // TODO: change the desired vx/vy
133 }
134 
139 void guidance_h_module_run(bool in_flight)
140 {
141  /* Update the setpoint */
143 
144  /* Run the default attitude stabilization */
145  stabilization_attitude_run(in_flight);
146 }
147 
151 static void stabilization_opticflow_vel_cb(uint8_t sender_id __attribute__((unused)),
152  uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z)
153 {
154  /* Check if we are in the correct AP_MODE before setting commands */
156  return;
157  }
158 
159  if (noise_x >= 0.f)
160  {
161  /* Calculate the error */
162  float err_vx = opticflow_stab.desired_vx - vel_x;
163 
164  /* Calculate the integrated errors (TODO: bound??) */
166 
167  /* Calculate the commands */
170 
171  /* Bound the roll and pitch commands */
172  BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
173  }
174 
175  if (noise_y >= 0.f)
176  {
177  /* Calculate the error */
178  float err_vy = opticflow_stab.desired_vy - vel_y;
179 
180  /* Calculate the integrated errors (TODO: bound??) */
182 
183  /* Calculate the commands */
186 
187  /* Bound the roll and pitch commands */
188  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
189  }
190 }
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
Core autopilot interface common to all firmwares.
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
float err_vy
Definition: guidance_OA.c:124
float err_vx
Definition: guidance_OA.c:123
float err_vx_int
The integrated velocity error in x direction (m/s)
struct Int32Eulers cmd
The commands that are send to the hover loop.
float err_vy_int
The integrated velocity error in y direction (m/s)
int32_t theta_pgain
The pitch P gain on the err_vy.
int32_t theta_igain
The pitch I gain on the err_vy_int.
float desired_vy
The desired velocity in the y direction (cm/s)
float desired_vx
The desired velocity in the x direction (cm/s)
int32_t phi_pgain
The roll P gain on the err_vx.
int32_t phi_igain
The roll I gain on the err_vx_int.
struct opticflow_stab_t opticflow_stab
#define VISION_VELOCITY_ESTIMATE_ID
Default sender to accect VELOCITY_ESTIMATE messages from.
void guidance_h_module_read_rc(void)
Read the RC commands.
#define VISION_THETA_PGAIN
#define VISION_DESIRED_VY
void guidance_h_module_run(bool in_flight)
Main guidance loop.
static void stabilization_opticflow_vel_cb(uint8_t sender_id, uint32_t stamp, float vel_x, float vel_y, float vel_z, float noise_x, float noise_y, float noise_z)
Update the controls on a new VELOCITY_ESTIMATE ABI message.
static abi_event velocity_est_ev
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
#define VISION_DESIRED_VX
#define VISION_PHI_PGAIN
#define CMD_OF_SAT
#define VISION_PHI_IGAIN
#define VISION_THETA_IGAIN
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Optical-flow based control for Linux based systems.
#define AP_MODE_MODULE
Vertical guidance for rotorcrafts.
General attitude stabilization interface for rotorcrafts.
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
void stabilization_attitude_run(bool in_flight)
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