Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_attitude_quat_float.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
41
43
45
47static struct FloatRates body_rate_d;
48
49float stabilization_att_fb_cmd[COMMANDS_NB];
50float stabilization_att_ff_cmd[COMMANDS_NB];
51
53
54static const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN;
55static const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN;
56static const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN;
57
58static const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN;
59static const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN;
60static const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN;
61
62static const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN;
63static const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN;
64static const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN;
65
66static const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN;
67static const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN;
68static const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN;
69
70static const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D;
71static const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D;
72static const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D;
73
74
75#if defined COMMAND_ROLL_SURFACE && defined COMMAND_PITCH_SURFACE && defined COMMAND_YAW_SURFACE
76#define HAS_SURFACE_COMMANDS 1
77#endif
78
79#ifdef HAS_SURFACE_COMMANDS
80static const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_PHI_PGAIN_SURFACE;
81static const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_THETA_PGAIN_SURFACE;
82static const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_PSI_PGAIN_SURFACE;
83
84static const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_PHI_DGAIN_SURFACE;
85static const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_THETA_DGAIN_SURFACE;
86static const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_PSI_DGAIN_SURFACE;
87
88static const float phi_igain_surface[] = STABILIZATION_ATTITUDE_PHI_IGAIN_SURFACE;
89static const float theta_igain_surface[] = STABILIZATION_ATTITUDE_THETA_IGAIN_SURFACE;
90static const float psi_igain_surface[] = STABILIZATION_ATTITUDE_PSI_IGAIN_SURFACE;
91
92static const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_PHI_DDGAIN_SURFACE;
93static const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_THETA_DDGAIN_SURFACE;
94static const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_PSI_DDGAIN_SURFACE;
95#endif
96
97#define IERROR_SCALE 1024
98
99#if PERIODIC_TELEMETRY
101
102static void send_att(struct transport_tx *trans, struct link_device *dev)
103{
104 struct FloatRates *body_rate = stateGetBodyRates_f();
106 pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
107 &(body_rate->p), &(body_rate->q), &(body_rate->r),
108 &(att->phi), &(att->theta), &(att->psi),
110 &stab_att_sp_euler.theta,
117 &stabilization_att_fb_cmd[COMMAND_YAW],
120 &stabilization_att_ff_cmd[COMMAND_YAW],
123 &stabilization.cmd[COMMAND_YAW],
125}
126
127static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
128{
129 pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
131 &stab_att_sp_euler.theta,
133 &att_ref_quat_f.euler.phi,
134 &att_ref_quat_f.euler.theta,
135 &att_ref_quat_f.euler.psi,
136 &att_ref_quat_f.rate.p,
137 &att_ref_quat_f.rate.q,
138 &att_ref_quat_f.rate.r,
139 &att_ref_quat_f.accel.p,
140 &att_ref_quat_f.accel.q,
141 &att_ref_quat_f.accel.r);
142}
143
144static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
145{
146 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
147 struct Int32Quat refquat;
148 QUAT_BFP_OF_REAL(refquat, att_ref_quat_f.quat);
149 pprz_msg_send_AHRS_REF_QUAT(trans, dev, AC_ID,
150 &refquat.qi,
151 &refquat.qx,
152 &refquat.qy,
153 &refquat.qz,
154 &(quat->qi),
155 &(quat->qx),
156 &(quat->qy),
157 &(quat->qz));
158}
159#endif
160
162{
163 /* setpoints */
166 /* reference */
169
170 for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
176#ifdef HAS_SURFACE_COMMANDS
177 VECT3_ASSIGN(stabilization_gains[i].surface_p, phi_pgain_surface[i], theta_pgain_surface[i], psi_pgain_surface[i]);
178 VECT3_ASSIGN(stabilization_gains[i].surface_d, phi_dgain_surface[i], theta_dgain_surface[i], psi_dgain_surface[i]);
179 VECT3_ASSIGN(stabilization_gains[i].surface_i, phi_igain_surface[i], theta_igain_surface[i], psi_igain_surface[i]);
180 VECT3_ASSIGN(stabilization_gains[i].surface_dd, phi_ddgain_surface[i], theta_ddgain_surface[i], psi_ddgain_surface[i]);
181#endif
182 }
183
187
188#if PERIODIC_TELEMETRY
189 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
190 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
192#endif
193}
194
196{
198 // This could be bad -- Just say no.
199 return;
200 }
201 gain_idx = idx;
203}
204
212
213#ifndef GAIN_PRESCALER_FF
214#define GAIN_PRESCALER_FF 1
215#endif
216static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
217{
218 /* Compute feedforward based on reference acceleration */
219
220 ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * ref_accel->p;
221 ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * ref_accel->q;
222 ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * ref_accel->r;
223#ifdef HAS_SURFACE_COMMANDS
224 ff_commands[COMMAND_ROLL_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.x * ref_accel->p;
225 ff_commands[COMMAND_PITCH_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.y * ref_accel->q;
226 ff_commands[COMMAND_YAW_SURFACE] = GAIN_PRESCALER_FF * gains->surface_dd.z * ref_accel->r;
227#endif
228}
229
230#ifndef GAIN_PRESCALER_P
231#define GAIN_PRESCALER_P 1
232#endif
233#ifndef GAIN_PRESCALER_D
234#define GAIN_PRESCALER_D 1
235#endif
236#ifndef GAIN_PRESCALER_I
237#define GAIN_PRESCALER_I 1
238#endif
239static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err,
240 struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
241{
242 /* PID feedback */
243 fb_commands[COMMAND_ROLL] =
244 GAIN_PRESCALER_P * gains->p.x * att_err->qx +
245 GAIN_PRESCALER_D * gains->d.x * rate_err->p +
246 GAIN_PRESCALER_D * gains->rates_d.x * rate_err_d->p +
247 GAIN_PRESCALER_I * gains->i.x * sum_err->qx;
248
249 fb_commands[COMMAND_PITCH] =
250 GAIN_PRESCALER_P * gains->p.y * att_err->qy +
251 GAIN_PRESCALER_D * gains->d.y * rate_err->q +
252 GAIN_PRESCALER_D * gains->rates_d.y * rate_err_d->q +
253 GAIN_PRESCALER_I * gains->i.y * sum_err->qy;
254
255 fb_commands[COMMAND_YAW] =
256 GAIN_PRESCALER_P * gains->p.z * att_err->qz +
257 GAIN_PRESCALER_D * gains->d.z * rate_err->r +
258 GAIN_PRESCALER_D * gains->rates_d.z * rate_err_d->r +
259 GAIN_PRESCALER_I * gains->i.z * sum_err->qz;
260
261#ifdef HAS_SURFACE_COMMANDS
262 fb_commands[COMMAND_ROLL_SURFACE] =
263 GAIN_PRESCALER_P * gains->surface_p.x * att_err->qx +
264 GAIN_PRESCALER_D * gains->surface_d.x * rate_err->p +
265 GAIN_PRESCALER_I * gains->surface_i.x * sum_err->qx;
266
267 fb_commands[COMMAND_PITCH_SURFACE] =
268 GAIN_PRESCALER_P * gains->surface_p.y * att_err->qy +
269 GAIN_PRESCALER_D * gains->surface_d.y * rate_err->q +
270 GAIN_PRESCALER_I * gains->surface_i.y * sum_err->qy;
271
272 fb_commands[COMMAND_YAW_SURFACE] =
273 GAIN_PRESCALER_P * gains->surface_p.z * att_err->qz +
274 GAIN_PRESCALER_D * gains->surface_d.z * rate_err->r +
275 GAIN_PRESCALER_I * gains->surface_i.z * sum_err->qz;
276#endif
277}
278
279void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
280{
283
284 /*
285 * Update reference
286 */
287 static const float dt = (1./PERIODIC_FREQUENCY);
289
290 /*
291 * Compute errors for feedback
292 */
293
294 /* attitude error */
295 struct FloatQuat att_err;
296 struct FloatQuat *att_quat = stateGetNedToBodyQuat_f();
297 float_quat_inv_comp(&att_err, att_quat, &att_ref_quat_f.quat);
298 /* wrap it in the shortest direction */
299 float_quat_wrap_shortest(&att_err);
300
301 /* rate error */
302 struct FloatRates rate_err;
303 struct FloatRates *body_rate = stateGetBodyRates_f();
304 RATES_DIFF(rate_err, att_ref_quat_f.rate, *body_rate);
305 /* rate_d error */
307 RATES_COPY(last_body_rate, *body_rate);
308
309#define INTEGRATOR_BOUND 1.0
310 /* integrated error */
311 if (enable_integrator) {
318 } else {
319 /* reset accumulator */
321 }
322
324
327
330 cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
331 cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
332
333#ifdef HAS_SURFACE_COMMANDS
334 cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] +
335 stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE];
336 cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] +
337 stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE];
338 cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] +
339 stabilization_att_ff_cmd[COMMAND_YAW_SURFACE];
340#endif
341
342 /* bound the result */
343 BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
344 BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
345 BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
346 BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
347#ifdef HAS_SURFACE_COMMANDS
348 BoundAbs(cmd[COMMAND_ROLL_SURFACE], MAX_PPRZ);
349 BoundAbs(cmd[COMMAND_PITCH_SURFACE], MAX_PPRZ);
350 BoundAbs(cmd[COMMAND_YAW_SURFACE], MAX_PPRZ);
351#endif
352}
353
static struct uart_periph * dev
#define COMMAND_PITCH
#define COMMAND_ROLL
float q
in rad/s
float phi
in radians
float p
in rad/s
float r
in rad/s
float theta
in radians
float psi
in radians
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
#define FLOAT_EULERS_ZERO(_e)
void float_quat_inv_comp(struct FloatQuat *b2c, const struct FloatQuat *a2b, const struct FloatQuat *a2c)
Composition (multiplication) of two quaternions.
static void float_quat_wrap_shortest(struct FloatQuat *q)
#define FLOAT_RATES_ZERO(_r)
euler angles
Roation quaternion.
angular rates
#define RATES_COPY(_a, _b)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define RATES_DIFF(_c, _a, _b)
Rotation quaternion.
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition state.h:1295
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static float p[2][2]
static uint32_t idx
float gains[MAX_SAMPLES_LEARNING]
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi floating point algebra.
Paparazzi fixed point algebra.
struct Stabilization stabilization
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
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
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)
static struct Int32Quat stab_att_sp_quat
static void send_ahrs_ref_quat(struct transport_tx *trans, struct link_device *dev)
static void attitude_run_ff(float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
static const float theta_ddgain[]
static struct FloatRates last_body_rate
struct AttRefQuatFloat att_ref_quat_f
void stabilization_attitude_enter(void)
Attitude control enter function.
static const float theta_igain[]
void stabilization_attitude_run(bool enable_integrator, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
static const float phi_dgain_d[]
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
static const float theta_pgain[]
void stabilization_attitude_quat_float_init(void)
static const float psi_igain[]
static const float theta_dgain_d[]
static const float phi_ddgain[]
static const float phi_igain[]
static const float psi_pgain[]
struct FloatQuat stabilization_att_sum_err_quat
static const float phi_pgain[]
static const float theta_dgain[]
static const float psi_ddgain[]
static void attitude_run_fb(float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err, struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
static const float phi_dgain[]
static struct FloatRates body_rate_d
static const float psi_dgain[]
void stabilization_attitude_gain_schedule(uint8_t idx)
static const float psi_dgain_d[]
#define INTEGRATOR_BOUND
static void send_att(struct transport_tx *trans, struct link_device *dev)
Rotorcraft attitude stabilization in quaternion float version.
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define STABILIZATION_ATTITUDE_GAIN_NB
Quaternion transformation functions.
void attitude_ref_quat_float_update(struct AttRefQuatFloat *ref, struct FloatQuat *sp_quat, float dt)
void attitude_ref_quat_float_enter(struct AttRefQuatFloat *ref, struct FloatQuat *state_quat)
void attitude_ref_quat_float_init(struct AttRefQuatFloat *ref)
void attitude_ref_quat_float_schedule(struct AttRefQuatFloat *ref, uint8_t idx)
Attitude reference models and state/output (float).
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.