Paparazzi UAS  v5.18.0_stable
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 "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
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 }
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
VISION_PHI_PGAIN
#define VISION_PHI_PGAIN
Definition: guidance_opticflow_hover.c:50
CMD_OF_SAT
#define CMD_OF_SAT
Definition: guidance_opticflow_hover.c:47
stabilization_attitude.h
err_vx
float err_vx
Definition: guidance_OA.c:123
velocity_est_ev
static abi_event velocity_est_ev
Definition: guidance_opticflow_hover.c:87
VISION_PHI_IGAIN
#define VISION_PHI_IGAIN
Definition: guidance_opticflow_hover.c:55
AP_MODE_MODULE
#define AP_MODE_MODULE
Definition: autopilot_static.h:52
abi.h
opticflow_stab_t::phi_pgain
int32_t phi_pgain
The roll P gain on the err_vx.
Definition: guidance_opticflow_hover.h:38
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
opticflow_stab_t::desired_vx
float desired_vx
The desired velocity in the x direction (cm/s)
Definition: guidance_opticflow_hover.h:42
opticflow_stab_t::theta_igain
int32_t theta_igain
The pitch I gain on the err_vy_int.
Definition: guidance_opticflow_hover.h:41
uint32_t
unsigned long uint32_t
Definition: types.h:18
guidance_opticflow_hover.h
Optical-flow based control for Linux based systems.
guidance_v.h
stabilization_opticflow_vel_cb
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.
Definition: guidance_opticflow_hover.c:151
opticflow_stab
struct opticflow_stab_t opticflow_stab
Definition: guidance_opticflow_hover.c:90
opticflow_stab_t::err_vx_int
float err_vx_int
The integrated velocity error in x direction (m/s)
Definition: guidance_opticflow_hover.h:45
opticflow_stab_t::desired_vy
float desired_vy
The desired velocity in the y direction (cm/s)
Definition: guidance_opticflow_hover.h:43
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
VISION_DESIRED_VY
#define VISION_DESIRED_VY
Definition: guidance_opticflow_hover.c:75
VISION_THETA_IGAIN
#define VISION_THETA_IGAIN
Definition: guidance_opticflow_hover.c:65
guidance_h_module_init
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
Definition: guidance_opticflow_hover.c:105
VISION_DESIRED_VX
#define VISION_DESIRED_VX
Definition: guidance_opticflow_hover.c:70
uint8_t
unsigned char uint8_t
Definition: types.h:14
opticflow_stab_t::theta_pgain
int32_t theta_pgain
The pitch P gain on the err_vy.
Definition: guidance_opticflow_hover.h:40
stabilization_attitude_run
void stabilization_attitude_run(bool in_flight)
Definition: stabilization_attitude_euler_float.c:176
autopilot.h
guidance_h_module_run
void guidance_h_module_run(bool in_flight)
Main guidance loop.
Definition: guidance_opticflow_hover.c:139
err_vy
float err_vy
Definition: guidance_OA.c:124
f
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204
VISION_THETA_PGAIN
#define VISION_THETA_PGAIN
Definition: guidance_opticflow_hover.c:60
VISION_VELOCITY_ESTIMATE_ID
#define VISION_VELOCITY_ESTIMATE_ID
Default sender to accect VELOCITY_ESTIMATE messages from.
Definition: guidance_opticflow_hover.c:43
opticflow_stab_t::cmd
struct Int32Eulers cmd
The commands that are send to the hover loop.
Definition: guidance_opticflow_hover.h:47
guidance_h_module_read_rc
void guidance_h_module_read_rc(void)
Read the RC commands.
Definition: guidance_opticflow_hover.c:130
guidance_h_module_enter
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Definition: guidance_opticflow_hover.c:115
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_float.c:154
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
autopilot_get_mode
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:184
opticflow_stab_t::phi_igain
int32_t phi_igain
The roll I gain on the err_vx_int.
Definition: guidance_opticflow_hover.h:39
opticflow_stab_t::err_vy_int
float err_vy_int
The integrated velocity error in y direction (m/s)
Definition: guidance_opticflow_hover.h:46
opticflow_stab_t
Definition: guidance_opticflow_hover.h:37