Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_indi.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
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 
34 #include "generated/airframe.h"
36 #include "subsystems/ins/ins_int.h"
38 #include "state.h"
39 #include "subsystems/imu.h"
44 #include "mcu_periph/sys_time.h"
45 #include "autopilot.h"
49 #include "subsystems/abi.h"
50 
51 // The acceleration reference is calculated with these gains. If you use GPS,
52 // they are probably limited by the update rate of your GPS. The default
53 // values are tuned for 4 Hz GPS updates. If you have high speed position updates, the
54 // gains can be higher, depending on the speed of the inner loop.
55 #ifdef GUIDANCE_INDI_POS_GAIN
56 float guidance_indi_pos_gain = GUIDANCE_INDI_POS_GAIN;
57 #else
59 #endif
60 
61 #ifdef GUIDANCE_INDI_SPEED_GAIN
62 float guidance_indi_speed_gain = GUIDANCE_INDI_SPEED_GAIN;
63 #else
65 #endif
66 
67 #ifndef GUIDANCE_INDI_ACCEL_SP_ID
68 #define GUIDANCE_INDI_ACCEL_SP_ID ABI_BROADCAST
69 #endif
71 static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp);
72 struct FloatVect3 indi_accel_sp = {0.0, 0.0, 0.0};
73 bool indi_accel_sp_set_2d = false;
74 bool indi_accel_sp_set_3d = false;
75 
76 struct FloatVect3 sp_accel = {0.0, 0.0, 0.0};
77 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
78 float thrust_in_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
79 static void guidance_indi_filter_thrust(void);
80 
81 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS
82 #ifndef STABILIZATION_INDI_ACT_DYN_P
83 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
84 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
85 #define GUIDANCE_INDI_THRUST_DYNAMICS STABILIZATION_INDI_ACT_DYN_P
86 #endif
87 #endif //GUIDANCE_INDI_THRUST_DYNAMICS
88 
89 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
90 
91 #ifndef GUIDANCE_INDI_FILTER_CUTOFF
92 #ifdef STABILIZATION_INDI_FILT_CUTOFF
93 #define GUIDANCE_INDI_FILTER_CUTOFF STABILIZATION_INDI_FILT_CUTOFF
94 #else
95 #define GUIDANCE_INDI_FILTER_CUTOFF 3.0
96 #endif
97 #endif
98 
99 float thrust_act = 0;
104 
105 struct FloatMat33 Ga;
107 struct FloatVect3 control_increment; // [dtheta, dphi, dthrust]
108 
111 
114 
116 float thrust_in;
117 
118 static void guidance_indi_propagate_filters(struct FloatEulers *eulers);
119 static void guidance_indi_calcG(struct FloatMat33 *Gmat);
120 static void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz);
121 
126 {
127  AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);
128 }
129 
135 {
136  thrust_in = stabilization_cmd[COMMAND_THRUST];
138 
139  float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
140  float sample_time = 1.0 / PERIODIC_FREQUENCY;
141  for (int8_t i = 0; i < 3; i++) {
142  init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
143  }
144  init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
145  init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta);
146  init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
147 }
148 
154 void guidance_indi_run(float heading_sp)
155 {
156  struct FloatEulers eulers_yxz;
157  struct FloatQuat * statequat = stateGetNedToBodyQuat_f();
158  float_eulers_of_quat_yxz(&eulers_yxz, statequat);
159 
160  //filter accel to get rid of noise and filter attitude to synchronize with accel
161  guidance_indi_propagate_filters(&eulers_yxz);
162 
163  //Linear controller to find the acceleration setpoint from position and velocity
164  float pos_x_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.x) - stateGetPositionNed_f()->x;
165  float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
166  float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
167 
168  float speed_sp_x = pos_x_err * guidance_indi_pos_gain;
169  float speed_sp_y = pos_y_err * guidance_indi_pos_gain;
170  float speed_sp_z = pos_z_err * guidance_indi_pos_gain;
171 
172  // If the acceleration setpoint is set over ABI message
173  if (indi_accel_sp_set_2d) {
174  sp_accel.x = indi_accel_sp.x;
175  sp_accel.y = indi_accel_sp.y;
176  // In 2D the vertical motion is derived from the flight plan
177  sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
179  // If the input command is not updated after a timeout, switch back to flight plan control
180  if (dt > 0.5) {
181  indi_accel_sp_set_2d = false;
182  }
183  } else if (indi_accel_sp_set_3d) {
184  sp_accel.x = indi_accel_sp.x;
185  sp_accel.y = indi_accel_sp.y;
186  sp_accel.z = indi_accel_sp.z;
188  // If the input command is not updated after a timeout, switch back to flight plan control
189  if (dt > 0.5) {
190  indi_accel_sp_set_3d = false;
191  }
192  } else {
193  sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
194  sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
195  sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
196  }
197 
198 #if GUIDANCE_INDI_RC_DEBUG
199 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
200  //for rc control horizontal, rotate from body axes to NED
201  float psi = stateGetNedToBodyEulers_f()->psi;
202  float rc_x = -(radio_control.values[RADIO_PITCH] / 9600.0) * 8.0;
203  float rc_y = (radio_control.values[RADIO_ROLL] / 9600.0) * 8.0;
204  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
205  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
206 
207  //for rc vertical control
208  sp_accel.z = -(radio_control.values[RADIO_THROTTLE] - 4500) * 8.0 / 9600.0;
209 #endif
210 
211  //Calculate matrix of partial derivatives
212  guidance_indi_calcG_yxz(&Ga, &eulers_yxz);
213 
214  //Invert this matrix
215  MAT33_INV(Ga_inv, Ga);
216 
217  struct FloatVect3 a_diff = { sp_accel.x - filt_accel_ned[0].o[0], sp_accel.y - filt_accel_ned[1].o[0], sp_accel.z - filt_accel_ned[2].o[0]};
218 
219  //Bound the acceleration error so that the linearization still holds
220  Bound(a_diff.x, -6.0, 6.0);
221  Bound(a_diff.y, -6.0, 6.0);
222  Bound(a_diff.z, -9.0, 9.0);
223 
224  //If the thrust to specific force ratio has been defined, include vertical control
225  //else ignore the vertical acceleration error
226 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
227 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
228  a_diff.z = 0.0;
229 #endif
230 #endif
231 
232  //Calculate roll,pitch and thrust command
234 
235  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, control_increment.z);
236 
237  guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
238  guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
239  guidance_euler_cmd.psi = heading_sp;
240 
241 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
242  guidance_indi_filter_thrust();
243 
244  //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
245  thrust_in = thrust_filt.o[0] + control_increment.z * thrust_in_specific_force_gain;
246  Bound(thrust_in, 0, 9600);
247 
248 #if GUIDANCE_INDI_RC_DEBUG
249  if (radio_control.values[RADIO_THROTTLE] < 300) {
250  thrust_in = 0;
251  }
252 #endif
253 
254  //Overwrite the thrust command from guidance_v
255  stabilization_cmd[COMMAND_THRUST] = thrust_in;
256 #endif
257 
258  //Bound euler angles to prevent flipping
261 
262  //set the quat setpoint with the calculated roll and pitch
263  struct FloatQuat q_sp;
266 }
267 
268 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
269 
272 void guidance_indi_filter_thrust(void)
273 {
274  // Actuator dynamics
275  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
276 
277  // same filter as for the acceleration
279 }
280 #endif
281 
288 {
289  struct NedCoor_f *accel = stateGetAccelNed_f();
290  update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
291  update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
292  update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
293 
294  update_butterworth_2_low_pass(&roll_filt, eulers->phi);
295  update_butterworth_2_low_pass(&pitch_filt, eulers->theta);
296 }
297 
305 void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz)
306 {
307 
308  float sphi = sinf(euler_yxz->phi);
309  float cphi = cosf(euler_yxz->phi);
310  float stheta = sinf(euler_yxz->theta);
311  float ctheta = cosf(euler_yxz->theta);
312  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
313  float T = -9.81;
314 
315  RMAT_ELMT(*Gmat, 0, 0) = ctheta * cphi * T;
316  RMAT_ELMT(*Gmat, 1, 0) = 0;
317  RMAT_ELMT(*Gmat, 2, 0) = -stheta * cphi * T;
318  RMAT_ELMT(*Gmat, 0, 1) = -stheta * sphi * T;
319  RMAT_ELMT(*Gmat, 1, 1) = -cphi * T;
320  RMAT_ELMT(*Gmat, 2, 1) = -ctheta * sphi * T;
321  RMAT_ELMT(*Gmat, 0, 2) = stheta * cphi;
322  RMAT_ELMT(*Gmat, 1, 2) = -sphi;
323  RMAT_ELMT(*Gmat, 2, 2) = ctheta * cphi;
324 }
325 
334 {
335 
336  struct FloatEulers *euler = stateGetNedToBodyEulers_f();
337 
338  float sphi = sinf(euler->phi);
339  float cphi = cosf(euler->phi);
340  float stheta = sinf(euler->theta);
341  float ctheta = cosf(euler->theta);
342  float spsi = sinf(euler->psi);
343  float cpsi = cosf(euler->psi);
344  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
345  float T = -9.81;
346 
347  RMAT_ELMT(*Gmat, 0, 0) = (cphi * spsi - sphi * cpsi * stheta) * T;
348  RMAT_ELMT(*Gmat, 1, 0) = (-sphi * spsi * stheta - cpsi * cphi) * T;
349  RMAT_ELMT(*Gmat, 2, 0) = -ctheta * sphi * T;
350  RMAT_ELMT(*Gmat, 0, 1) = (cphi * cpsi * ctheta) * T;
351  RMAT_ELMT(*Gmat, 1, 1) = (cphi * spsi * ctheta) * T;
352  RMAT_ELMT(*Gmat, 2, 1) = -stheta * cphi * T;
353  RMAT_ELMT(*Gmat, 0, 2) = sphi * spsi + cphi * cpsi * stheta;
354  RMAT_ELMT(*Gmat, 1, 2) = cphi * spsi * stheta - cpsi * sphi;
355  RMAT_ELMT(*Gmat, 2, 2) = cphi * ctheta;
356 }
357 
362 static void accel_sp_cb(uint8_t sender_id __attribute__((unused)), uint8_t flag, struct FloatVect3 *accel_sp)
363 {
364  if (flag == 0) {
365  indi_accel_sp.x = accel_sp->x;
366  indi_accel_sp.y = accel_sp->y;
367  indi_accel_sp_set_2d = true;
369  } else if (flag == 1) {
370  indi_accel_sp.x = accel_sp->x;
371  indi_accel_sp.y = accel_sp->y;
372  indi_accel_sp.z = accel_sp->z;
373  indi_accel_sp_set_3d = true;
375  }
376 }
377 
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
void guidance_indi_enter(void)
Call upon entering indi guidance.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
bool indi_accel_sp_set_2d
Definition: guidance_indi.c:73
#define RADIO_ROLL
Definition: intermcu_ap.h:40
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
float y
in meters
#define GUIDANCE_H_MAX_BANK
Definition: guidance_h.c:66
float phi
in radians
#define GUIDANCE_INDI_ACCEL_SP_ID
Definition: guidance_indi.c:68
struct FloatMat33 Ga
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
General attitude stabilization interface for rotorcrafts.
struct FloatVect3 sp_accel
Definition: guidance_indi.c:76
Simple first order low pass filter with bilinear transform.
uint8_t last_wp UNUSED
Definition: navigation.c:92
static void accel_sp_cb(uint8_t sender_id, uint8_t flag, struct FloatVect3 *accel_sp)
ABI callback that obtains the acceleration setpoint from telemetry flag: 0 -> 2D, 1 -> 3D...
void guidance_indi_init(void)
Init function.
Main include for ABI (AirBorneInterface).
static void guidance_indi_calcG(struct FloatMat33 *Gmat)
float thrust_act
Definition: guidance_indi.c:99
float psi
in radians
Vertical guidance for rotorcrafts.
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:101
pprz_t values[RADIO_CONTROL_NB_CHANNEL]
Definition: radio_control.h:69
float time_of_accel_sp_2d
#define GUIDANCE_INDI_FILTER_CUTOFF
Definition: guidance_indi.c:95
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
Butterworth2LowPass filt_accel_ned[3]
struct Int32Quat stab_att_sp_quat
with INT32_QUAT_FRAC
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
euler angles
float z
in meters
float o[2]
output history
Roation quaternion.
float time_of_accel_sp_3d
void guidance_indi_run(float heading_sp)
float theta
in radians
abi_event accel_sp_ev
Definition: guidance_indi.c:70
Butterworth2LowPass pitch_filt
float filter_cutoff
Some helper functions to check RC sticks.
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:908
Architecture independent timing functions.
float guidance_indi_pos_gain
Definition: guidance_indi.c:58
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
struct FloatMat33 Ga_inv
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
struct RadioControl radio_control
Definition: radio_control.c:30
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:80
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:128
struct FloatEulers guidance_euler_cmd
Inertial Measurement Unit interface.
#define RADIO_THROTTLE
Definition: intermcu_ap.h:39
float guidance_indi_speed_gain
Definition: guidance_indi.c:64
float guidance_indi_max_bank
#define THRUST_INCREMENT_ID
Core autopilot interface common to all firmwares.
#define MAT33_VECT3_MUL(_vout, _mat, _vin)
Definition: pprz_algebra.h:463
struct Int32Vect2 pos
with INT32_POS_FRAC
Definition: guidance_h.h:79
unsigned char uint8_t
Definition: types.h:14
API to get/set the generic vehicle states.
Butterworth2LowPass thrust_filt
#define RADIO_PITCH
Definition: intermcu_ap.h:41
struct FloatVect3 indi_accel_sp
Definition: guidance_indi.c:72
struct FloatVect3 control_increment
#define RMAT_ELMT(_rm, _row, _col)
Definition: pprz_algebra.h:660
General stabilization interface for rotorcrafts.
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
Definition: stabilization.c:32
static void guidance_indi_propagate_filters(struct FloatEulers *eulers)
Low pass the accelerometer measurements to remove noise from vibrations.
#define POS_FLOAT_OF_BFP(_ai)
void float_eulers_of_quat_yxz(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'YXZ' This function calculates from a quaternion the Euler angles with the order YXZ...
Second order low pass filter structure.
static struct NedCoor_i * stateGetPositionNed_i(void)
Get position in local NED coordinates (int).
Definition: state.h:665
Horizontal guidance for rotorcrafts.
signed char int8_t
Definition: types.h:15
#define MAT33_INV(_minv, _m)
Definition: pprz_algebra.h:489
Butterworth2LowPass roll_filt
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
float thrust_in
static void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz)
Rotorcraft attitude reference generation.
A guidance mode based on Incremental Nonlinear Dynamic Inversion.
void float_quat_of_eulers_yxz(struct FloatQuat *q, struct FloatEulers *e)
quat from euler rotation 'YXZ' This function calculates a quaternion from Euler angles with the order...
float x
in meters
INS for rotorcrafts combining vertical and horizontal filters.
bool indi_accel_sp_set_3d
Definition: guidance_indi.c:74