Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
wind_estimation_quadrotor.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2021 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
25
28#include "math/pprz_isa.h"
29#include "state.h"
30#include "generated/airframe.h"
32#include "generated/modules.h"
33#include "modules/core/abi.h"
34#include "mcu_periph/sys_time.h"
36
37#if PERIODIC_TELEMETRY
39#endif
40
41#define WE_QUAD_STATUS_IDLE 0
42#define WE_QUAD_STATUS_RUN 1
43
44#define WE_QUAD_STATE_SIZE 4
45#define WE_QUAD_CMD_SIZE 2
46#define WE_QUAD_MEAS_SIZE 2
47
48#define WE_QUAD_VA_X 0
49#define WE_QUAD_W_X 1
50#define WE_QUAD_VA_Y 2
51#define WE_QUAD_W_Y 3
52
53#ifndef WE_QUAD_P0_VA
54#define WE_QUAD_P0_VA 1.f
55#endif
56
57#ifndef WE_QUAD_P0_W
58#define WE_QUAD_P0_W 1.f
59#endif
60
61#ifndef WE_QUAD_Q_VA
62#define WE_QUAD_Q_VA .05f
63#endif
64
65#ifndef WE_QUAD_Q_W
66#define WE_QUAD_Q_W .001f
67#endif
68
69#ifndef WE_QUAD_R
70#define WE_QUAD_R .5f
71#endif
72
73// update wind in state interface directly
74#ifndef WE_QUAD_UPDATE_STATE
75#define WE_QUAD_UPDATE_STATE TRUE
76#endif
77
78static const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD;
79
80static void send_wind(struct transport_tx *trans, struct link_device *dev);
81
86
89
91{
92 we_quad.filter.P[0][0] = WE_QUAD_P0_VA;
93 we_quad.filter.P[1][1] = WE_QUAD_P0_W;
94 we_quad.filter.P[2][2] = WE_QUAD_P0_VA;
95 we_quad.filter.P[3][3] = WE_QUAD_P0_W;
97}
98
100{
102
103 // init filter and model
105 we_quad.filter.A[0][0] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
106 we_quad.filter.A[1][1] = 1.f;
107 we_quad.filter.A[2][2] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
108 we_quad.filter.A[3][3] = 1.f;
109 we_quad.filter.B[0][0] = we_dt / WE_QUAD_MASS;
110 we_quad.filter.B[2][1] = we_dt / WE_QUAD_MASS;
111 we_quad.filter.C[0][0] = 1.f;
112 we_quad.filter.C[0][1] = 1.f;
113 we_quad.filter.C[1][2] = 1.f;
114 we_quad.filter.C[1][3] = 1.f;
115 we_quad.filter.Q[0][0] = WE_QUAD_Q_VA;
116 we_quad.filter.Q[1][1] = WE_QUAD_Q_W;
117 we_quad.filter.Q[2][2] = WE_QUAD_Q_VA;
118 we_quad.filter.Q[3][3] = WE_QUAD_Q_W;
119 we_quad.filter.R[0][0] = WE_QUAD_R;
120 we_quad.filter.R[1][1] = WE_QUAD_R;
121 // reset covariance and state
123 // external params
127
128#if PERIODIC_TELEMETRY
129 // register for periodic telemetry
130 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
131#endif
132}
133
135{
136 // compute command from state interface
137 float U[WE_QUAD_CMD_SIZE];
138 struct FloatRMat rot = *stateGetNedToBodyRMat_f();
139 float coef = Max(MAT33_ELMT(rot, 2, 2), 0.5); // limit to 1/2
140 float Tnorm = WE_QUAD_MASS * PPRZ_ISA_GRAVITY / coef;
141 struct FloatVect3 Tbody = { 0.f, 0.f, -Tnorm };
142 struct FloatVect3 Tned;
143 float_rmat_transp_vmult(&Tned, &rot, &Tbody);
144 U[0] = Tned.x;
145 U[1] = Tned.y;
146
148
149 // compute correction from measure
150 float Y[WE_QUAD_MEAS_SIZE];
151 Y[0] = stateGetSpeedNed_f()->x; // north ground speed
152 Y[1] = stateGetSpeedNed_f()->y; // east ground speed
153
155
156 // send airspeed as ABI message
157 struct FloatVect2 tas = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] };
158 float eas = eas_from_tas(float_vect2_norm(&tas));
159 AbiSendMsgAIRSPEED_RAW(AIRSPEED_WE_QUAD_ID, get_sys_time_usec(), eas);
160 // update wind in state interface only if enabled
161#if WE_QUAD_UPDATE_STATE
162 struct FloatVect2 wind_ne = {
163 we_quad.filter.X[WE_QUAD_W_X],
164 we_quad.filter.X[WE_QUAD_W_Y]
165 };
166 stateSetHorizontalWindspeed_f(MODULE_WIND_ESTIMATION_QUADROTOR_ID, &wind_ne);
167#endif
168}
169
171{
173#if WE_QUAD_UPDATE_STATE
174 struct FloatVect2 zero_wind_ne = { 0.f, 0.f };
175 stateSetHorizontalWindspeed_f(MODULE_WIND_ESTIMATION_QUADROTOR_ID, &zero_wind_ne);
176#endif
177}
178
184
186{
187 we_quad_params.Q_va = Q_va;
188 we_quad.filter.Q[0][0] = Q_va;
189 we_quad.filter.Q[2][2] = Q_va;
190 return Q_va;
191}
192
194{
195 we_quad_params.Q_w = Q_w;
196 we_quad.filter.Q[1][1] = Q_w;
197 we_quad.filter.Q[3][3] = Q_w;
198 return Q_w;
199}
200
202{
203 we_quad_params.R = R;
204 we_quad.filter.R[0][0] = R;
205 we_quad.filter.R[1][1] = R;
206 return R;
207}
208
210{
211 DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, WE_QUAD_STATE_SIZE, we_quad.filter.X);
212}
213
214static void send_wind(struct transport_tx *trans, struct link_device *dev)
215{
216 uint8_t flags = 5; // send horizontal wind and airspeed
217 float upwind = 0.f;
218 struct FloatVect2 as = { we_quad.filter.X[WE_QUAD_VA_X], we_quad.filter.X[WE_QUAD_VA_Y] };
219 float airspeed = float_vect2_norm(&as);
220 pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID,
221 &flags,
222 &we_quad.filter.X[WE_QUAD_W_Y], // east
223 &we_quad.filter.X[WE_QUAD_W_X], // north
224 &upwind,
225 &airspeed);
226}
227
Main include for ABI (AirBorneInterface).
#define AIRSPEED_WE_QUAD_ID
float eas_from_tas(float tas)
Calculate equivalent airspeed from true airspeed.
Definition air_data.c:967
Air Data interface.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
static void float_vect_zero(float *a, const int n)
a = 0
static float float_vect2_norm(const struct FloatVect2 *v)
void float_rmat_transp_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_b2a, const struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
rotation matrix
#define MAT33_ELMT(_m, _row, _col)
#define PPRZ_ISA_GRAVITY
earth-surface gravitational acceleration in m/s^2
Definition pprz_isa.h:56
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1319
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
static void stateSetHorizontalWindspeed_f(uint16_t id, struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition state.h:1489
static void send_wind(struct transport_tx *trans, struct link_device *dev)
bool linear_kalman_filter_init(struct linear_kalman_filter *filter, uint8_t n, uint8_t c, uint8_t m)
Init all matrix and vectors to zero.
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
Prediction step.
void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
Update step.
float x
in meters
float y
in meters
Paparazzi atmospheric pressure conversion utilities.
API to get/set the generic vehicle states.
Architecture independent timing functions.
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
float wind_estimation_quadrotor_SetQva(float Q_va)
#define WE_QUAD_STATE_SIZE
void wind_estimation_quadrotor_start(void)
void wind_estimation_quadrotor_stop(void)
void wind_estimation_quadrotor_periodic(void)
float wind_estimation_quadrotor_SetQw(float Q_w)
float wind_estimation_quadrotor_SetR(float R)
#define WE_QUAD_W_X
static void send_wind(struct transport_tx *trans, struct link_device *dev)
static struct wind_estimation_quadrotor we_quad
static void wind_estimation_quadrotor_reset(void)
#define WE_QUAD_CMD_SIZE
#define WE_QUAD_STATUS_RUN
void wind_estimation_quadrotor_report(void)
#define WE_QUAD_VA_X
void wind_estimation_quadrotor_init(void)
#define WE_QUAD_VA_Y
#define WE_QUAD_Q_W
#define WE_QUAD_STATUS_IDLE
#define WE_QUAD_R
#define WE_QUAD_P0_VA
struct linear_kalman_filter filter
static const float we_dt
#define WE_QUAD_P0_W
#define WE_QUAD_W_Y
#define WE_QUAD_MEAS_SIZE
struct wind_estimation_quadrotor_params we_quad_params
#define WE_QUAD_Q_VA