Paparazzi UAS  v6.2_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 
wind_estimation_quadrotor_init
void wind_estimation_quadrotor_init(void)
Definition: wind_estimation_quadrotor.c:98
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
eas_from_tas
float eas_from_tas(float tas)
Calculate equivalent airspeed from true airspeed.
Definition: air_data.c:404
WE_QUAD_VA_X
#define WE_QUAD_VA_X
Definition: wind_estimation_quadrotor.c:47
linear_kalman_filter.h
linear_kalman_filter::A
float A[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
dynamic matrix
Definition: linear_kalman_filter.h:48
WE_QUAD_STATE_SIZE
#define WE_QUAD_STATE_SIZE
Definition: wind_estimation_quadrotor.c:43
abi.h
wind_estimation_quadrotor_report
void wind_estimation_quadrotor_report(void)
Definition: wind_estimation_quadrotor.c:208
linear_kalman_filter::R
float R[KF_MAX_MEAS_SIZE][KF_MAX_MEAS_SIZE]
measurement covariance noise
Definition: linear_kalman_filter.h:53
WE_QUAD_Q_VA
#define WE_QUAD_Q_VA
Definition: wind_estimation_quadrotor.c:61
PPRZ_ISA_GRAVITY
#define PPRZ_ISA_GRAVITY
earth-surface gravitational acceleration in m/s^2
Definition: pprz_isa.h:56
linear_kalman_filter::B
float B[KF_MAX_STATE_SIZE][KF_MAX_CMD_SIZE]
command matrix
Definition: linear_kalman_filter.h:49
WE_QUAD_R
#define WE_QUAD_R
Definition: wind_estimation_quadrotor.c:69
linear_kalman_filter
Definition: linear_kalman_filter.h:46
stateSetHorizontalWindspeed_f
static void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
Set horizontal windspeed (float).
Definition: state.h:1291
WE_QUAD_P0_W
#define WE_QUAD_P0_W
Definition: wind_estimation_quadrotor.c:57
linear_kalman_filter_predict
void linear_kalman_filter_predict(struct linear_kalman_filter *filter, float *U)
Prediction step.
Definition: linear_kalman_filter.c:78
WE_QUAD_VA_Y
#define WE_QUAD_VA_Y
Definition: wind_estimation_quadrotor.c:49
WE_QUAD_P0_VA
#define WE_QUAD_P0_VA
Definition: wind_estimation_quadrotor.c:53
MAT33_ELMT
#define MAT33_ELMT(_m, _row, _col)
Definition: pprz_algebra.h:436
wind_estimation_quadrotor_SetQva
float wind_estimation_quadrotor_SetQva(float Q_va)
Definition: wind_estimation_quadrotor.c:184
we_quad_params
struct wind_estimation_quadrotor_params we_quad_params
Definition: wind_estimation_quadrotor.c:87
wind_estimation_quadrotor_periodic
void wind_estimation_quadrotor_periodic(void)
Definition: wind_estimation_quadrotor.c:133
FloatVect2
Definition: pprz_algebra_float.h:49
FloatVect3
Definition: pprz_algebra_float.h:54
float_rmat_transp_vmult
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
Definition: pprz_algebra_float.c:120
telemetry.h
linear_kalman_filter::C
float C[KF_MAX_MEAS_SIZE][KF_MAX_STATE_SIZE]
observation matrix
Definition: linear_kalman_filter.h:50
float_vect2_norm
static float float_vect2_norm(struct FloatVect2 *v)
Definition: pprz_algebra_float.h:139
stateGetSpeedNed_f
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
WE_QUAD_W_Y
#define WE_QUAD_W_Y
Definition: wind_estimation_quadrotor.c:50
send_wind
static void send_wind(struct transport_tx *trans, struct link_device *dev)
Definition: wind_estimation_quadrotor.c:213
wind_estimation_quadrotor_params::Q_w
float Q_w
model noise on wind
Definition: wind_estimation_quadrotor.h:31
WE_QUAD_W_X
#define WE_QUAD_W_X
Definition: wind_estimation_quadrotor.c:48
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
float_vect_zero
static void float_vect_zero(float *a, const int n)
a = 0
Definition: pprz_algebra_float.h:542
linear_kalman_filter::X
float X[KF_MAX_STATE_SIZE]
estimated state X
Definition: linear_kalman_filter.h:55
WE_QUAD_Q_W
#define WE_QUAD_Q_W
Definition: wind_estimation_quadrotor.c:65
wind_estimation_quadrotor_params
Definition: wind_estimation_quadrotor.h:29
register_periodic_telemetry
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
wind_estimation_quadrotor.h
wind_estimation_quadrotor::status
uint8_t status
Definition: wind_estimation_quadrotor.c:83
stateGetNedToBodyRMat_f
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
FloatVect3::y
float y
Definition: pprz_algebra_float.h:56
wind_estimation_quadrotor_params::Q_va
float Q_va
model noise on airspeed
Definition: wind_estimation_quadrotor.h:30
WE_QUAD_STATUS_IDLE
#define WE_QUAD_STATUS_IDLE
Definition: wind_estimation_quadrotor.c:40
NedCoor_f::y
float y
in meters
Definition: pprz_geodetic_float.h:65
linear_kalman_filter_update
void linear_kalman_filter_update(struct linear_kalman_filter *filter, float *Y)
Update step.
Definition: linear_kalman_filter.c:111
we_quad
static struct wind_estimation_quadrotor we_quad
Definition: wind_estimation_quadrotor.c:86
WE_QUAD_MEAS_SIZE
#define WE_QUAD_MEAS_SIZE
Definition: wind_estimation_quadrotor.c:45
linear_kalman_filter_init
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.
Definition: linear_kalman_filter.c:38
we_dt
static const float we_dt
Definition: wind_estimation_quadrotor.c:77
wind_estimation_quadrotor_reset
static void wind_estimation_quadrotor_reset(void)
Definition: wind_estimation_quadrotor.c:89
FloatVect3::x
float x
Definition: pprz_algebra_float.h:55
wind_estimation_quadrotor
Definition: wind_estimation_quadrotor.c:81
wind_estimation_quadrotor::filter
struct linear_kalman_filter filter
Definition: wind_estimation_quadrotor.c:82
WE_QUAD_CMD_SIZE
#define WE_QUAD_CMD_SIZE
Definition: wind_estimation_quadrotor.c:44
AIRSPEED_WE_QUAD_ID
#define AIRSPEED_WE_QUAD_ID
Definition: abi_sender_ids.h:115
wind_estimation_quadrotor_params::R
float R
measurement noise (ground speed)
Definition: wind_estimation_quadrotor.h:32
WE_QUAD_STATUS_RUN
#define WE_QUAD_STATUS_RUN
Definition: wind_estimation_quadrotor.c:41
FloatRMat
rotation matrix
Definition: pprz_algebra_float.h:77
NedCoor_f::x
float x
in meters
Definition: pprz_geodetic_float.h:64
air_data.h
state.h
wind_estimation_quadrotor_start
void wind_estimation_quadrotor_start(void)
Definition: wind_estimation_quadrotor.c:178
linear_kalman_filter::P
float P[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
state covariance matrix
Definition: linear_kalman_filter.h:51
wind_estimation_quadrotor_SetR
float wind_estimation_quadrotor_SetR(float R)
Definition: wind_estimation_quadrotor.c:200
wind_estimation_quadrotor_SetQw
float wind_estimation_quadrotor_SetQw(float Q_w)
Definition: wind_estimation_quadrotor.c:192
linear_kalman_filter::Q
float Q[KF_MAX_STATE_SIZE][KF_MAX_STATE_SIZE]
proces covariance noise
Definition: linear_kalman_filter.h:52
wind_estimation_quadrotor_stop
void wind_estimation_quadrotor_stop(void)
Definition: wind_estimation_quadrotor.c:169
mesonh.mesonh_atmosphere.Y
int Y
Definition: mesonh_atmosphere.py:44
DefaultPeriodic
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.