Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_indi_hinf.c
Go to the documentation of this file.
1/*
2 * Copyright (C) Gautier Hattenberger, Mohamad Hachem
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
20
23#include "autopilot.h"
24
25static struct FloatRates rate_state;
26static struct FloatRates att_state;
27
28// proportional control part (attitude)
29static float Ap = STABILIZATION_INDI_HINF_Ap;
30static float Bp = STABILIZATION_INDI_HINF_Bp;
31static float Cp = STABILIZATION_INDI_HINF_Cp;
32static float Dp = STABILIZATION_INDI_HINF_Dp;
33// derivative control part (rates)
34static float Ad = STABILIZATION_INDI_HINF_Ad;
35static float Bd = STABILIZATION_INDI_HINF_Bd;
36static float Cd = STABILIZATION_INDI_HINF_Cd;
37static float Dd = STABILIZATION_INDI_HINF_Dd;
38
44{
45 struct FloatRates rate_error;
46 RATES_DIFF(rate_error, sp, rates);
47
48 struct FloatRates accel_ref;
49 if (autopilot_in_flight()) {
50 accel_ref.p = Cd * rate_state.p + Dd * rate_error.p;
51 rate_state.p = Ad * rate_state.p + Bd * rate_error.p;
52
53 accel_ref.q = Cd * rate_state.q + Dd * rate_error.q;
54 rate_state.q = Ad * rate_state.q + Bd * rate_error.q;
55
56 accel_ref.r = rate_error.r * indi_gains.rate.r;
57 } else {
59 FLOAT_RATES_ZERO(accel_ref);
60 }
61
62 return accel_ref;
63}
64
70struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
71{
72 /* attitude error */
73 struct FloatQuat att_err;
74 float_quat_inv_comp_norm_shortest(&att_err, &att, &att_sp);
75
76 struct FloatVect3 att_fb;
77#if TILT_TWIST_CTRL
78 struct FloatQuat tilt;
79 struct FloatQuat twist;
80 float_quat_tilt_twist(&tilt, &twist, &att_err);
81 att_fb.x = tilt.qx;
82 att_fb.y = tilt.qy;
83 att_fb.z = twist.qz;
84#else
85 att_fb.x = att_err.qx;
86 att_fb.y = att_err.qy;
87 att_fb.z = att_err.qz;
88#endif
89
90 struct FloatRates rate_sp;
91 if (autopilot_in_flight()) {
92 rate_sp.p = Cp * att_state.p + Dp * att_fb.x;
93 att_state.p = Ap * att_state.p + Bp * att_fb.x;
94
95 rate_sp.q = Cp * att_state.q + Dp * att_fb.y;
96 att_state.q = Ap * att_state.q + Bp * att_fb.y;
97
98 rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;
99
100 // add feed-forward term
101 RATES_ADD(rate_sp, rates_ff);
102 } else {
104 FLOAT_RATES_ZERO(rate_sp);
105 }
106
107 return rate_sp;
108}
109
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
float q
in rad/s
float p
in rad/s
float r
in rad/s
void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, const struct FloatQuat *quat)
Tilt twist decomposition of quaternion.
#define FLOAT_RATES_ZERO(_r)
void float_quat_inv_comp_norm_shortest(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions with normalization.
Roation quaternion.
angular rates
#define RATES_ADD(_a, _b)
#define RATES_DIFF(_c, _a, _b)
static float Ad
static float Dp
static float Cp
static float Bd
static float Ap
static float Bp
static float Cd
static float Dd
Paparazzi floating point algebra.
struct FloatRates WEAK stabilization_indi_attitude_controller(struct FloatQuat att, struct FloatQuat att_sp, struct FloatRates rates_ff)
Default PD angular rate controller.
struct Indi_gains indi_gains
struct FloatRates WEAK stabilization_indi_rate_controller(struct FloatRates rates, struct FloatRates sp)
Default PD angular acceleration controller.
static struct FloatRates att_state
static struct FloatRates rate_state