Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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
57 #else
59 #endif
60 
61 #ifdef 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 guidance_indi_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  // Use feed forward part from reference model
169  float speed_sp_x = pos_x_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.x);
170  float speed_sp_y = pos_y_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.y);
171  float speed_sp_z = pos_z_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_v_zd_ref);
172 
173  // If the acceleration setpoint is set over ABI message
174  if (indi_accel_sp_set_2d) {
175  sp_accel.x = indi_accel_sp.x;
176  sp_accel.y = indi_accel_sp.y;
177  // In 2D the vertical motion is derived from the flight plan
178  sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
180  // If the input command is not updated after a timeout, switch back to flight plan control
181  if (dt > 0.5) {
182  indi_accel_sp_set_2d = false;
183  }
184  } else if (indi_accel_sp_set_3d) {
185  sp_accel.x = indi_accel_sp.x;
186  sp_accel.y = indi_accel_sp.y;
187  sp_accel.z = indi_accel_sp.z;
189  // If the input command is not updated after a timeout, switch back to flight plan control
190  if (dt > 0.5) {
191  indi_accel_sp_set_3d = false;
192  }
193  } else {
197  }
198 
199 #if GUIDANCE_INDI_RC_DEBUG
200 #warning "GUIDANCE_INDI_RC_DEBUG lets you control the accelerations via RC, but disables autonomous flight!"
201  //for rc control horizontal, rotate from body axes to NED
202  float psi = stateGetNedToBodyEulers_f()->psi;
203  float rc_x = -(radio_control.values[RADIO_PITCH] / 9600.0) * 8.0;
204  float rc_y = (radio_control.values[RADIO_ROLL] / 9600.0) * 8.0;
205  sp_accel.x = cosf(psi) * rc_x - sinf(psi) * rc_y;
206  sp_accel.y = sinf(psi) * rc_x + cosf(psi) * rc_y;
207 
208  //for rc vertical control
209  sp_accel.z = -(radio_control.values[RADIO_THROTTLE] - 4500) * 8.0 / 9600.0;
210 #endif
211 
212  //Calculate matrix of partial derivatives
213  guidance_indi_calcG_yxz(&Ga, &eulers_yxz);
214 
215  //Invert this matrix
216  MAT33_INV(Ga_inv, Ga);
217 
218  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]};
219 
220  //Bound the acceleration error so that the linearization still holds
221  Bound(a_diff.x, -6.0, 6.0);
222  Bound(a_diff.y, -6.0, 6.0);
223  Bound(a_diff.z, -9.0, 9.0);
224 
225  //If the thrust to specific force ratio has been defined, include vertical control
226  //else ignore the vertical acceleration error
227 #ifndef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
228 #ifndef STABILIZATION_ATTITUDE_INDI_FULL
229  a_diff.z = 0.0;
230 #endif
231 #endif
232 
233  //Calculate roll,pitch and thrust command
235 
236  AbiSendMsgTHRUST(THRUST_INCREMENT_ID, control_increment.z);
237 
238  guidance_euler_cmd.theta = pitch_filt.o[0] + control_increment.x;
239  guidance_euler_cmd.phi = roll_filt.o[0] + control_increment.y;
240  guidance_euler_cmd.psi = *heading_sp;
241 
242 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
243  guidance_indi_filter_thrust();
244 
245  //Add the increment in specific force * specific_force_to_thrust_gain to the filtered thrust
247  Bound(thrust_in, 0, 9600);
248 
249 #if GUIDANCE_INDI_RC_DEBUG
250  if (radio_control.values[RADIO_THROTTLE] < 300) {
251  thrust_in = 0;
252  }
253 #endif
254 
255  //Overwrite the thrust command from guidance_v
256  stabilization_cmd[COMMAND_THRUST] = thrust_in;
257 #endif
258 
259  //Bound euler angles to prevent flipping
262 
263  //set the quat setpoint with the calculated roll and pitch
264  struct FloatQuat q_sp;
267 }
268 
269 #ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
270 
273 void guidance_indi_filter_thrust(void)
274 {
275  // Actuator dynamics
276  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
277 
278  // same filter as for the acceleration
280 }
281 #endif
282 
289 {
290  struct NedCoor_f *accel = stateGetAccelNed_f();
291  update_butterworth_2_low_pass(&filt_accel_ned[0], accel->x);
292  update_butterworth_2_low_pass(&filt_accel_ned[1], accel->y);
293  update_butterworth_2_low_pass(&filt_accel_ned[2], accel->z);
294 
295  update_butterworth_2_low_pass(&roll_filt, eulers->phi);
296  update_butterworth_2_low_pass(&pitch_filt, eulers->theta);
297 }
298 
306 void guidance_indi_calcG_yxz(struct FloatMat33 *Gmat, struct FloatEulers *euler_yxz)
307 {
308 
309  float sphi = sinf(euler_yxz->phi);
310  float cphi = cosf(euler_yxz->phi);
311  float stheta = sinf(euler_yxz->theta);
312  float ctheta = cosf(euler_yxz->theta);
313  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
314  float T = -9.81;
315 
316  RMAT_ELMT(*Gmat, 0, 0) = ctheta * cphi * T;
317  RMAT_ELMT(*Gmat, 1, 0) = 0;
318  RMAT_ELMT(*Gmat, 2, 0) = -stheta * cphi * T;
319  RMAT_ELMT(*Gmat, 0, 1) = -stheta * sphi * T;
320  RMAT_ELMT(*Gmat, 1, 1) = -cphi * T;
321  RMAT_ELMT(*Gmat, 2, 1) = -ctheta * sphi * T;
322  RMAT_ELMT(*Gmat, 0, 2) = stheta * cphi;
323  RMAT_ELMT(*Gmat, 1, 2) = -sphi;
324  RMAT_ELMT(*Gmat, 2, 2) = ctheta * cphi;
325 }
326 
335 {
336 
337  struct FloatEulers *euler = stateGetNedToBodyEulers_f();
338 
339  float sphi = sinf(euler->phi);
340  float cphi = cosf(euler->phi);
341  float stheta = sinf(euler->theta);
342  float ctheta = cosf(euler->theta);
343  float spsi = sinf(euler->psi);
344  float cpsi = cosf(euler->psi);
345  //minus gravity is a guesstimate of the thrust force, thrust measurement would be better
346  float T = -9.81;
347 
348  RMAT_ELMT(*Gmat, 0, 0) = (cphi * spsi - sphi * cpsi * stheta) * T;
349  RMAT_ELMT(*Gmat, 1, 0) = (-sphi * spsi * stheta - cpsi * cphi) * T;
350  RMAT_ELMT(*Gmat, 2, 0) = -ctheta * sphi * T;
351  RMAT_ELMT(*Gmat, 0, 1) = (cphi * cpsi * ctheta) * T;
352  RMAT_ELMT(*Gmat, 1, 1) = (cphi * spsi * ctheta) * T;
353  RMAT_ELMT(*Gmat, 2, 1) = -stheta * cphi * T;
354  RMAT_ELMT(*Gmat, 0, 2) = sphi * spsi + cphi * cpsi * stheta;
355  RMAT_ELMT(*Gmat, 1, 2) = cphi * spsi * stheta - cpsi * sphi;
356  RMAT_ELMT(*Gmat, 2, 2) = cphi * ctheta;
357 }
358 
363 static void accel_sp_cb(uint8_t sender_id __attribute__((unused)), uint8_t flag, struct FloatVect3 *accel_sp)
364 {
365  if (flag == 0) {
366  indi_accel_sp.x = accel_sp->x;
367  indi_accel_sp.y = accel_sp->y;
368  indi_accel_sp_set_2d = true;
370  } else if (flag == 1) {
371  indi_accel_sp.x = accel_sp->x;
372  indi_accel_sp.y = accel_sp->y;
373  indi_accel_sp.z = accel_sp->z;
374  indi_accel_sp_set_3d = true;
376  }
377 }
378 
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:41
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:70
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:96
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.
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
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.
#define SPEED_FLOAT_OF_BFP(_ai)
struct HorizontalGuidanceReference ref
reference calculated from setpoints
Definition: guidance_h.h:104
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:134
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
float theta
in radians
abi_event accel_sp_ev
Definition: guidance_indi.c:70
Butterworth2LowPass pitch_filt
float filter_cutoff
struct Int32Vect2 speed
with INT32_SPEED_FRAC
Definition: guidance_h.h:83
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
void guidance_indi_run(float *heading_sp)
static struct NedCoor_f * stateGetAccelNed_f(void)
Get acceleration in NED coordinates (float).
Definition: state.h:1038
#define ACCEL_FLOAT_OF_BFP(_ai)
struct FloatMat33 Ga_inv
#define QUAT_BFP_OF_REAL(_qi, _qf)
Definition: pprz_algebra.h:752
int32_t guidance_v_zdd_ref
vertical acceleration reference in meter/s^2.
Definition: guidance_v.c:134
struct RadioControl radio_control
Definition: radio_control.c:30
struct HorizontalGuidance guidance_h
Definition: guidance_h.c:90
int32_t guidance_v_z_ref
altitude reference in meters.
Definition: guidance_v.c:132
struct FloatEulers guidance_euler_cmd
Inertial Measurement Unit interface.
#define RADIO_THROTTLE
Definition: intermcu_ap.h:40
float guidance_indi_speed_gain
Definition: guidance_indi.c:64
int32_t guidance_v_zd_ref
vertical speed reference in meter/s.
Definition: guidance_v.c:133
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:82
#define GUIDANCE_INDI_SPEED_GAIN
API to get/set the generic vehicle states.
Butterworth2LowPass thrust_filt
#define RADIO_PITCH
Definition: intermcu_ap.h:42
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
#define GUIDANCE_INDI_POS_GAIN
static void guidance_indi_propagate_filters(struct FloatEulers *eulers)
Low pass the accelerometer measurements to remove noise from vibrations.
struct Int32Vect2 accel
with INT32_ACCEL_FRAC
Definition: guidance_h.h:84
#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.
#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
float guidance_indi_specific_force_gain
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
INS for rotorcrafts combining vertical and horizontal filters.
bool indi_accel_sp_set_3d
Definition: guidance_indi.c:74