Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_indi_simple.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Ewoud Smeur <ewoud_smeur@msn.com>
3 * MAVLab Delft University of Technology
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, write to
19 * the Free Software Foundation, 59 Temple Place - Suite 330,
20 * Boston, MA 02111-1307, USA.
21 */
22
36
37#include "state.h"
38#include "generated/airframe.h"
39#include "paparazzi.h"
42
43#if defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
44#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
45#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
46#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
47#else
48#if !defined(STABILIZATION_INDI_ACT_FREQ_P) && !defined(STABILIZATION_INDI_ACT_FREQ_Q) && !defined(STABILIZATION_INDI_ACT_FREQ_R)
49#warning You have to define the corner frequency of the first order actuator dynamics model in rad/s!
50#endif
51#endif
52
53// these parameters are used in the filtering of the angular acceleration
54// define them in the airframe file if different values are required
55#ifndef STABILIZATION_INDI_FILT_CUTOFF
56#define STABILIZATION_INDI_FILT_CUTOFF 8.0
57#endif
58
59// the yaw sometimes requires more filtering
60#ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
61#define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
62#endif
63
64#ifndef STABILIZATION_INDI_MAX_RATE
65#define STABILIZATION_INDI_MAX_RATE 6.0
66#endif
67
68#if STABILIZATION_INDI_USE_ADAPTIVE
69#warning "Use caution with adaptive indi. See the wiki for more info"
70#endif
71
72#ifndef STABILIZATION_INDI_MAX_R
73#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
74#endif
75
76#ifndef STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
77#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
78#endif
79
80#ifdef STABILIZATION_INDI_FILT_CUTOFF_P
81#define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
82#else
83#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
84#endif
85
86#ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
87#define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
88#else
89#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
90#endif
91
92#ifdef STABILIZATION_INDI_FILT_CUTOFF_R
93#define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
94#else
95#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
96#endif
97
100
102
103static inline void lms_estimation(void);
104static void indi_init_filters(void);
105
106//The G values are scaled to avoid numerical problems during the estimation
107#define INDI_EST_SCALE 0.001
108
111 .max_rate = STABILIZATION_INDI_MAX_RATE,
112 .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
113
116 .gains = {
117 .att = {
121 },
122 .rate = {
126 },
127 },
128
129 /* Estimation parameters for adaptive INDI */
130 .est = {
131 .g1 = {
135 },
138 },
139
140#if STABILIZATION_INDI_USE_ADAPTIVE
141 .adaptive = TRUE,
142#else
143 .adaptive = FALSE,
144#endif
145};
146
147#if PERIODIC_TELEMETRY
149
150static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
151{
152 float zero = 0.0;
153 float temp_indi_rate[3] = {indi.rate[0].o[0], indi.rate[1].o[0], indi.rate[2].o[0]};
156 1, &zero, // att des
157 1, &zero, // att
158 1, &zero, // att ref
159 3, temp_indi_rate, // rate
160 1, &zero, // rate ref
161 3, indi.rate_d, // ang.acc = rate.diff
162 3, temp_indi_ang_acc_ref, // ang.acc.ref
163 1, &zero, // jerk ref
164 1, &zero); // u
165}
166static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
167{
168 //The estimated G values are scaled, so scale them back before sending
169 struct FloatRates g1_disp;
171 float g2_disp = indi.est.g2 * INDI_EST_SCALE;
172 float zero = 0.0;
174 1, &g1_disp.p,
175 1, &g1_disp.q,
176 1, &g1_disp.r,
177 1, &g2_disp,
178 1, &zero);
179}
180
181static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
182{
183 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
189 &(quat->qi),
190 &(quat->qx),
191 &(quat->qy),
192 &(quat->qz));
193}
194#endif
195
217
219{
220 // tau = 1/(2*pi*Fc)
221 float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
222 float tau_rdot = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_RDOT);
223 float tau_axis[3] = {tau, tau, tau_rdot};
225 float sample_time = 1.0 / PERIODIC_FREQUENCY;
226 // Filtering of gyroscope and actuators
227 for (int8_t i = 0; i < 3; i++) {
232 }
233
234 // Init rate filter for feedback
236
240}
241
242// Callback function for setting cutoff frequency for r
249
251{
252 /* reset psi setpoint to current psi angle */
254
258
259 // Re-initialize filters
261}
262
269static inline void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
270{
274}
275
284{
285 for (int8_t i = 0; i < 3; i++) {
286 output[i] = (filter[i].o[0] - filter[i].o[1]) * PERIODIC_FREQUENCY;
287 }
288}
289
297static inline void finite_difference(float output[3], float new[3], float old[3])
298{
299 for (int8_t i = 0; i < 3; i++) {
300 output[i] = (new[i] - old[i])*PERIODIC_FREQUENCY;
301 }
302}
303
312void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
313{
314
315 //Propagate input filters
316 //first order actuator dynamics
320
321 // Propagate the filter on the gyroscopes and actuators
325
326 // Calculate the derivative of the rates
328
329 //The rates used for feedback are by default the measured rates.
330 //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
331 //Note that due to the delay, the PD controller may need relaxed gains.
332 struct FloatRates rates_filt;
333#if STABILIZATION_INDI_FILTER_ROLL_RATE
335#else
336 rates_filt.p = body_rates->p;
337#endif
338#if STABILIZATION_INDI_FILTER_PITCH_RATE
340#else
341 rates_filt.q = body_rates->q;
342#endif
343#if STABILIZATION_INDI_FILTER_YAW_RATE
345#else
346 rates_filt.r = body_rates->r;
347#endif
348
349 //This lets you impose a maximum yaw rate.
352
353 // Compute reference angular acceleration:
357
358 //Increment in angular acceleration requires increment in control input
359 //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
360 //It takes care of the angular acceleration caused by the change in rotation rate of the propellers
361 //(they have significant inertia, see the paper mentioned in the header for more explanation)
362 indi.du.p = 1.0 / indi.g1.p * (indi.angular_accel_ref.p - indi.rate_d[0]);
363 indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
364 indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
365
366 //Don't increment if thrust is off and on the ground
367 //without this the inputs will increment to the maximum before even getting in the air.
368 if (th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z) < 300 && !in_flight) {
370
371 // If on the gournd, no increments, just proportional control
372 indi.u_in.p = indi.du.p;
373 indi.u_in.q = indi.du.q;
374 indi.u_in.r = indi.du.r;
375 } else {
376 //add the increment to the total control input
377 indi.u_in.p = indi.u[0].o[0] + indi.du.p;
378 indi.u_in.q = indi.u[1].o[0] + indi.du.q;
379 indi.u_in.r = indi.u[2].o[0] + indi.du.r;
380
381 // only run the estimation if the commands are not zero.
383 }
384
385 //bound the total control input
386#if STABILIZATION_INDI_FULL_AUTHORITY
387 Bound(indi.u_in.p, -9600, 9600);
388 Bound(indi.u_in.q, -9600, 9600);
389 float rlim = 9600 - fabs(indi.u_in.q);
390 Bound(indi.u_in.r, -rlim, rlim);
391 Bound(indi.u_in.r, -9600, 9600);
392#else
393 Bound(indi.u_in.p, -4500, 4500);
394 Bound(indi.u_in.q, -4500, 4500);
395 Bound(indi.u_in.r, -4500, 4500);
396#endif
397
398 /* INDI feedback */
399 cmd[COMMAND_ROLL] = indi.u_in.p;
400 cmd[COMMAND_PITCH] = indi.u_in.q;
401 cmd[COMMAND_YAW] = indi.u_in.r;
403
404 /* bound the result */
409}
410
417void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
418{
419 stab_att_sp_euler = stab_sp_to_eulers_i(att_sp); // stab_att_sp_euler.psi still used in ref..
420 stab_att_sp_quat = stab_sp_to_quat_i(att_sp); // quat attitude setpoint
421
422 /* attitude error in float */
423 struct FloatQuat att_err;
426
428
429 struct FloatVect3 att_fb;
430#if TILT_TWIST_CTRL
431 struct FloatQuat tilt;
432 struct FloatQuat twist;
434 att_fb.x = tilt.qx;
435 att_fb.y = tilt.qy;
436 att_fb.z = twist.qz;
437#else
438 att_fb.x = att_err.qx;
439 att_fb.y = att_err.qy;
440 att_fb.z = att_err.qz;
441#endif
442
443 struct FloatRates rate_sp;
444 // Divide by rate gain to make it equivalent to a parallel structure
448
449 // Add feed-forward rates to the attitude feedback part
452
453 /* compute the INDI command */
455 stabilization_indi_rate_run(in_flight, &sp, thrust, cmd);
456}
457
464static inline void lms_estimation(void)
465{
466 static struct IndiEstimation *est = &indi.est;
467 // Only pass really low frequencies so you don't adapt to noise
469 filter_pqr(est->u, &indi.u_act_dyn);
471
472 // Calculate the first and second derivatives of the rates and actuators
473 float rate_d_prev[3];
474 float u_d_prev[3];
476 float_vect_copy(u_d_prev, est->u_d, 3);
480 finite_difference(est->u_dd, est->u_d, u_d_prev);
481
482 // The inputs are scaled in order to avoid overflows
483 float du[3];
484 float_vect_copy(du, est->u_d, 3);
486 est->g1.p = est->g1.p - (est->g1.p * du[0] - est->rate_dd[0]) * du[0] * est->mu;
487 est->g1.q = est->g1.q - (est->g1.q * du[1] - est->rate_dd[1]) * du[1] * est->mu;
488 float ddu = est->u_dd[2] * INDI_EST_SCALE / PERIODIC_FREQUENCY;
489 float error = (est->g1.r * du[2] + est->g2 * ddu - est->rate_dd[2]);
490 est->g1.r = est->g1.r - error * du[2] * est->mu / 3;
491 est->g2 = est->g2 - error * 1000 * ddu * est->mu / 3;
492
493 //the g values should be larger than zero, otherwise there is positive feedback, the command will go to max and there is nothing to learn anymore...
494 if (est->g1.p < 0.01) { est->g1.p = 0.01; }
495 if (est->g1.q < 0.01) { est->g1.q = 0.01; }
496 if (est->g1.r < 0.01) { est->g1.r = 0.01; }
497 if (est->g2 < 0.01) { est->g2 = 0.01; }
498
499 if (indi.adaptive) {
500 //Commit the estimated G values and apply the scaling
501 indi.g1.p = est->g1.p * INDI_EST_SCALE;
502 indi.g1.q = est->g1.q * INDI_EST_SCALE;
503 indi.g1.r = est->g1.r * INDI_EST_SCALE;
504 indi.g2 = est->g2 * INDI_EST_SCALE;
505 }
506}
507
float q
in rad/s
float p
in rad/s
float r
in rad/s
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, struct FloatQuat *a2b, struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
static void float_vect_scale(float *a, const float s, const int n)
a *= s
static void float_vect_copy(float *a, const float *b, const int n)
a = b
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, struct FloatQuat *quat)
Tilt twist decomposition of a quaternion (z axis)
#define FLOAT_RATES_ZERO(_r)
Roation quaternion.
angular rates
#define RATES_SMUL(_ro, _ri, _s)
#define RATES_ADD(_a, _b)
int32_t psi
in rad with INT32_ANGLE_FRAC
euler angles
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1276
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1294
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1367
static float p[2][2]
Simple first order low pass filter with bilinear transform.
float o[2]
output history
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, float value)
Update first order low pass filter state with a new value.
static void init_butterworth_2_low_pass(Butterworth2LowPass *filter, float tau, float sample_time, float value)
Init a second order Butterworth filter.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, float sample_time, float value)
Init first order low pass filter.
static float update_butterworth_2_low_pass(Butterworth2LowPass *filter, float value)
Update second order Butterworth low pass filter state with a new value.
First order low pass filter structure.
Second order low pass filter structure.
uint16_t foo
Definition main_demo5.c:58
#define MAX_PPRZ
Definition paparazzi.h:8
Generic interface for radio control modules.
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define THRUST_AXIS_Z
Quaternion transformation functions.
int32_t stabilization_attitude_get_heading_i(void)
Get attitude heading as int (avoiding jumps)
Read an attitude setpoint from the RC.
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_enter(void)
Function that resets important values upon engaging INDI.
static void finite_difference(float output[3], float new[3], float old[3])
Calculate derivative of an array via finite difference.
static struct FirstOrderLowPass rates_filt_fo[3]
#define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF
static void lms_estimation(void)
This is a Least Mean Squares adaptive filter It estimates the actuator effectiveness online,...
#define STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILT_CUTOFF_Q
static void send_att_indi(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff)
static void finite_difference_from_filter(float *output, Butterworth2LowPass *filter)
Caclulate finite difference form a filter array The filter already contains the previous values.
struct Int32Eulers stab_att_sp_euler
#define STABILIZATION_INDI_MAX_RATE
static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_device *dev)
void stabilization_indi_init(void)
Function that initializes important values upon engaging INDI.
#define STABILIZATION_INDI_FILT_CUTOFF
#define INDI_EST_SCALE
void stabilization_indi_attitude_run(bool in_flight, struct StabilizationSetpoint *att_sp, struct ThrustSetpoint *thrust, int32_t *cmd)
runs stabilization indi
void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Does the INDI calculations.
struct IndiVariables indi
#define STABILIZATION_INDI_MAX_R
#define STABILIZATION_INDI_FILT_CUTOFF_R
static void filter_pqr(Butterworth2LowPass *filter, struct FloatRates *new_values)
Update butterworth filter for p, q and r of a FloatRates struct.
struct Int32Quat stab_att_sp_quat
#define STABILIZATION_INDI_FILT_CUTOFF_RDOT
static void indi_init_filters(void)
struct Indi_gains gains
Butterworth2LowPass rate[3]
struct FloatRates act_dyn
struct IndiEstimation est
Estimation parameters for adaptive INDI.
Butterworth2LowPass u[3]
Butterworth2LowPass u[3]
struct FloatRates u_act_dyn
struct FloatRates u_in
struct FloatRates rate
struct FloatRates angular_accel_ref
struct FloatRates att
bool adaptive
Enable adataptive estimation.
float attitude_max_yaw_rate
Maximum yaw rate in atttiude control in rad/s.
Butterworth2LowPass rate[3]
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
Stabilization setpoint.
union StabilizationSetpoint::@278 sp
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition telemetry.h:66
int int32_t
Typedef defining 32 bit int type.
signed char int8_t
Typedef defining 8 bit char type.