Paparazzi UAS  v7.0_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 }
Core autopilot interface common to all firmwares.
Interface for electrical status: supply voltage, current, battery status, etc.
float psi
in radians
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
#define ANGLE_BFP_OF_REAL(_af)
#define INT32_ANGLE_NORMALIZE(_a)
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
float err_vy
Definition: guidance_OA.c:124
struct NedCoor_f opti_speed_read
Definition: guidance_OA.c:129
int8_t filter_flag
Definition: guidance_OA.c:93
float alpha_fil
Definition: guidance_OA.c:112
struct opticflow_stab_t opticflow_stab
Definition: guidance_OA.c:83
float r_dot_new
Definition: guidance_OA.c:107
int32_t yaw_rate
Definition: guidance_OA.c:117
float new_heading
Definition: guidance_OA.c:114
struct FloatVect3 Total_force
Definition: guidance_OA.c:130
void guidance_h_module_read_rc(void)
Read the RC commands.
Definition: guidance_OA.c:166
float vref_max
Definition: guidance_OA.c:99
float heading_target
Definition: guidance_OA.c:113
#define VISION_THETA_PGAIN
Definition: guidance_OA.c:55
float speed_pot
Definition: guidance_OA.c:110
#define VISION_DESIRED_VY
Definition: guidance_OA.c:70
float Total_Kan_x
Definition: guidance_OA.c:126
void guidance_h_module_run(bool in_flight)
Main guidance loop.
Definition: guidance_OA.c:175
oa_method OA_method_flag
Definition: guidance_OA.c:96
float Total_Kan_y
Definition: guidance_OA.c:127
float ref_pitch
Definition: guidance_OA.c:103
float v_desired
Definition: guidance_OA.c:115
int8_t repulsionforce_filter_flag
Definition: guidance_OA.c:94
float yaw_ref_write
Definition: guidance_OA.c:119
int32_t keep_turning
Definition: guidance_OA.c:121
int8_t opti_speed_flag
Definition: guidance_OA.c:98
int32_t keep_yaw_rate
Definition: guidance_OA.c:120
float yaw_rate_write
Definition: guidance_OA.c:118
void guidance_h_module_init(void)
Initialization of horizontal guidance module.
Definition: guidance_OA.c:138
#define VISION_DESIRED_VX
Definition: guidance_OA.c:65
float desired_vx
Definition: guidance_OA.c:108
#define VISION_PHI_PGAIN
Definition: guidance_OA.c:45
#define CMD_OF_SAT
Definition: guidance_OA.c:41
#define VISION_PHI_IGAIN
Definition: guidance_OA.c:50
#define VISION_THETA_IGAIN
Definition: guidance_OA.c:60
float err_vx
Definition: guidance_OA.c:123
float yaw_diff
Definition: guidance_OA.c:111
float desired_vy
Definition: guidance_OA.c:109
void OA_update()
Update the controls based on a vision result.
Definition: guidance_OA.c:189
void guidance_h_module_enter(void)
Horizontal guidance mode enter resets the errors and starts the controller.
Definition: guidance_OA.c:147
float ref_roll
Definition: guidance_OA.c:104
Guidance for the obstacle avoidance methods.
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.
oa_method
Definition: guidance_OA.h:52
@ SAFETYZONE
Definition: guidance_OA.h:52
@ LOGICBASED
Definition: guidance_OA.h:52
@ PINGPONG
Definition: guidance_OA.h:52
@ POT_HEADING
Definition: guidance_OA.h:52
@ NO_OBSTACLE_AVOIDANCE
Definition: guidance_OA.h:52
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.
Definition: point.c:84
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
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)
Periodic telemetry system header (includes downlink utility and generated code).
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103