Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_OA.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Roland + Clint
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
26 // Own Header
27 #include "guidance_OA.h"
28 
29 // Stabilization
30 #include <stdio.h>
33 #include "autopilot.h"
37 #include "std.h"
38 //#include "modules/read_matrix_serial/read_matrix_serial.h"
39 
40 //#ifndef CMD_OF_SAT
41 #define CMD_OF_SAT 1500 // 1500 = 40 deg = 2859.1851
42 //#endif
43 
44 #ifndef VISION_PHI_PGAIN
45 #define VISION_PHI_PGAIN 400
46 #endif
47 PRINT_CONFIG_VAR(VISION_PHI_PGAIN)
48 
49 #ifndef VISION_PHI_IGAIN
50 #define VISION_PHI_IGAIN 20
51 #endif
52 PRINT_CONFIG_VAR(VISION_PHI_IGAIN)
53 
54 #ifndef VISION_THETA_PGAIN
55 #define VISION_THETA_PGAIN 400
56 #endif
57 PRINT_CONFIG_VAR(VISION_THETA_PGAIN)
58 
59 #ifndef VISION_THETA_IGAIN
60 #define VISION_THETA_IGAIN 20
61 #endif
62 PRINT_CONFIG_VAR(VISION_THETA_IGAIN)
63 
64 #ifndef VISION_DESIRED_VX
65 #define VISION_DESIRED_VX 0
66 #endif
67 PRINT_CONFIG_VAR(VISION_DESIRED_VX)
68 
69 #ifndef VISION_DESIRED_VY
70 #define VISION_DESIRED_VY 0
71 #endif
72 PRINT_CONFIG_VAR(VISION_DESIRED_VY)
73 
74 /* Check the control gains */
75 #if (VISION_PHI_PGAIN < 0) || \
76  (VISION_PHI_IGAIN < 0) || \
77  (VISION_THETA_PGAIN < 0) || \
78  (VISION_THETA_IGAIN < 0)
79 #error "ALL control gains have to be positive!!!"
80 #endif
81 
82 /* Initialize the default gains and settings */
85  .phi_igain = VISION_PHI_IGAIN,
86  .theta_pgain = VISION_THETA_PGAIN,
87  .theta_igain = VISION_THETA_IGAIN,
88  .desired_vx = VISION_DESIRED_VX,
89  .desired_vy = VISION_DESIRED_VY
90 };
91 
93 int8_t filter_flag = 0; //0 =>no filter 1 =>Kalman filter 2 =>Butterworth filter
94 int8_t repulsionforce_filter_flag = 0; //0 =>no filter 1 =>Butterworth filter
95 
97  PINGPONG; //0 =>No OA only opticflow 1=pingpong 2=>pot_heading 3=>pot_vel 4=>vector 5=>safetyzone
99 float vref_max = 100;
101 
102 //variables set by OA
103 float ref_pitch = 0.0;
104 float ref_roll = 0.0;
105 
106 //initialize variables
107 float r_dot_new = 0.0;
108 float desired_vx = 0.0;
109 float desired_vy = 0.0;
110 float speed_pot = 0.0;
111 float yaw_diff = 0.0;
112 float alpha_fil = 1.0;
113 float heading_target = 0;
114 float new_heading = 0;
115 float v_desired = 0.0;
116 
118 float yaw_rate_write = 0;
119 float yaw_ref_write = 0;
122 
123 float err_vx = 0;
124 float err_vy = 0;
125 
126 float Total_Kan_x = 0;
127 float Total_Kan_y = 0;
128 
130 struct FloatVect3 Total_force = {0, 0, 0};
131 //
132 //static void send_INPUT_CONTROL(void)
133 //{
134 // //DOWNLINK_SEND_INPUT_CONTROL(DefaultChannel, DefaultDevice, &Total_Kan_x, &Total_Kan_y, &opticflow_stab.desired_vx,
135 // // &opticflow_stab.desired_vy, &opti_speed_read.x, &opti_speed_read.y);
136 //}
137 
139 {
140 
141 }
142 
148 {
149  /* Reset the integrated errors */
152 
153  /* Set rool/pitch to 0 degrees and psi to current heading */
154  opticflow_stab.cmd.phi = 0;
157 
158  new_heading = 0;
159 
160 // register_periodic_telemetry(DefaultPeriodic, "INPUT_CONTROL", send_INPUT_CONTROL);
161 }
162 
167 {
168  // TODO: change the desired vx/vy
169 }
170 
175 void guidance_h_module_run(bool in_flight)
176 {
177  OA_update();
178  /* Update the setpoint */
180 
181  /* Run the default attitude stabilization */
182  stabilization_attitude_run(in_flight);
183 }
184 
189 void OA_update()
190 {
191  float v_x = 0;
192  float v_y = 0;
193 
194  if (opti_speed_flag == 1) {
195  //rotation_vector.psi = stateGetNedToBodyEulers_f()->psi;
196  //opti_speed = stateGetSpeedNed_f();
197 
198  //Transform to body frame.
199  //float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send;
200  //MAT33_VECT3_MUL(*opti_speed, T, *opti_speed);
201 
202  // Calculate the speed in body frame
203  struct FloatVect2 speed_cur;
204  float psi = stateGetNedToBodyEulers_f()->psi;
205  float s_psi = sin(psi);
206  float c_psi = cos(psi);
207  speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y;
208  speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y;
209 
210  opti_speed_read.x = speed_cur.x * 100;
211  opti_speed_read.y = speed_cur.y * 100;
212 
213  //set result_vel
214  v_x = speed_cur.y * 100;
215  v_y = speed_cur.x * 100;
216  } else {
217  }
218 
220  /* Calculate the error if we have enough flow */
223 
226 
227  /* Calculate the integrated errors (TODO: bound??) */
230 
231  /* Calculate the commands */
236 
237  /* Bound the roll and pitch commands */
238  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
239  BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
240  }
241 
242  if (OA_method_flag == PINGPONG) {
245  }
246 
247  if (OA_method_flag == 2) {
250 
251  alpha_fil = 0.1;
252 
255 
257 
260 
261  /* Calculate the error if we have enough flow */
262 
265 
266  /* Calculate the integrated errors (TODO: bound??) */
269 
270  /* Calculate the commands */
273 
276 
277  /* Bound the roll and pitch commands */
278  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
279  BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
280 
281  }
282  if (OA_method_flag == POT_HEADING) {
284 
287 
288  /* Calculate the error if we have enough flow */
291 
292 
293  /* Calculate the integrated errors (TODO: bound??) */
296 
297  /* Calculate the commands */
302 
303  /* Bound the roll and pitch commands */
304  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
306 
307  }
308 
310  //vector field method
311  float v_desired_total;
312 
315 
317  Total_Kan_y; //alpha_fil*(Repulsionforce_Kan.y+Attractforce_goal.y) + result->vel_x;
319  Total_Kan_x; //alpha_fil*(Repulsionforce_Kan.x+Attractforce_goal.x) + result->vel_y;
320 
323 
324  if (v_desired_total >= vref_max) {
327  }
328 
329  /* Calculate the error if we have enough flow */
330 
331  //alpha_fil needs to be tuned!
334 
335  /* Calculate the integrated errors (TODO: bound??) */
338 
339  /* Calculate the commands */
344 
345  /* Bound the roll and pitch commands */
346  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
347  BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
348 
349  }
350 }
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
electrical.h
Total_force
struct FloatVect3 Total_force
Definition: guidance_OA.c:130
int8_t
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
NedCoor_f
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_float.h:63
VISION_PHI_IGAIN
#define VISION_PHI_IGAIN
Definition: guidance_OA.c:50
stabilization_attitude.h
err_vx
float err_vx
Definition: guidance_OA.c:123
oa_method
oa_method
Definition: guidance_OA.h:52
new_heading
float new_heading
Definition: guidance_OA.c:114
v_desired
float v_desired
Definition: guidance_OA.c:115
keep_turning
int32_t keep_turning
Definition: guidance_OA.c:121
VECTOR
Definition: point.c:84
VISION_DESIRED_VX
#define VISION_DESIRED_VX
Definition: guidance_OA.c:65
PINGPONG
@ PINGPONG
Definition: guidance_OA.h:52
opticflow_stab_t::phi_pgain
int32_t phi_pgain
The roll P gain on the err_vx.
Definition: guidance_opticflow_hover.h:38
alpha_fil
float alpha_fil
Definition: guidance_OA.c:112
ref_roll
float ref_roll
Definition: guidance_OA.c:104
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
opticflow_stab_t::desired_vx
float desired_vx
The desired velocity in the x direction (cm/s)
Definition: guidance_opticflow_hover.h:42
LOGICBASED
@ LOGICBASED
Definition: guidance_OA.h:52
guidance_h_module_init
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
Definition: guidance_OA.c:138
opticflow_stab_t::theta_igain
int32_t theta_igain
The pitch I gain on the err_vy_int.
Definition: guidance_opticflow_hover.h:41
opti_speed_flag
int8_t opti_speed_flag
Definition: guidance_OA.c:98
yaw_rate
int32_t yaw_rate
Definition: guidance_OA.c:117
SAFETYZONE
@ SAFETYZONE
Definition: guidance_OA.h:52
repulsionforce_filter_flag
int8_t repulsionforce_filter_flag
Definition: guidance_OA.c:94
Total_Kan_y
float Total_Kan_y
Definition: guidance_OA.c:127
yaw_rate_write
float yaw_rate_write
Definition: guidance_OA.c:118
guidance_v.h
opti_speed_read
struct NedCoor_f opti_speed_read
Definition: guidance_OA.c:129
yaw_diff
float yaw_diff
Definition: guidance_OA.c:111
OA_update
void OA_update()
Update the controls based on a vision result.
Definition: guidance_OA.c:189
ref_pitch
float ref_pitch
Definition: guidance_OA.c:103
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
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
telemetry.h
opticflow_stab_t::desired_vy
float desired_vy
The desired velocity in the y direction (cm/s)
Definition: guidance_opticflow_hover.h:43
std.h
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
VISION_DESIRED_VY
#define VISION_DESIRED_VY
Definition: guidance_OA.c:70
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
keep_yaw_rate
int32_t keep_yaw_rate
Definition: guidance_OA.c:120
guidance_OA.h
Guidance for the obstacle avoidance methods.
desired_vy
float desired_vy
Definition: guidance_OA.c:109
r_dot_new
float r_dot_new
Definition: guidance_OA.c:107
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
heading_target
float heading_target
Definition: guidance_OA.c:113
vref_max
float vref_max
Definition: guidance_OA.c:99
NO_OBSTACLE_AVOIDANCE
@ NO_OBSTACLE_AVOIDANCE
Definition: guidance_OA.h:52
Total_Kan_x
float Total_Kan_x
Definition: guidance_OA.c:126
POT_HEADING
@ POT_HEADING
Definition: guidance_OA.h:52
filter_flag
int8_t filter_flag
Definition: guidance_OA.c:93
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
VISION_THETA_PGAIN
#define VISION_THETA_PGAIN
Definition: guidance_OA.c:55
autopilot.h
OA_method_flag
oa_method OA_method_flag
Definition: guidance_OA.c:96
err_vy
float err_vy
Definition: guidance_OA.c:124
FloatVect2::y
float y
Definition: pprz_algebra_float.h:51
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
guidance_h_module_run
void guidance_h_module_run(bool in_flight)
Main guidance loop.
Definition: guidance_OA.c:175
speed_pot
float speed_pot
Definition: guidance_OA.c:110
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
VISION_PHI_PGAIN
#define VISION_PHI_PGAIN
Definition: guidance_OA.c:45
opticflow_stab
struct opticflow_stab_t opticflow_stab
Definition: guidance_OA.c:83
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_enter
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Definition: guidance_OA.c:147
CMD_OF_SAT
#define CMD_OF_SAT
Definition: guidance_OA.c:41
stabilization_attitude_set_rpy_setpoint_i
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
Definition: stabilization_attitude_euler_float.c:154
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
FloatVect2::x
float x
Definition: pprz_algebra_float.h:50
stateGetNedToBodyEulers_i
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
VISION_THETA_IGAIN
#define VISION_THETA_IGAIN
Definition: guidance_OA.c:60
guidance_h_module_read_rc
void guidance_h_module_read_rc(void)
Read the RC commands.
Definition: guidance_OA.c:166
FloatEulers::psi
float psi
in radians
Definition: pprz_algebra_float.h:87
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
yaw_ref_write
float yaw_ref_write
Definition: guidance_OA.c:119
desired_vx
float desired_vx
Definition: guidance_OA.c:108
INT32_ANGLE_NORMALIZE
#define INT32_ANGLE_NORMALIZE(_a)
Definition: pprz_algebra_int.h:126
opticflow_stab_t
Definition: guidance_opticflow_hover.h:37