Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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"
36 #include "subsystems/electrical.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 */
150  opticflow_stab.err_vx_int = 0;
151  opticflow_stab.err_vy_int = 0;
152 
153  /* Set rool/pitch to 0 degrees and psi to current heading */
154  opticflow_stab.cmd.phi = 0;
155  opticflow_stab.cmd.theta = 0;
156  opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi;
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_t 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 
219  if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) {
220  /* Calculate the error if we have enough flow */
221  opticflow_stab.desired_vx = 0;
222  opticflow_stab.desired_vy = 0;
223 
224  err_vx = opticflow_stab.desired_vx - v_x;
225  err_vy = opticflow_stab.desired_vy - v_y;
226 
227  /* Calculate the integrated errors (TODO: bound??) */
228  opticflow_stab.err_vx_int += err_vx / 100;
229  opticflow_stab.err_vy_int += err_vy / 100;
230 
231  /* Calculate the commands */
232  opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
233  + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
234  opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
235  + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
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) {
243  opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll);
244  opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch);
245  }
246 
247  if (OA_method_flag == 2) {
248  Total_Kan_x = r_dot_new;
249  Total_Kan_y = speed_pot;
250 
251  alpha_fil = 0.1;
252 
253  yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new));
254  opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi + yaw_rate;
255 
256  INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi);
257 
258  opticflow_stab.desired_vx = 0;
259  opticflow_stab.desired_vy = speed_pot;
260 
261  /* Calculate the error if we have enough flow */
262 
263  err_vx = opticflow_stab.desired_vx - v_x;
264  err_vy = opticflow_stab.desired_vy - v_y;
265 
266  /* Calculate the integrated errors (TODO: bound??) */
267  opticflow_stab.err_vx_int += err_vx / 100;
268  opticflow_stab.err_vy_int += err_vy / 100;
269 
270  /* Calculate the commands */
271  opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
272  + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
273 
274  opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
275  + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
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) {
283  new_heading = ref_pitch;
284 
285  opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100;
286  opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100;
287 
288  /* Calculate the error if we have enough flow */
289  err_vx = opticflow_stab.desired_vx - v_x;
290  err_vy = opticflow_stab.desired_vy - v_y;
291 
292 
293  /* Calculate the integrated errors (TODO: bound??) */
294  opticflow_stab.err_vx_int += err_vx / 100;
295  opticflow_stab.err_vy_int += err_vy / 100;
296 
297  /* Calculate the commands */
298  opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
299  + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
300  opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
301  + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
302 
303  /* Bound the roll and pitch commands */
304  BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
305  BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT)
306 
307  }
308 
309  if (OA_method_flag == VECTOR || OA_method_flag == SAFETYZONE || OA_method_flag == LOGICBASED) {
310  //vector field method
311  float v_desired_total;
312 
313  Total_Kan_x = ref_pitch;
314  Total_Kan_y = ref_roll;
315 
316  opticflow_stab.desired_vx = alpha_fil *
317  Total_Kan_y; //alpha_fil*(Repulsionforce_Kan.y+Attractforce_goal.y) + result->vel_x;
318  opticflow_stab.desired_vy = alpha_fil *
319  Total_Kan_x; //alpha_fil*(Repulsionforce_Kan.x+Attractforce_goal.x) + result->vel_y;
320 
321  v_desired_total = sqrt(opticflow_stab.desired_vx * opticflow_stab.desired_vx + opticflow_stab.desired_vy *
322  opticflow_stab.desired_vy);
323 
324  if (v_desired_total >= vref_max) {
325  opticflow_stab.desired_vx = (vref_max / v_desired_total) * opticflow_stab.desired_vx;
326  opticflow_stab.desired_vy = (vref_max / v_desired_total) * opticflow_stab.desired_vy;
327  }
328 
329  /* Calculate the error if we have enough flow */
330 
331  //alpha_fil needs to be tuned!
332  err_vx = opticflow_stab.desired_vx - v_x;
333  err_vy = opticflow_stab.desired_vy - v_y;
334 
335  /* Calculate the integrated errors (TODO: bound??) */
336  opticflow_stab.err_vx_int += err_vx / 100;
337  opticflow_stab.err_vy_int += err_vy / 100;
338 
339  /* Calculate the commands */
340  opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
341  + opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
342  opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
343  + opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
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 }
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t keep_turning
Definition: guidance_OA.c:121
float Total_Kan_y
Definition: guidance_OA.c:127
float y
in meters
float desired_vx
Definition: guidance_OA.c:108
int32_t keep_yaw_rate
Definition: guidance_OA.c:120
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
Periodic telemetry system header (includes downlink utility and generated code).
General attitude stabilization interface for rotorcrafts.
int32_t theta
in rad with INT32_ANGLE_FRAC
float yaw_diff
Definition: guidance_OA.c:111
struct NedCoor_f opti_speed_read
Definition: guidance_OA.c:129
void guidance_h_module_read_rc(void)
Read the RC commands.
Definition: guidance_OA.c:166
float psi
in radians
Vertical guidance for rotorcrafts.
float v_desired
Definition: guidance_OA.c:115
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)
int8_t filter_flag
Definition: guidance_OA.c:93
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 VISION_PHI_PGAIN
Definition: guidance_OA.c:45
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
Definition: guidance_OA.c:138
int32_t theta_igain
The pitch I gain on the err_vy_int.
float r_dot_new
Definition: guidance_OA.c:107
float yaw_rate_write
Definition: guidance_OA.c:118
Guidance for the obstacle avoidance methods.
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Definition: guidance_OA.c:147
Definition: point.c:83
vector in North East Down coordinates Units: meters
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:885
Interface for electrical status: supply voltage, current, battery status, etc.
void stabilization_attitude_run(bool_t in_flight)
float Total_Kan_x
Definition: guidance_OA.c:126
float desired_vy
Definition: guidance_OA.c:109
float err_vy
Definition: guidance_OA.c:124
int8_t opti_speed_flag
Definition: guidance_OA.c:98
float ref_pitch
Definition: guidance_OA.c:103
void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy)
int8_t repulsionforce_filter_flag
Definition: guidance_OA.c:94
#define INT32_ANGLE_NORMALIZE(_a)
#define VISION_DESIRED_VX
Definition: guidance_OA.c:65
struct opticflow_stab_t opticflow_stab
Definition: guidance_OA.c:83
float alpha_fil
Definition: guidance_OA.c:112
struct Int32Eulers cmd
The commands that are send to the hover loop.
float ref_roll
Definition: guidance_OA.c:104
void OA_update()
Update the controls based on a vision result.
Definition: guidance_OA.c:189
int32_t yaw_rate
Definition: guidance_OA.c:117
int32_t phi_igain
The roll I gain on the err_vx_int.
signed long int32_t
Definition: types.h:19
#define ANGLE_BFP_OF_REAL(_af)
#define VISION_DESIRED_VY
Definition: guidance_OA.c:70
float speed_pot
Definition: guidance_OA.c:110
int32_t phi
in rad with INT32_ANGLE_FRAC
#define VISION_PHI_IGAIN
Definition: guidance_OA.c:50
float desired_vy
The desired velocity in the y direction (cm/s)
struct FloatVect3 Total_force
Definition: guidance_OA.c:130
float vref_max
Definition: guidance_OA.c:99
float heading_target
Definition: guidance_OA.c:113
oa_method
Definition: guidance_OA.h:52
#define CMD_OF_SAT
Definition: guidance_OA.c:41
float err_vx
Definition: guidance_OA.c:123
void guidance_h_module_run(bool_t in_flight)
Main guidance loop.
Definition: guidance_OA.c:175
float yaw_ref_write
Definition: guidance_OA.c:119
signed char int8_t
Definition: types.h:15
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1096
float err_vy_int
The integrated velocity error in y direction (m/s)
#define VISION_THETA_PGAIN
Definition: guidance_OA.c:55
oa_method OA_method_flag
Definition: guidance_OA.c:96
float x
in meters
float new_heading
Definition: guidance_OA.c:114
#define VISION_THETA_IGAIN
Definition: guidance_OA.c:60