Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_attitude_euler_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
27
28#include "generated/airframe.h"
29
32
33#include "std.h"
34#include "paparazzi.h"
36#include "state.h"
37
38#ifndef USE_ATT_REF
39#define USE_ATT_REF 1
40#endif
41
44
47
48float stabilization_att_fb_cmd[COMMANDS_NB];
49float stabilization_att_ff_cmd[COMMANDS_NB];
50
51#if PERIODIC_TELEMETRY
53
54static void send_att(struct transport_tx *trans, struct link_device *dev)
55{
56 struct FloatRates *body_rate = stateGetBodyRates_f();
58 float foo = 0.0;
59 pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
60 &(body_rate->p), &(body_rate->q), &(body_rate->r),
61 &(att->phi), &(att->theta), &(att->psi),
63 &stab_att_sp_euler.theta,
70 &stabilization_att_fb_cmd[COMMAND_YAW],
73 &stabilization_att_ff_cmd[COMMAND_YAW],
76 &stabilization.cmd[COMMAND_YAW],
77 &foo, &foo, &foo);
78}
79
80static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
81{
82 pprz_msg_send_STAB_ATTITUDE_REF_FLOAT(trans, dev, AC_ID,
84 &stab_att_sp_euler.theta,
86 &att_ref_euler_f.euler.phi,
87 &att_ref_euler_f.euler.theta,
88 &att_ref_euler_f.euler.psi,
89 &att_ref_euler_f.rate.p,
90 &att_ref_euler_f.rate.q,
91 &att_ref_euler_f.rate.r,
92 &att_ref_euler_f.accel.p,
93 &att_ref_euler_f.accel.q,
94 &att_ref_euler_f.accel.r);
95}
96#endif
97
99{
100
102
104 STABILIZATION_ATTITUDE_PHI_PGAIN,
105 STABILIZATION_ATTITUDE_THETA_PGAIN,
106 STABILIZATION_ATTITUDE_PSI_PGAIN);
107
109 STABILIZATION_ATTITUDE_PHI_DGAIN,
110 STABILIZATION_ATTITUDE_THETA_DGAIN,
111 STABILIZATION_ATTITUDE_PSI_DGAIN);
112
114 STABILIZATION_ATTITUDE_PHI_IGAIN,
115 STABILIZATION_ATTITUDE_THETA_IGAIN,
116 STABILIZATION_ATTITUDE_PSI_IGAIN);
117
119 STABILIZATION_ATTITUDE_PHI_DDGAIN,
120 STABILIZATION_ATTITUDE_THETA_DDGAIN,
121 STABILIZATION_ATTITUDE_PSI_DDGAIN);
122
124
125#if PERIODIC_TELEMETRY
126 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_FLOAT, send_att);
127 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE_REF_FLOAT, send_att_ref);
128#endif
129}
130
138
139#define MAX_SUM_ERR 200
140
141void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
142{
144#if USE_ATT_REF
145 static const float dt = (1./PERIODIC_FREQUENCY);
147#else
151#endif
152
153 /* Compute feedforward */
158 stabilization_att_ff_cmd[COMMAND_YAW] =
160
161 /* Compute feedback */
162 /* attitude error */
163 struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
164 struct FloatEulers att_err;
165 EULERS_DIFF(att_err, att_ref_euler_f.euler, *att_float);
166 FLOAT_ANGLE_NORMALIZE(att_err.psi);
167
168 if (in_flight) {
169 /* update integrator */
172 } else {
174 }
175
176 /* rate error */
177 struct FloatRates *rate_float = stateGetBodyRates_f();
178 struct FloatRates rate_err;
179 RATES_DIFF(rate_err, att_ref_euler_f.rate, *rate_float);
180
181 /* PID */
182
184 stabilization_gains.p.x * att_err.phi +
185 stabilization_gains.d.x * rate_err.p +
187
189 stabilization_gains.p.y * att_err.theta +
190 stabilization_gains.d.y * rate_err.q +
192
193 stabilization_att_fb_cmd[COMMAND_YAW] =
194 stabilization_gains.p.z * att_err.psi +
195 stabilization_gains.d.z * rate_err.r +
197
198
201 cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW];
202 cmd[COMMAND_THRUST] = th_sp_to_thrust_i(thrust, 0, THRUST_AXIS_Z);
203
204 /* bound the result */
205 BoundAbs(cmd[COMMAND_ROLL], MAX_PPRZ);
206 BoundAbs(cmd[COMMAND_PITCH], MAX_PPRZ);
207 BoundAbs(cmd[COMMAND_YAW], MAX_PPRZ);
208 BoundAbs(cmd[COMMAND_THRUST], MAX_PPRZ);
209}
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
#define FLOAT_ANGLE_NORMALIZE(_a)
#define FLOAT_EULERS_ZERO(_e)
#define FLOAT_RATES_ZERO(_r)
euler angles
angular rates
#define EULERS_BOUND_CUBE(_v, _min, _max)
#define EULERS_COPY(_a, _b)
#define EULERS_DIFF(_c, _a, _b)
#define VECT3_ASSIGN(_a, _x, _y, _z)
#define EULERS_ADD(_a, _b)
#define RATES_DIFF(_c, _a, _b)
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
uint16_t foo
Definition main_demo5.c:58
#define MAX_PPRZ
Definition paparazzi.h:8
Paparazzi floating point algebra.
struct Stabilization stabilization
struct FloatEulers stab_sp_to_eulers_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
void stabilization_attitude_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
Attitude control run function.
void stabilization_attitude_euler_float_init(void)
void stabilization_attitude_enter(void)
Attitude control enter function.
static void send_att_ref(struct transport_tx *trans, struct link_device *dev)
static struct AttRefEulerFloat att_ref_euler_f
struct FloatEulers stabilization_att_sum_err
static void send_att(struct transport_tx *trans, struct link_device *dev)
Rotorcraft attitude stabilization in euler float version.
float stabilization_attitude_get_heading_f(void)
Get attitude heading as float (avoiding jumps).
void attitude_ref_euler_float_enter(struct AttRefEulerFloat *ref, float psi)
void attitude_ref_euler_float_update(struct AttRefEulerFloat *ref, struct FloatEulers *sp_eulers, float dt)
void attitude_ref_euler_float_init(struct AttRefEulerFloat *ref)
Attitude reference state/output (euler 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.