Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_attitude_plane_pid.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2024 Gautier Hattenberger <gautier.hattenberger@enac.fr>
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
27#include "generated/airframe.h"
28
30
31#include "std.h"
32#include "paparazzi.h"
34#include "state.h"
35
38
40
42
43#if PERIODIC_TELEMETRY
45
46static void send_att(struct transport_tx *trans, struct link_device *dev)
47{
48 struct FloatRates *body_rate = stateGetBodyRates_f();
50 float foo = 0.0;
52 &(body_rate->p), &(body_rate->q), &(body_rate->r),
53 &(att->phi), &(att->theta), &(att->psi),
62 &foo, &foo, &foo, &foo,
66 &foo, &foo, &foo);
67}
68
69#endif
70
94
99
100#define MAX_SUM_ERR 200
101
102void stabilization_attitude_plane_pid_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
103{
104
105 /* Compute feedback */
106 /* attitude error */
109 struct FloatEulers att_err;
111 att_err.psi = 0.f;
112
113 /* update integrator */
114 if (in_flight) {
117 } else {
119 }
120
121 /* rate error */
126
127 /* PID */
132
134 stab_plane_gains.p.y * att_err.theta +
137
140 cmd[COMMAND_YAW] = 0;
142
143 /* bound the result */
148}
149
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_EULERS_ZERO(_e)
#define FLOAT_RATES_ZERO(_r)
euler angles
angular rates
#define EULERS_BOUND_CUBE(_v, _min, _max)
#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:1306
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1367
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_X
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
static struct FloatEulers stab_att_sp_euler
struct PlaneAttitudeGains stab_plane_gains
void stabilization_attitude_plane_pid_run(bool in_flight, struct StabilizationSetpoint *sp, struct ThrustSetpoint *thrust, int32_t *cmd)
void stabilization_attitude_plane_pid_init(void)
struct FloatEulers stab_plane_att_sum_err
float stab_plane_att_cmd[COMMANDS_NB]
static void send_att(struct transport_tx *trans, struct link_device *dev)
void stabilization_attitude_plane_pid_enter(void)
Basic fixed-wing attitude stabilization in euler float version.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
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.