Paparazzi UAS  v5.15_devel-47-g0391b4d
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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
108  AbiBindMsgVELOCITY_ESTIMATE(VISION_VELOCITY_ESTIMATE_ID, &velocity_est_ev, stabilization_opticflow_vel_cb);
109 }
110 
116 {
117  /* Reset the integrated errors */
118  opticflow_stab.err_vx_int = 0;
119  opticflow_stab.err_vy_int = 0;
120 
121  /* Set rool/pitch to 0 degrees and psi to current heading */
122  opticflow_stab.cmd.phi = 0;
123  opticflow_stab.cmd.theta = 0;
124  opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi;
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??) */
165  opticflow_stab.err_vx_int += err_vx / 512;
166 
167  /* Calculate the commands */
168  opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vx
169  + opticflow_stab.theta_igain * opticflow_stab.err_vx_int);
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??) */
181  opticflow_stab.err_vy_int += err_vy / 512;
182 
183  /* Calculate the commands */
184  opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vy
185  + opticflow_stab.phi_igain * opticflow_stab.err_vy_int;
186 
187  /* Bound the roll and pitch commands */
188  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
189  }
190 }
int32_t psi
in rad with INT32_ANGLE_FRAC
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
General attitude stabilization interface for rotorcrafts.
int32_t theta
in rad with INT32_ANGLE_FRAC
Main include for ABI (AirBorneInterface).
#define VISION_THETA_IGAIN
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.
Vertical guidance for rotorcrafts.
float err_vx_int
The integrated velocity error in x direction (m/s)
float desired_vx
The desired velocity in the x direction (cm/s)
void stabilization_attitude_run(bool in_flight)
int32_t phi_pgain
The roll P gain on the err_vx.
int32_t theta_pgain
The pitch P gain on the err_vy.
#define AP_MODE_MODULE
int32_t theta_igain
The pitch I gain on the err_vy_int.
void guidance_h_module_read_rc(void)
Read the RC commands.
void guidance_h_module_run(bool in_flight)
Main guidance loop.
float err_vy
Definition: guidance_OA.c:124
struct opticflow_stab_t opticflow_stab
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
unsigned long uint32_t
Definition: types.h:18
struct Int32Eulers cmd
The commands that are send to the hover loop.
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
int32_t phi_igain
The roll I gain on the err_vx_int.
#define VISION_PHI_IGAIN
Core autopilot interface common to all firmwares.
#define VISION_DESIRED_VX
int32_t phi
in rad with INT32_ANGLE_FRAC
unsigned char uint8_t
Definition: types.h:14
float desired_vy
The desired velocity in the y direction (cm/s)
float err_vx
Definition: guidance_OA.c:123
#define VISION_VELOCITY_ESTIMATE_ID
Default sender to accect VELOCITY_ESTIMATE messages from.
static abi_event velocity_est_ev
#define VISION_THETA_PGAIN
#define VISION_PHI_PGAIN
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
float err_vy_int
The integrated velocity error in y direction (m/s)
#define CMD_OF_SAT
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
#define VISION_DESIRED_VY
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
Optical-flow based control for Linux based systems.