Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
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"
35 
36 #if PERIODIC_TELEMETRY
38 #endif
39 
40 #define WE_QUAD_STATUS_IDLE 0
41 #define WE_QUAD_STATUS_RUN 1
42 
43 #define WE_QUAD_STATE_SIZE 4
44 #define WE_QUAD_CMD_SIZE 2
45 #define WE_QUAD_MEAS_SIZE 2
46 
47 #define WE_QUAD_VA_X 0
48 #define WE_QUAD_W_X 1
49 #define WE_QUAD_VA_Y 2
50 #define WE_QUAD_W_Y 3
51 
52 #ifndef WE_QUAD_P0_VA
53 #define WE_QUAD_P0_VA 1.f
54 #endif
55 
56 #ifndef WE_QUAD_P0_W
57 #define WE_QUAD_P0_W 1.f
58 #endif
59 
60 #ifndef WE_QUAD_Q_VA
61 #define WE_QUAD_Q_VA .05f
62 #endif
63 
64 #ifndef WE_QUAD_Q_W
65 #define WE_QUAD_Q_W .001f
66 #endif
67 
68 #ifndef WE_QUAD_R
69 #define WE_QUAD_R .5f
70 #endif
71 
72 // update wind in state interface directly
73 #ifndef WE_QUAD_UPDATE_STATE
74 #define WE_QUAD_UPDATE_STATE TRUE
75 #endif
76 
77 static const float we_dt = WIND_ESTIMATION_QUADROTOR_PERIODIC_PERIOD;
78 
79 static void send_wind(struct transport_tx *trans, struct link_device *dev);
80 
84 };
85 
88 
90 {
91  we_quad.filter.P[0][0] = WE_QUAD_P0_VA;
92  we_quad.filter.P[1][1] = WE_QUAD_P0_W;
93  we_quad.filter.P[2][2] = WE_QUAD_P0_VA;
94  we_quad.filter.P[3][3] = WE_QUAD_P0_W;
96 }
97 
99 {
101 
102  // init filter and model
104  we_quad.filter.A[0][0] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
105  we_quad.filter.A[1][1] = 1.f;
106  we_quad.filter.A[2][2] = 1.f - WE_QUAD_DRAG * we_dt / WE_QUAD_MASS;
107  we_quad.filter.A[3][3] = 1.f;
108  we_quad.filter.B[0][0] = we_dt / WE_QUAD_MASS;
109  we_quad.filter.B[2][1] = we_dt / WE_QUAD_MASS;
110  we_quad.filter.C[0][0] = 1.f;
111  we_quad.filter.C[0][1] = 1.f;
112  we_quad.filter.C[1][2] = 1.f;
113  we_quad.filter.C[1][3] = 1.f;
114  we_quad.filter.Q[0][0] = WE_QUAD_Q_VA;
115  we_quad.filter.Q[1][1] = WE_QUAD_Q_W;
116  we_quad.filter.Q[2][2] = WE_QUAD_Q_VA;
117  we_quad.filter.Q[3][3] = WE_QUAD_Q_W;
118  we_quad.filter.R[0][0] = WE_QUAD_R;
119  we_quad.filter.R[1][1] = WE_QUAD_R;
120  // reset covariance and state
122  // external params
126 
127 #if PERIODIC_TELEMETRY
128  // register for periodic telemetry
129  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
130 #endif
131 }
132 
134 {
135  // compute command from state interface
136  float U[WE_QUAD_CMD_SIZE];
137  struct FloatRMat rot = *stateGetNedToBodyRMat_f();
138  float coef = Max(MAT33_ELMT(rot, 2, 2), 0.5); // limit to 1/2
139  float Tnorm = WE_QUAD_MASS * PPRZ_ISA_GRAVITY / coef;
140  struct FloatVect3 Tbody = { 0.f, 0.f, -Tnorm };
141  struct FloatVect3 Tned;
142  float_rmat_transp_vmult(&Tned, &rot, &Tbody);
143  U[0] = Tned.x;
144  U[1] = Tned.y;
145 
147 
148  // compute correction from measure
149  float Y[WE_QUAD_MEAS_SIZE];
150  Y[0] = stateGetSpeedNed_f()->x; // north ground speed
151  Y[1] = stateGetSpeedNed_f()->y; // east ground speed
152 
154 
155  // send airspeed as ABI message
157  float eas = eas_from_tas(float_vect2_norm(&tas));
158  AbiSendMsgAIRSPEED(AIRSPEED_WE_QUAD_ID, eas);
159  // update wind in state interface only if enabled
160 #if WE_QUAD_UPDATE_STATE
161  struct FloatVect2 wind_ne = {
164  };
166 #endif
167 }
168 
170 {
172 #if WE_QUAD_UPDATE_STATE
173  struct FloatVect2 zero_wind_ne = { 0.f, 0.f };
174  stateSetHorizontalWindspeed_f(&zero_wind_ne);
175 #endif
176 }
177 
179 {
182 }
183 
185 {
186  we_quad_params.Q_va = Q_va;
187  we_quad.filter.Q[0][0] = Q_va;
188  we_quad.filter.Q[2][2] = Q_va;
189  return Q_va;
190 }
191 
193 {
194  we_quad_params.Q_w = Q_w;
195  we_quad.filter.Q[1][1] = Q_w;
196  we_quad.filter.Q[3][3] = Q_w;
197  return Q_w;
198 }
199 
201 {
202  we_quad_params.R = R;
203  we_quad.filter.R[0][0] = R;
204  we_quad.filter.R[1][1] = R;
205  return R;
206 }
207 
209 {
210  DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, WE_QUAD_STATE_SIZE, we_quad.filter.X);
211 }
212 
213 static void send_wind(struct transport_tx *trans, struct link_device *dev)
214 {
215  uint8_t flags = 5; // send horizontal wind and airspeed
216  float upwind = 0.f;
218  float airspeed = float_vect2_norm(&as);
219  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID,
220  &flags,
221  &we_quad.filter.X[WE_QUAD_W_Y], // east
222  &we_quad.filter.X[WE_QUAD_W_X], // north
223  &upwind,
224  &airspeed);
225 }
226 
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:409
Air Data interface.
static void float_vect_zero(float *a, const int n)
a = 0
static float float_vect2_norm(struct FloatVect2 *v)
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
rotation matrix
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
#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:1137
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
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 C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]
observation matrix
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]
measurement covariance noise
float X[KF_MAX_STATE_SIZE]
estimated state X
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
state covariance matrix
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
dynamic matrix
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]
command matrix
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
proces covariance noise
float x
in meters
float y
in meters
Paparazzi atmospheric pressure conversion utilities.
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.
Definition: vl53l1_types.h:98
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
float R
measurement noise (ground speed)