Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_attitude_quat_int.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2008-2009 Antoine Drouin <poinix@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
25
26#include "generated/airframe.h"
27
30
31#include "std.h"
32#include "paparazzi.h"
35#include "state.h"
36
38#ifndef STABILIZATION_ATTITUDE_PHI_FFDGAIN
39#define STABILIZATION_ATTITUDE_PHI_FFDGAIN 0
40#endif
41#ifndef STABILIZATION_ATTITUDE_THETA_FFDGAIN
42#define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0
43#endif
44#ifndef STABILIZATION_ATTITUDE_PSI_FFDGAIN
45#define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0
46#endif
47
49 {STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
50 {STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
51 {STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
52 {STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN },
54};
55
56/* warn if some gains are still negative */
57#if (STABILIZATION_ATTITUDE_PHI_PGAIN < 0) || \
58 (STABILIZATION_ATTITUDE_THETA_PGAIN < 0) || \
59 (STABILIZATION_ATTITUDE_PSI_PGAIN < 0) || \
60 (STABILIZATION_ATTITUDE_PHI_DGAIN < 0) || \
61 (STABILIZATION_ATTITUDE_THETA_DGAIN < 0) || \
62 (STABILIZATION_ATTITUDE_PSI_DGAIN < 0) || \
63 (STABILIZATION_ATTITUDE_PHI_IGAIN < 0) || \
64 (STABILIZATION_ATTITUDE_THETA_IGAIN < 0) || \
65 (STABILIZATION_ATTITUDE_PSI_IGAIN < 0)
66#error "ALL control gains have to be positive!!!"
67#endif
68
70
73
76
78
79#define IERROR_SCALE 128
80#define GAIN_PRESCALER_FF 48
81#define GAIN_PRESCALER_P 12
82#define GAIN_PRESCALER_D 3
83#define GAIN_PRESCALER_I 3
84
85#if PERIODIC_TELEMETRY
87
88static void send_att(struct transport_tx *trans, struct link_device *dev) //FIXME really use this message here ?
89{
90 struct Int32Rates *body_rate = stateGetBodyRates_i();
92 pprz_msg_send_STAB_ATTITUDE_INT(trans, dev, AC_ID,
93 &(body_rate->p), &(body_rate->q), &(body_rate->r),
94 &(att->phi), &(att->theta), &(att->psi),
96 &stab_att_sp_euler.theta,
103 &stabilization_att_fb_cmd[COMMAND_YAW],
106 &stabilization_att_ff_cmd[COMMAND_YAW],
109 &stabilization.cmd[COMMAND_YAW]);
110}
111
112static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
113{
114 // ref eulers in message are with REF_ANGLE_FRAC, convert
115 struct Int32Eulers ref_euler;
117 pprz_msg_send_STAB_ATTITUDE_REF_INT(trans, dev, AC_ID,
119 &stab_att_sp_euler.theta,
121 &ref_euler.phi,
122 &ref_euler.theta,
123 &ref_euler.psi,
124 &att_ref_quat_i.rate.p,
125 &att_ref_quat_i.rate.q,
126 &att_ref_quat_i.rate.r,
127 &att_ref_quat_i.accel.p,
128 &att_ref_quat_i.accel.q,
129 &att_ref_quat_i.accel.r);
130}
131
132static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
133{
134 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
135 pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
136 &att_ref_quat_i.quat.qi,
137 &att_ref_quat_i.quat.qx,
138 &att_ref_quat_i.quat.qy,
139 &att_ref_quat_i.quat.qz,
140 &(quat->qi),
141 &(quat->qx),
142 &(quat->qy),
143 &(quat->qz));
144}
145#endif
146
148{
151
152#if PERIODIC_TELEMETRY
153 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_INT, send_att);
154 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_INT, send_att_ref);
156#endif
157}
158
166
167#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
168#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
169
170static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
171{
172 /* Compute feedforward based on reference acceleration */
173
174 ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7);
175 ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7);
176 ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7);
177}
178
179static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err,
180 struct Int32Rates *rate_err, struct Int32Quat *sum_err)
181{
182 /* PID feedback */
183 fb_commands[COMMAND_ROLL] =
184 GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) +
185 GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) +
186 GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx);
187
188 fb_commands[COMMAND_PITCH] =
189 GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) +
190 GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) +
191 GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy);
192
193 fb_commands[COMMAND_YAW] =
194 GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) +
195 GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) +
196 GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz);
197
198}
199
200void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
201{
204
205 /*
206 * Update reference
207 * Warning: dt is currently not used in the quat_int ref impl
208 * PERIODIC_FREQUENCY is assumed to be 512Hz
209 */
210 static const float dt = (1./PERIODIC_FREQUENCY);
212
213 /*
214 * Compute errors for feedback
215 */
216
217 /* attitude error */
218 struct Int32Quat att_err;
219 struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
220 int32_quat_inv_comp(&att_err, att_quat, &att_ref_quat_i.quat);
221 /* wrap it in the shortest direction */
222 int32_quat_wrap_shortest(&att_err);
223 int32_quat_normalize(&att_err);
224
225 /* rate error */
226 const struct Int32Rates rate_ref_scaled = {
230 };
231 struct Int32Rates rate_err;
232 struct Int32Rates *body_rate = stateGetBodyRates_i();
233 RATES_DIFF(rate_err, rate_ref_scaled, (*body_rate));
234
235#define INTEGRATOR_BOUND 100000
236 /* integrated error */
237 if (enable_integrator) {
244 } else {
245 /* reset accumulator */
247 }
248
249 /* compute the feed forward command */
251
252 /* compute the feed back command */
254
255 /* sum feedforward and feedback */
258 cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
259 cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
260
261 /* bound the result */
262 BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
263 BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
264 BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
265 BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
266}
267
static struct uart_periph * dev
#define COMMAND_PITCH
#define COMMAND_ROLL
#define RATES_DIFF(_c, _a, _b)
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
static void int32_quat_normalize(struct Int32Quat *q)
normalize a quaternion inplace
#define QUAT1_FLOAT_OF_BFP(_qi)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
#define RATE_FLOAT_OF_BFP(_ai)
static void int32_quat_wrap_shortest(struct Int32Quat *q)
#define INT32_ANGLE_FRAC
#define INT32_EULERS_LSHIFT(_o, _i, _r)
#define INT32_RATE_FRAC
void int32_quat_inv_comp(struct Int32Quat *b2c, struct Int32Quat *a2b, struct Int32Quat *a2c)
Composition (multiplication) of two quaternions.
euler angles
Rotation quaternion.
angular rates
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static struct Int32Rates * stateGetBodyRates_i(void)
Get vehicle body angular rate (int).
Definition state.h:1383
float gains[MAX_SAMPLES_LEARNING]
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
struct Stabilization stabilization
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
int32_t th_sp_to_thrust_i(struct ThrustSetpoint *th, int32_t thrust, uint8_t axis)
#define THRUST_AXIS_Z
float stabilization_att_ff_cmd[COMMANDS_NB]
float stabilization_att_fb_cmd[COMMANDS_NB]
struct Int32AttitudeGains stabilization_gains
static struct FloatEulers stab_att_sp_euler
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
static void send_att(struct transport_tx *trans, struct link_device *dev)
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN
explicitly define to zero to disable feed-forward rate term by default
static struct Int32Quat stab_att_sp_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
struct FloatQuat stabilization_att_sum_err_quat
#define INTEGRATOR_BOUND
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
void stabilization_attitude_enter(void)
Attitude control enter function.
void stabilization_attitude_quat_int_init(void)
void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
#define OFFSET_AND_ROUND(_a, _b)
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *sum_err)
static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
struct AttRefQuatInt att_ref_quat_i
static void send_att(struct transport_tx *trans, struct link_device *dev)
Quaternion transformation functions.
#define REF_RATE_FRAC
#define REF_ANGLE_FRAC
void attitude_ref_quat_int_update(struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
Propagate reference.
void attitude_ref_quat_int_enter(struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
void attitude_ref_quat_int_init(struct AttRefQuatInt *ref)
Attitude reference models and state/output (quat int).
API to get/set the generic vehicle states.
Stabilization setpoint.
Thrust setpoint // TODO to a setpoint header Structure to store the desired thrust vector with differ...
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_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.