Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
ins_flow.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Guido de Croon <g.c.h.e.decroon@tudelft.nl>
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 
36 #include "ins_flow.h"
37 #include "modules/core/abi.h"
38 #include "generated/airframe.h"
39 #include "mcu_periph/sys_time.h"
40 #include "autopilot.h"
42 #include "generated/airframe.h"
43 #include "generated/flight_plan.h"
44 #include "mcu_periph/sys_time.h"
46 #include "stabilization.h"
47 #include <stdio.h>
48 
49 #define DEBUG_INS_FLOW 0
50 #if DEBUG_INS_FLOW
51 #include <stdio.h>
53 #define DEBUG_PRINT printf
54 #define DEBUG_MAT_PRINT MAT_PRINT
55 //#define DEBUG_MAT_PRINT(...)
56 #else
57 #define DEBUG_PRINT(...)
58 #define DEBUG_MAT_PRINT(...)
59 #endif
60 
61 #ifndef AHRS_ICQ_OUTPUT_ENABLED
62 #define AHRS_ICQ_OUTPUT_ENABLED TRUE
63 #endif
64 PRINT_CONFIG_VAR(AHRS_ICQ_OUTPUT_ENABLED)
65 
66 /* default Gyro to use in INS */
67 #ifndef INS_FLOW_GYRO_ID
68 #define INS_FLOW_GYRO_ID ABI_BROADCAST
69 #endif
70 PRINT_CONFIG_VAR(INS_FLOW_GYRO_ID)
71 
72 /* default Accelerometer to use in INS */
73 #ifndef INS_FLOW_ACCEL_ID
74 #define INS_FLOW_ACCEL_ID ABI_BROADCAST
75 #endif
76 PRINT_CONFIG_VAR(INS_FLOW_ACCEL_ID)
77 
78 /* default IMU lowpass to use in INS */
79 #ifndef INS_FLOW_IMU_ID
80 #define INS_FLOW_IMU_ID ABI_BROADCAST
81 #endif
82 PRINT_CONFIG_VAR(INS_FLOW_IMU_ID)
83 
84 
85 /* default GPS to use in INS */
86 #ifndef INS_FLOW_GPS_ID
87 #define INS_FLOW_GPS_ID GPS_MULTI_ID
88 #endif
89 PRINT_CONFIG_VAR(INS_FLOW_GPS_ID)
90 
91 /* Use optical flow estimates */
92 #ifndef INS_OPTICAL_FLOW_ID
93 #define INS_OPTICAL_FLOW_ID ABI_BROADCAST
94 #endif
95 PRINT_CONFIG_VAR(INS_OPTICAL_FLOW_ID)
96 
97 // reading RPMs:
98 #ifndef INS_RPM_ID
99 #define INS_RPM_ID ABI_BROADCAST
100 #endif
101 PRINT_CONFIG_VAR(INS_RPM_ID)
102 
103 /* All registered ABI events */
110 //static abi_event mag_ev;
111 //static abi_event geo_mag_ev;
112 
113 /* All ABI callbacks */
114 static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
115 static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
116 /*static void mag_cb(uint8_t __attribute__((unused)) sender_id,
117  uint32_t __attribute__((unused)) stamp,
118  struct Int32Vect3 *mag);
119 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h);*/
120 //static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i_f);
121 static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
122 void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
123  int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
124 static void ins_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
125 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
126  uint32_t stamp __attribute__((unused)),
127  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
128  struct Int32Vect3 *lp_mag);
129 static void print_ins_flow_state(void);
130 static void print_true_state(void);
131 /* Static local functions */
132 //static bool ahrs_icq_output_enabled;
135 static void set_body_state_from_quat(void);
136 static void ins_reset_filter(void);
137 
138 struct InsFlow {
139 
140  // data elements for gps passthrough:
141  struct LtpDef_i ltp_def;
142  bool ltp_initialized;
143 
144  /* output LTP NED */
145  struct NedCoor_i ltp_pos;
146  struct NedCoor_i ltp_speed;
147  struct NedCoor_i ltp_accel;
148 
149  // vision measurements:
152  float divergence;
153  float vision_time; // perhaps better to use microseconds (us) instead of float in seconds
155 
156  // RPMs:
157  uint16_t RPM[8]; // max an octocopter
159 
161  float lp_gyro_bias_pitch; // determine the bias before take-off
163  float lp_gyro_bias_roll; // determine the bias before take-off
164  float thrust_factor; // determine the additional required scale factor to have unbiased thrust estimates
165  float lp_thrust;
167 
168 };
169 struct InsFlow ins_flow;
170 
171 // Kalman filter parameters and variables:
172 
173 #define MOMENT_DELAY 20
174 float moments[MOMENT_DELAY] = {0.};
176 
177 float OF_X[N_STATES_OF_KF] = {0.};
180 float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}};
181 
182 #define OF_N_ROTORS 4
184 
185 float of_time;
187 float lp_factor;
194 
195 float GT_phi;
196 float GT_theta;
197 
198 
199 #define USE_STANDARD_PARAMS 0
200 
201 #if USE_STANDARD_PARAMS
202 // Note that not all of the values are used.
203 // Moment of Inertia, mass, distance motors from center of gravity, 4 params for thrust and moment generation,
204 // measurement noise R (2), actuation noise Q(5),
205 // initial P (5), linear drag factor
206 #if USE_NPS
207 float parameters[20] = {0.0018244, 0.400, 0.085, 0.152163; 0.170734; 0.103436; 0.122109,
208  0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
209  1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
210  };
211 #else
212 float parameters[20] = {0.0018244, 0.400, 0.085, 0.108068 0.115448 0.201207 0.208834,
213  0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
214  1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
215  };
216 #endif
217 #else
218 // Define parameters for the filter, fitted in MATLAB:
219 #if USE_NPS
220 #if N_MEAS_OF_KF == 3
221 // with rate measurement:
222 float parameters[20] = {1.234994e-01, 3.603662e-01, 8.751691e-02, 1.636867e-01, 1.561769e-01, 1.856140e-01, 1.601066e-02, 1.187989e-01, 1.507075e-01, 2.471644e-01, 7.934140e-02, 1.770048e+00, 1.345862e-01, 2.881410e+00, 1.003584e+00, 1.280523e-01, 7.549402e-02, 9.640423e-01, 1.078312e+00, 3.468849e-01};
223 #else
224 // without rate state / measurement:
225 #if CONSTANT_ALT_FILTER
226 #if OF_DRAG
227 float parameters[20] = {1.396428e-01, 2.517970e-01, 3.575834e-02, 2.626194e-01, 1.078661e-01, 3.126137e-01, 4.621823e-02, 3.258048e-01, 8.456147e-02, 2.275105e-01, 2.820394e-02, 1.937395e+00, -4.259889e-02, 2.755648e+00, 1.000810e+00, -3.474577e-03, 3.146387e-01, 8.809383e-01, 9.878757e-01, 6.741976e-01};
228 #else
229 float parameters[20] = {3.363769e-01, 4.917425e-01, 1.903805e-01, 2.945672e-01, 1.258647e-01, 1.513736e-01, 5.894541e-01, 2.162745e-01, 5.527361e-01, 1.385623e-01, 8.307731e-01, 1.488212e+00, 2.439721e-01, 3.052758e+00, 8.246426e-01, 9.988101e-02, 1.247046e-01, 8.834364e-01, 7.971876e-01, 1.112319e+00};
230 #endif
231 #else
232 float parameters[20] = {4.370754e-02, 3.770587e-01, 1.187542e-01, 1.174995e-01, 1.419432e-01, 6.950201e-02, 2.251078e-01, 9.113943e-02, 2.230198e-01, 5.767389e-02, 1.855676e-02, 1.676359e+00, 5.822681e-02, 2.869468e+00, 1.140625e+00, 6.831383e-02, 1.600776e-01, 9.853843e-01, 1.000381e+00, 5.081224e-01};
233 #endif
234 #endif
235 #else
236 // TODO: train constant alt filter without drag, also with and without measuring the gyro.
237 #if CONSTANT_ALT_FILTER
238 #if OF_DRAG
239 // float parameters[20] = {1.557784e-01, 3.186275e-01, 8.341852e-02, 9.320449e-02, 1.706694e-01, 3.950497e-01, 3.338107e-01, 1.947852e-01, 2.429782e-01, 1.216562e-01, 2.885142e-01, 1.765480e+00, 2.427392e-01, 3.014556e+00, 1.004227e+00, 1.798174e-01, 2.821081e-01, 9.314043e-01, 1.005090e+00, 2.630276e-01};
240 float parameters[20] = {8.407886e-03, 4.056093e-01, 1.555919e-01, 1.291584e-01, 2.594766e-01, 1.927331e-01, 9.599609e-02, 1.688265e-01, 5.589618e-02, 1.605665e-01, 1.195912e-01, 1.809532e+00, 4.268251e-02, 3.003060e+00, 1.098473e+00, 1.944433e-01, 2.363352e-01, 1.110390e+00, 1.190994e+00, 6.211962e-01};
241 #endif
242 #else
243 #if N_MEAS_OF_KF == 3
244 #if PREDICT_GYROS == 0
245 // with rate measurement (last two values copied from the last condition)
246 float parameters[22] = {0.041001, 1.015066, -0.058495, 0.498353, -0.156362, 0.383511, 0.924635, 0.681918, 0.318947, 0.298235, 0.224906, 1.371037, 0.008888, 3.045428, 0.893953, 0.529789, 0.295028, 1.297515, 0.767550, 0.334040, 0.192238, 0.301966};
247 #else
248 // Estimate gyro directly instead of moment:
249 float parameters[26] = {1.065019, 0.270407, 0.164520, 0.008321, 0.083628, 0.261853, 0.210707, 0.204501, 0.164267, 0.097979, 0.053705, 1.640180, 0.151171, 3.086366, 1.025684, 0.011813, 0.177164, 0.995710, 1.050374, 0.617920, 0.028360, 0.447258, 0.077277, 0.360559, 0.555940, 0.133979};
250 #endif
251 #else
252 // without rate measurement:
253 // float parameters[20] = {4.098677e-01, 7.766318e-01, 3.614751e-01, 4.745865e-01, 5.144065e-01, 3.113647e-01, -8.737287e-03, 6.370274e-01, 3.863760e-01, -3.527670e-01, 4.873666e-01, 1.688456e+00, -6.037967e-02, 2.759148e+00, 1.385455e+00, 1.044881e-01, -1.170409e-01, 1.126136e+00, 1.097562e+00, 2.680243e-01};
254 float parameters[22] = {0.219033, 0.376572, 0.184002, 0.096388, 0.240843, 0.172390, 0.133111, 0.495885, 0.357086, 0.233624, 0.125611, 1.661682, 0.136735, 2.812652, 0.715887, 0.166932, 0.371409, 1.043920, 0.840683, 0.567703, 0.192238, 0.301966};
255 #endif
256 #endif
257 #endif
258 #endif
259 // parameter indices (TODO: how important are these numbers? Some are not used, others, like P may be not so important).
260 #define PAR_IX 0
261 #define PAR_MASS 1
262 #define PAR_BASE 2
263 #define PAR_K0 3
264 #define PAR_K1 4
265 #define PAR_K2 5
266 #define PAR_K3 6
267 #define PAR_R0 7
268 #define PAR_R1 8
269 #define PAR_Q0 9
270 #define PAR_Q1 10
271 #define PAR_Q2 11
272 #define PAR_Q3 12
273 #define PAR_Q4 13
274 #define PAR_P0 14
275 #define PAR_P1 15
276 #define PAR_P2 16
277 #define PAR_P3 17
278 #define PAR_P4 18
279 #define PAR_KD 19
280 #define PAR_Q_TB 20
281 #define PAR_P_TB 21
282 #define PAR_PRED_ROLL_1 22
283 #define PAR_PRED_ROLL_2 23
284 #define PAR_PRED_ROLL_3 24
285 
286 
287 
288 /*
289 struct InsFlowState {
290  // vector representation of state:
291  // v, angle, angle_dot, z, z_dot
292 
293 
294 };
295 struct InsFlowState ins_flow_state;
296 */
297 
298 #if PERIODIC_TELEMETRY
300 #include "mcu_periph/sys_time.h"
301 #include "state.h"
302 
303 // attitude part:
304 static void send_quat(struct transport_tx *trans, struct link_device *dev)
305 {
306  struct Int32Quat *quat = stateGetNedToBodyQuat_i();
307  pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
308  &ahrs_icq.weight,
313  &(quat->qi),
314  &(quat->qx),
315  &(quat->qy),
316  &(quat->qz),
317  &ahrs_flow_id);
318 }
319 
320 static void send_euler(struct transport_tx *trans, struct link_device *dev)
321 {
322  struct Int32Eulers ltp_to_imu_euler;
323  int32_eulers_of_quat(&ltp_to_imu_euler, &ahrs_icq.ltp_to_body_quat);
324  struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
325  pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
326  &ltp_to_imu_euler.phi,
327  &ltp_to_imu_euler.theta,
328  &ltp_to_imu_euler.psi,
329  &(eulers->phi),
330  &(eulers->theta),
331  &(eulers->psi),
332  &ahrs_flow_id);
333 
334 }
335 
336 static void send_bias(struct transport_tx *trans, struct link_device *dev)
337 {
338  pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
341 }
342 
343 static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
344 {
345  struct FloatVect3 h_float;
346  h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x);
347  h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y);
348  h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z);
349  pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
350  &h_float.x, &h_float.y, &h_float.z, &ahrs_flow_id);
351 }
352 
353 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
354 {
355  uint8_t mde = 3;
356  uint16_t val = 0;
357  if (!ahrs_icq.is_aligned) { mde = 2; }
359  /* set lost if no new gyro measurements for 50ms */
360  if (t_diff > 50000) { mde = 5; }
361  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_flow_id, &mde, &val);
362 }
363 
364 // ins part
365 static void send_ins(struct transport_tx *trans, struct link_device *dev)
366 {
367  pprz_msg_send_INS(trans, dev, AC_ID,
371 }
372 
373 /*static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
374 {
375  static float fake_baro_z = 0.0;
376  pprz_msg_send_INS_Z(trans, dev, AC_ID,
377  (float *)&fake_baro_z, &ins_flow.ltp_pos.z,
378  &ins_flow.ltp_speed.z, &ins_flow.ltp_accel.z);
379 }*/
380 
381 static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
382 {
383  static float fake_qfe = 0.0;
385  pprz_msg_send_INS_REF(trans, dev, AC_ID,
388  &ins_flow.ltp_def.hmsl, (float *)&fake_qfe);
389  }
390 }
391 
392 
393 #endif
394 
395 static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
396 {
397  // TODO: add sending of theta:
398  struct FloatEulers *eulers = stateGetNedToBodyEulers_f();
399  struct NedCoor_f *position = stateGetPositionNed_f();
400  struct NedCoor_f *velocities = stateGetSpeedNed_f();
401  struct FloatRates *rates = stateGetBodyRates_f();
402 
403  float phi = (180.0 / M_PI) * OF_X[OF_ANGLE_IND];
404  float theta;
405  if (OF_TWO_DIM) {
406  theta = (180.0 / M_PI) * OF_X[OF_THETA_IND];
407  } else {
408  // if not filtering the second dimension, just take the ground truth
409  theta = (180.0 / M_PI) * eulers->theta;
410  }
411 
412  float phi_dot = 0.0f;
413  float z_dot = 0.0f;
414  if (!CONSTANT_ALT_FILTER) {
415  phi_dot = (180.0 / M_PI) * OF_X[OF_ANGLE_DOT_IND];
416  z_dot = OF_X[OF_Z_DOT_IND];
417  }
418 
419  struct FloatRMat *NTB = stateGetNedToBodyRMat_f();
420  struct FloatVect3 NED_velocities, body_velocities;
421  NED_velocities.x = velocities->x;
422  NED_velocities.y = velocities->y;
423  NED_velocities.z = velocities->z;
424  float_rmat_vmult(&body_velocities, NTB, &NED_velocities);
425 
426  float vy_GT = body_velocities.y;
427 
428  float phi_GT;
429  if (use_filter < USE_ANGLE) {
430  phi_GT = (180.0 / M_PI) * eulers->phi;
431  } else {
432  phi_GT = (180.0 / M_PI) * GT_phi;
433  }
434  float vx_GT = body_velocities.x;
435  float theta_GT;
436  if (use_filter < USE_ANGLE) {
437  theta_GT = (180.0 / M_PI) * eulers->theta;
438  } else if (OF_TWO_DIM) {
439  theta_GT = (180.0 / M_PI) * GT_theta;
440  }
441  float p_GT = rates->p;
442  float q_GT = rates->q;
443  float z_GT = -position->z;
444  float vz_GT = -velocities->z;
445 
446  float vy = OF_X[OF_V_IND];
447  float vx;
448  if (OF_TWO_DIM) {
449  vx = OF_X[OF_VX_IND];
450  } else {
451  vx = vx_GT;
452  }
453  float z = OF_X[OF_Z_IND];
454  float p, q;
455  if (!CONSTANT_ALT_FILTER) {
456  // normally:
457  p = phi_dot;
458  // when estimating the gyros:
459  // // p = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
460  // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
461  // TODO: expand the full filter later as well, to include q:
462  q = q_GT;
463  } else {
465  // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
467  }
468 
469  float thrust_bias;
471  thrust_bias = 0.0f;
472  } else {
473  thrust_bias = OF_X[OF_THRUST_BIAS_IND];
474  }
475  // This code is to actually compare the unbiased thrust with gravity:
476  // TODO: code copied from below, put in a function?
477  float thrust = 0.0f;
478  for (int i = 0; i < OF_N_ROTORS; i++) {
479  thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
480  }
481  thrust *= thrust_factor; // ins_flow.thrust_factor;
482  thrust -= thrust_bias;
483  float mass = parameters[PAR_MASS];
484  float g = 9.81;
485  float actual_lp_thrust = mass * g;
486  thrust_bias = thrust - actual_lp_thrust;
487 
488  pprz_msg_send_INS_FLOW_INFO(trans, dev, AC_ID,
489  &vy, &phi, &p, &vx, &theta, &q, &z, &z_dot,
490  &vy_GT, &phi_GT, &p_GT, &vx_GT, &theta_GT, &q_GT,
491  &z_GT, &vz_GT, &thrust_bias, &use_filter);
492 }
493 
495 {
496 
497  // (re-)initialize the state:
498  for (int i = 0; i < N_STATES_OF_KF; i++) {
499  OF_X[i] = 0.0f;
500  }
501  OF_X[OF_Z_IND] = 1.0; // nonzero z
502 
503  // P-matrix:
504  for (int i = 0; i < N_STATES_OF_KF; i++) {
505  for (int j = 0; j < N_STATES_OF_KF; j++) {
506  OF_P[i][j] = 0.0f;
507  }
508  }
509  if (CONSTANT_ALT_FILTER == 1) {
514  if (OF_TWO_DIM) {
517  }
518  } else {
524  if (OF_THRUST_BIAS) {
526  }
527  }
528 
529  counter = 0;
530 
531  // TODO: what to do with thrust, gyro bias, and low-passed roll command?
532 
533 }
534 
535 
536 /* Initialize the flow ins */
537 void ins_flow_init(void)
538 {
539 
540  //ahrs_icq_output_enabled = AHRS_ICQ_OUTPUT_ENABLED;
541  ahrs_icq_init();
542  //ahrs_register_impl(ahrs_icq_enable_output);
543 
544  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
545  llh_nav0.lat = NAV_LAT0;
546  llh_nav0.lon = NAV_LON0;
547  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
548  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
549  struct EcefCoor_i ecef_nav0;
550  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
551  struct LtpDef_i ltp_def;
552  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
553 
554  ltp_def_from_ecef_i(&ins_flow.ltp_def, &ecef_nav0);
555  ins_flow.ltp_def.hmsl = NAV_ALT0;
557  ins_flow.ltp_initialized = true;
559  ins_flow.lp_gyro_pitch = 0.0f;
561  ins_flow.lp_gyro_roll = 0.0f;
563  ins_flow.thrust_factor = 1.0f;
564  ins_flow.lp_thrust = 0.0f;
565  ins_flow.lp_roll_command = 0.0f;
566 
567  lp_factor = 0.95;
568  lp_factor_strong = 1 - 1E-3;
569 
570  GT_phi = 0.0f;
571  GT_theta = 0.0f;
572 
573  // Extended Kalman filter:
574  // reset the state and P matrix:
576 
577  // R-matrix, measurement noise (TODO: make params)
580  if (OF_TWO_DIM) {
582  } else if (N_MEAS_OF_KF == 3) {
583  OF_R[OF_RATE_IND][OF_RATE_IND] = 1.0 * (M_PI / 180.0f); // not a param yet, used to be 10, but we could trust it more
584  }
585  // Q-matrix, actuation noise (TODO: make params)
590  if (!CONSTANT_ALT_FILTER) {
592  } else if (OF_TWO_DIM) {
595  }
596  if (OF_THRUST_BIAS) {
598  }
599 
600 
601  // based on a fit, factor * rpm^2:
602  // TODO: with the parameters, we don't need this if / else any more.
603 #if USE_NPS
604  // K = [0.152163; 0.170734; 0.103436; 0.122109] * 1E-7;
605  // K = [0.222949; 0.160458; 0.114227; 0.051396] * 1E-7;
606  // rpm:
607  // [2708.807954; 2587.641476; -379.728916; -501.203388]
608  RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7;
609  RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7;
610  RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7;
611  RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7;
612 #else
613  // % Bebop 2, #45
614  // From fit_TM_2 script:
615  // K = [0.108068; 0.115448; 0.201207; 0.208834] * 1E-7
616  RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7;
617  RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7;
618  RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7;
619  RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7;
620 #endif
621 
622  reset_filter = false;
623  use_filter = 0;
624  run_filter = false;
625 
628 
629  // align the AHRS:
631  // set the initial attitude:
633 
634 #if PERIODIC_TELEMETRY
635  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_quat);
636  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler);
637  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
639  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
641  //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
643 #endif
644 
645  /*
646  * Subscribe to scaled IMU measurements and attach callbacks
647  */
648 // TODO: warning: passing argument 3 of ‘AbiBindMsgIMU_GYRO_INT’ from incompatible pointer type
649  AbiBindMsgIMU_GYRO(INS_FLOW_GYRO_ID, &gyro_ev, gyro_cb);
650  AbiBindMsgIMU_ACCEL(INS_FLOW_ACCEL_ID, &accel_ev, accel_cb);
651  AbiBindMsgGPS(INS_FLOW_GPS_ID, &gps_ev, gps_cb);
652  //AbiBindMsgBODY_TO_IMU_QUAT(ABI_BROADCAST, &body_to_imu_ev, body_to_imu_cb);
654  AbiBindMsgACT_FEEDBACK(INS_RPM_ID, &ins_act_feedback_ev, ins_act_feedback_cb);
655  AbiBindMsgIMU_LOWPASSED(INS_FLOW_IMU_ID, &aligner_ev, aligner_cb);
656  // AbiBindMsgIMU_MAG_INT32(ABI_BROADCAST, &mag_ev, mag_cb);
657  // AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
658 
659  // Telemetry:
660  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_FLOW_INFO, send_ins_flow);
661 
662  moment_ind = 0;
663 
664 }
665 
667 {
672  ins_flow.ltp_initialized = true;
673 }
674 
675 void ins_optical_flow_cb(uint8_t sender_id UNUSED, uint32_t stamp, int32_t flow_x UNUSED,
676  int32_t flow_y UNUSED,
677  int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
678 {
679 
680  // TODO: make parameters:
681  float subpixel_factor = 10.0f;
682  float focal_x = 347.22;
683  float new_time = ((float)stamp) / 1e6;
684  float fps = 1.0f / (new_time - ins_flow.vision_time);
685  ins_flow.optical_flow_x = (((float)flow_x) * fps) / (subpixel_factor * focal_x);
686  ins_flow.optical_flow_y = (((float)flow_y) * fps) / (subpixel_factor * focal_x);
687  ins_flow.divergence = 1.27 * size_divergence * fps;
688  //printf("Reading %f, %f, %f\n", ins_flow.optical_flow_x, ins_flow.optical_flow_y, ins_flow.divergence);
689  ins_flow.vision_time = new_time;
691 
692 }
693 
695 {
696  if (CONSTANT_ALT_FILTER) {
697  if (!OF_TWO_DIM) {
698  printf("v = %f, angle = %f, angle_dot = %f, z = %f.\n",
700  } else {
701  printf("v = %f, angle = %f, angle_dot = %f, z = %f, vx = %f, theta = %f.\n",
703  }
704 
705  } else {
706  if (!OF_THRUST_BIAS) {
707  printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
709  } else {
710  printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f, thrust bias = %f.\n",
713  }
714 
715  }
716 
717 }
718 
720 {
721  // TODO: rotate velocities to body frame:
722  // TODO: add also the theta axis:
723  struct FloatEulers *eulers = stateGetNedToBodyEulers_f();
724  struct NedCoor_f *position = stateGetPositionNed_f();
725  struct NedCoor_f *velocities = stateGetSpeedNed_f();
726  struct FloatRates *rates = stateGetBodyRates_f();
727 
728  printf("True: v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
729  velocities->y, eulers->phi, rates->p, -position->z, -velocities->z);
730 }
731 
732 void ins_flow_update(void)
733 {
734  float mass = parameters[PAR_MASS]; // 0.400;
735  float moment = 0.0f; // for now assumed to be 0
736  float Ix = parameters[PAR_IX]; // 0.0018244;
737  //float b = parameters[PAR_BASE];
738  float g = 9.81; // TODO: get a more accurate definition from pprz
739  float kd = parameters[PAR_KD]; // 0.5
740  float drag = 0.0f;
741 
742  if (reset_filter) {
744  reset_filter = false;
745  }
746 
747  // get ground truth data:
748  //struct FloatEulers* eulers = stateGetNedToBodyEulers_f();
749  //struct NedCoor_f* position = stateGetPositionNed_f();
750  //struct NedCoor_f *velocities = stateGetSpeedNed_f();
751  //struct FloatRates *rates = stateGetBodyRates_f();
752 
753  // TODO: record when starting from the ground: does that screw up the filter? Yes it does : )
754 
755 
756  // assuming that the typical case is no rotation, we can estimate the (initial) bias of the gyro:
759  if (OF_TWO_DIM) {
762  }
763  // same assumption for the roll command: assuming a close-to-hover situation and roll trim for staying in place:
765  stabilization.cmd[COMMAND_ROLL];
766 
767  // only start estimation when flying (and above 1 meter: || position->z > -1.0f )
768  // I removed the condition on height, since (1) we need to start the filter anyway explicitly now, and (2) it created a dependence on GPS fix.
769  if (!autopilot_in_flight()) {
770  return;
771  }
772 
773  if (!run_filter) {
774 
775  // TODO: should we do this if we have a thrust bias?
776 
777  // Drone is flying but not yet running the filter, in order to obtain unbiased thrust estimates we estimate an additional factor.
778  // Assumption is that the drone is hovering, which means that over a longer period of time, the thrust should equal gravity.
779  // If the low pass thrust is lower than the one expected by gravity, then it needs to be increased and viceversa.
780  float thrust = 0.0f;
781  for (int i = 0; i < OF_N_ROTORS; i++) {
782  thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
783  }
784  if (ins_flow.lp_thrust < 1E-3) {
785  // first time directly initialize with thrust to get quicker convergence:
786  ins_flow.lp_thrust = thrust;
787  } else {
789  }
790  float actual_lp_thrust = mass * g;
791  ins_flow.thrust_factor = actual_lp_thrust / ins_flow.lp_thrust;
793  //printf("Low pass predicted thrust = %f. Expected thrust = %f. Thrust factor = %f.\n", ins_flow.lp_thrust, actual_lp_thrust, ins_flow.thrust_factor);
794  // don't run the filter just yet:
795  return;
796  }
797 
798  if (DEBUG_INS_FLOW) { print_true_state(); }
799 
800  // in the sim, the gyro bias wanders so fast, that this does not seem to be useful:
801  // TODO: verify how this is in reality, and if not useful, remove all code to estimate this bias (or do it differently)
802  // ins_flow.lp_gyro_bias_roll = 0.0f;
803 
804  // This module needs to run at the autopilot speed when not yet using this filter. Plus it needs to estimate thrust and gyro bias at that speed.
805  // However, the updates of the filter themselves should be slower:
806  /*counter++;
807  if(counter < 5) {
808  return;
809  }
810  else {
811  counter = 0;
812  }*/
813 
814  // get the new time:
816  float dt = of_time - of_prev_time;
817  //printf("dt = %f.\n", dt);
818  if (dt > 1.0f) {
819  dt = 0.01f;
820  }
821 
822  // predict the thrust and moment:
823  float thrust = 0.0f;
824  for (int i = 0; i < OF_N_ROTORS; i++) {
825  thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
826  }
827  thrust *= thrust_factor; // ins_flow.thrust_factor;
829  thrust -= OF_X[OF_THRUST_BIAS_IND];
830  }
831  DEBUG_PRINT("Thrust acceleration = %f, g = %f\n", thrust / mass, g);
832 
833  // TODO: do we have an optimization that used the moment?
834  /*moment = b * RPM_FACTORS[0] * ins_flow.RPM[0]*ins_flow.RPM[0] -
835  b * RPM_FACTORS[1] * ins_flow.RPM[1]*ins_flow.RPM[1] -
836  b * RPM_FACTORS[2] * ins_flow.RPM[2]*ins_flow.RPM[2] +
837  b * RPM_FACTORS[3] * ins_flow.RPM[3]*ins_flow.RPM[3];*/
838  // M_est = Ix * (-0.000553060716181365 * cmd_roll(k) -3.23315441805895 * Xe(3, k));
839 #if USE_NPS
840  // TODO: moment in simulation is very easy to estimate with the roll command, so add that:
841  moment = 0;
842 #else
843 
844  /*
845  moments[moment_ind] = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
846 
847  int select_ind = moment_ind - MOMENT_DELAY;
848  if(select_ind < 0) {
849  select_ind += MOMENT_DELAY;
850  }
851 
852  // current moment is a delayed version:
853  moment = moments[select_ind];
854 
855  // update the moment's ind:
856  moment_ind++;
857  if(moment_ind >= MOMENT_DELAY) {
858  moment_ind = 0;
859  }
860  */
861  // moment = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
862  moment = 0;
863 #endif
864 
865  // printf("Predicted moment = %f, gyro = %f\n", moment, dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
866 
867 
868  // propagate the state with Euler integration:
869  DEBUG_PRINT("Before prediction: ");
871  if (CONSTANT_ALT_FILTER) {
872  OF_X[OF_V_IND] += dt * (g * tan(OF_X[OF_ANGLE_IND]));
873  if (OF_DRAG) {
874  // quadratic drag acceleration:
875  drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass;
876  // apply it in the right direction:
877  if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; }
878  else { OF_X[OF_V_IND] += drag; }
879  }
880 
881  /* // if we use gyros here, the angle dot estimate is ignored:
882  * if(OF_USE_GYROS) {
883  // OF_X[OF_ANGLE_IND] += dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f; // Code says scaled by 12, but... that does not fit...
884  } */
885 
886  // temporary insertion of gyro estimate here, for quicker effect:
887  // OF_X[OF_ANGLE_IND] += dt * -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
888 
890  OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix);
891 
892  if (OF_TWO_DIM) {
893  // Second axis, decoupled formulation:
894  OF_X[OF_VX_IND] += dt * (g * tan(OF_X[OF_THETA_IND]));
895  if (OF_DRAG) {
896  // quadratic drag acceleration:
897  drag = dt * kd * (OF_X[OF_VX_IND] * OF_X[OF_VX_IND]) / mass;
898  // apply it in the right direction:
899  if (OF_X[OF_VX_IND] > 0) { OF_X[OF_VX_IND] -= drag; }
900  else { OF_X[OF_VX_IND] += drag; }
901  }
902  // TODO: here also a moment estimate?
903  // TODO: add a THETA_DOT_IND
905  (M_PI / 180.0f) / 74.0f; // Code says scaled by 12, but... that does not fit...
906  }
907 
908  DEBUG_PRINT("Rate p = %f, gyro p = %f\n", rates->p,
909  (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f);
910  } else {
911  // make sure that the right hand state terms appear before they change:
912  OF_X[OF_V_IND] += dt * (thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
913  OF_X[OF_Z_IND] += dt * OF_X[OF_Z_DOT_IND];
914  OF_X[OF_Z_DOT_IND] += dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass - g);
916  /*
917  * // TODO: We now only keep this here because it worked on the real drone. It also worked without it. So to be deleted if it works as is.
918  else {
919  OF_X[OF_ANGLE_IND] += dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f;
920  }*/
921  OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix);
922 
923  // thrust bias does not change over time according to our model
924 
925  if (OF_DRAG) {
926  // quadratic drag acceleration:
927  drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass;
928  // apply it in the right direction:
929  if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; }
930  else { OF_X[OF_V_IND] += drag; }
931  }
932  }
933 
934  // ensure that z is not 0 (or lower)
935  if (OF_X[OF_Z_IND] < 1e-2) {
936  OF_X[OF_Z_IND] = 1e-2;
937  }
938 
939  DEBUG_PRINT("After prediction: ");
941 
942  // prepare the update and correction step:
943  // we have to recompute these all the time, as they depend on the state:
944  // discrete version of state transition matrix F: (ignoring t^2)
945  float F[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}};
946  for (int i = 0; i < N_STATES_OF_KF; i++) {
947  F[i][i] = 1.0f;
948  }
949 
950  if (CONSTANT_ALT_FILTER) {
951  F[OF_V_IND][OF_ANGLE_IND] = dt * (g / (cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND])));
952  F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
953  if (OF_TWO_DIM) {
954  F[OF_VX_IND][OF_THETA_IND] = dt * (g / (cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND])));
955  }
956  } else {
957  F[OF_V_IND][OF_ANGLE_IND] = dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass);
958  F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
959  F[OF_Z_IND][OF_Z_DOT_IND] = dt * 1.0f;
960  F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt * (-thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
961  if (OF_THRUST_BIAS) {
962  F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt * sinf(OF_X[OF_ANGLE_IND]) / mass;
963  F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt * cosf(OF_X[OF_ANGLE_IND]) / mass;
964  }
965  }
966 
967  if (OF_DRAG) {
968  // In MATLAB: -sign(v)*2*kd*v/m (always minus, whether v is positive or negative):
969  F[OF_V_IND][OF_V_IND] -= dt * 2 * kd * fabs(OF_X[OF_V_IND]) / mass;
970  if (OF_TWO_DIM) {
971  F[OF_VX_IND][OF_VX_IND] -= dt * 2 * kd * fabs(OF_X[OF_VX_IND]) / mass;
972  }
973  }
974 
975  // G matrix (whatever it may be):
976  float G[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}};
977  // TODO: we miss an off-diagonal element here (compare with MATLAB)
978  for (int i = 0; i < N_STATES_OF_KF; i++) {
979  G[i][i] = dt;
980  }
981 
982  // Jacobian observation matrix H:
983  float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}};
984 
985  if (CONSTANT_ALT_FILTER) {
986  // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
987  // lateral flow:
991  (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
993  // divergence:
994  H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
998 
999  if (OF_TWO_DIM) {
1000  // divergence measurement couples the two axes actually...:
1001  H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2 * OF_X[OF_THETA_IND]) / (2 * OF_X[OF_Z_IND]);
1003 
1004  // lateral flow in x direction:
1008  (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
1009  }
1010  } else {
1011  // lateral flow:
1014  + OF_X[OF_Z_DOT_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
1017  (OF_X[OF_Z_IND] * OF_X[OF_Z_IND])
1018  - OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
1019  H[OF_LAT_FLOW_IND][OF_Z_DOT_IND] = sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
1020  // divergence:
1021  H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
1023  + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
1028  }
1029 
1030  // rate measurement:
1031  if (OF_USE_GYROS) {
1032  H[OF_RATE_IND][OF_V_IND] = 0.0f;
1033  H[OF_RATE_IND][OF_ANGLE_IND] = 0.0f;
1034  H[OF_RATE_IND][OF_ANGLE_DOT_IND] = 1.0f;
1035  H[OF_RATE_IND][OF_Z_IND] = 0.0f;
1036  H[OF_RATE_IND][OF_Z_DOT_IND] = 0.0f;
1037  }
1038 
1039  // propagate uncertainty:
1040  // TODO: make pointers that don't change to init:
1043  MAKE_MATRIX_PTR(Gamma, G, N_STATES_OF_KF);
1047 
1048  DEBUG_PRINT("Phi:\n");
1050 
1051  DEBUG_PRINT("P:\n");
1053 
1054  DEBUG_PRINT("Gamma:\n");
1056 
1057  DEBUG_PRINT("Q:\n");
1059 
1060  DEBUG_PRINT("R:\n");
1062 
1063  DEBUG_PRINT("Jacobian:\n");
1065 
1066  // Corresponding MATLAB statement: :O
1067  // P_k1_k = Phi_k1_k*P*Phi_k1_k' + Gamma_k1_k*Q*Gamma_k1_k';
1068  float _PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1069  MAKE_MATRIX_PTR(PhiT, _PhiT, N_STATES_OF_KF);
1070  float _P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1071  MAKE_MATRIX_PTR(PPhiT, _P_PhiT, N_STATES_OF_KF);
1072  float _Phi_P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1073  MAKE_MATRIX_PTR(PhiPPhiT, _Phi_P_PhiT, N_STATES_OF_KF);
1074 
1077  float_mat_mul(PhiPPhiT, Phi, PPhiT, N_STATES_OF_KF, N_STATES_OF_KF, N_STATES_OF_KF);
1078 
1079  DEBUG_PRINT("Phi*P*PhiT:\n");
1081 
1082  float _GT[N_STATES_OF_KF][N_STATES_OF_KF];
1083  MAKE_MATRIX_PTR(GT, _GT, N_STATES_OF_KF);
1084  float _Q_GT[N_STATES_OF_KF][N_STATES_OF_KF];
1085  MAKE_MATRIX_PTR(QGT, _Q_GT, N_STATES_OF_KF);
1086  float _G_Q_GT[N_STATES_OF_KF][N_STATES_OF_KF];
1087  MAKE_MATRIX_PTR(GQGT, _G_Q_GT, N_STATES_OF_KF);
1088 
1092 
1093  DEBUG_PRINT("Gamma*Q*GammaT:\n");
1095 
1096  float_mat_sum(P, PhiPPhiT, GQGT, N_STATES_OF_KF, N_STATES_OF_KF);
1097  DEBUG_PRINT("P:\n");
1099 
1100  // correct state when there is a new vision measurement:
1102 
1103  DEBUG_PRINT("*********************\n");
1104  DEBUG_PRINT(" NEW MEASUREMENT \n");
1105  DEBUG_PRINT("*********************\n");
1106 
1107  // determine Kalman gain:
1108  // MATLAB statement:
1109  // S_k = Hx*P_k1_k*Hx' + R;
1110  float _JacT[N_STATES_OF_KF][N_MEAS_OF_KF];
1111  MAKE_MATRIX_PTR(JacT, _JacT, N_STATES_OF_KF);
1112  float _P_JacT[N_STATES_OF_KF][N_MEAS_OF_KF];
1113  MAKE_MATRIX_PTR(PJacT, _P_JacT, N_STATES_OF_KF);
1114  float _Jac_P_JacT[N_MEAS_OF_KF][N_MEAS_OF_KF];
1115  MAKE_MATRIX_PTR(JacPJacT, _Jac_P_JacT, N_MEAS_OF_KF);
1116 
1119  DEBUG_PRINT("P*JacT:\n");
1121 
1122  float_mat_mul(JacPJacT, Jac, PJacT, N_MEAS_OF_KF, N_STATES_OF_KF, N_MEAS_OF_KF);
1123 
1124  DEBUG_PRINT("Jac*P*JacT:\n");
1126 
1127  float _S[N_MEAS_OF_KF][N_MEAS_OF_KF];
1128  MAKE_MATRIX_PTR(S, _S, N_MEAS_OF_KF);
1129  float_mat_sum(S, JacPJacT, R, N_MEAS_OF_KF, N_MEAS_OF_KF);
1130 
1131  DEBUG_PRINT("S:\n");
1133 
1134  // MATLAB statement:
1135  // K_k1 = P_k1_k*Hx' * inv(S_k);
1136  float _K[N_STATES_OF_KF][N_MEAS_OF_KF];
1138  float _INVS[N_MEAS_OF_KF][N_MEAS_OF_KF];
1139  MAKE_MATRIX_PTR(INVS, _INVS, N_MEAS_OF_KF);
1140  float_mat_invert(INVS, S, N_MEAS_OF_KF);
1141  if (DEBUG_INS_FLOW) {
1142  // This should be the identity matrix:
1143  float _SINVS[N_MEAS_OF_KF][N_MEAS_OF_KF];
1144  MAKE_MATRIX_PTR(SINVS, _SINVS, N_MEAS_OF_KF);
1146  DEBUG_PRINT("S*Inv(S):\n");
1148  }
1149 
1151  DEBUG_PRINT("K:\n");
1153 
1154  // Correct the state:
1155  // MATLAB:
1156  // Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad;
1157  // (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
1158 
1159  float Z_expected[N_MEAS_OF_KF];
1160 
1161  // TODO: take this var out? It was meant for debugging...
1162  //float Z_expect_GT_angle;
1163 
1164  if (CONSTANT_ALT_FILTER) {
1165  Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
1166  + OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
1167 
1168 
1169  /* TODO: remove later, just for debugging:
1170  float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND];
1171  float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]+OF_X[OF_ANGLE_DOT_IND];
1172  printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate,
1173  ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
1174  if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) {
1175  printf("NO RATE WINS!");
1176  }
1177  printf("\n");*/
1178 
1179  Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
1180 
1181  if (OF_TWO_DIM) {
1182  Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
1183  OF_X[OF_Z_IND]; // TODO: no q?
1184  }
1185 
1186  //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND];
1187 
1188  if (OF_USE_GYROS) {
1189  Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
1190  }
1191  } else {
1192  Z_expected[OF_LAT_FLOW_IND] = -OF_X[OF_V_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND]
1193  + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
1194  + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
1195  // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
1196 
1197  Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
1198  - OF_X[OF_Z_DOT_IND] * cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
1199 
1200  //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]
1201  // + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
1202  //+ OF_X[OF_ANGLE_DOT_IND];
1203  if (N_MEAS_OF_KF == 3) {
1204  Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
1205  }
1206 
1207  /*
1208  float Z_exp_no_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
1209  + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
1210  float Z_exp_with_rate = -OF_X[OF_V_IND]*cosf(OF_X[OF_ANGLE_IND])*cosf(OF_X[OF_ANGLE_IND])/OF_X[OF_Z_IND]
1211  + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
1212  + OF_X[OF_ANGLE_DOT_IND];
1213  */
1214  /*
1215  printf("Z_exp_no_rate = %f, Z_exp_with_rate = %f, measured = %f, angle dot = %f, p = %f: ", Z_exp_no_rate, Z_exp_with_rate,
1216  ins_flow.optical_flow_x, OF_X[OF_ANGLE_DOT_IND], dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
1217  if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) {
1218  printf("NO RATE WINS!");
1219  }
1220  printf("\n");*/
1221  }
1222 
1223  // i_k1 = Z - Z_expected;
1224  float innovation[N_MEAS_OF_KF][1];
1225  //print_ins_flow_state();
1226  innovation[OF_LAT_FLOW_IND][0] = ins_flow.optical_flow_x - Z_expected[OF_LAT_FLOW_IND];
1227  DEBUG_PRINT("Expected flow filter: %f, Expected flow ground truth = %f, Real flow x: %f, Real flow y: %f.\n",
1228  Z_expected[OF_LAT_FLOW_IND], Z_expect_GT_angle, ins_flow.optical_flow_x, ins_flow.optical_flow_y);
1229  innovation[OF_DIV_FLOW_IND][0] = ins_flow.divergence - Z_expected[OF_DIV_FLOW_IND];
1230  DEBUG_PRINT("Expected div: %f, Real div: %f.\n", Z_expected[OF_DIV_FLOW_IND], ins_flow.divergence);
1232  innovation[OF_LAT_FLOW_X_IND][0] = ins_flow.optical_flow_y - Z_expected[OF_LAT_FLOW_X_IND];
1233  DEBUG_PRINT("Expected flow in body X direction filter: %f, Real flow in corresponding y direction: %f, gyro = %f, expected velocity = %f, real velocity = %f, expected theta = %f, real theta = %f.\n",
1235  OF_X[OF_VX_IND], velocities->x, OF_X[OF_THETA_IND], eulers->theta);
1236  }
1237  if (OF_USE_GYROS) {
1238  float gyro_meas_roll;
1239  if (!PREDICT_GYROS) {
1240  gyro_meas_roll = (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f;
1241  } else {
1242  // TODO: You can fake gyros here by estimating them as follows:
1243  // rate_p_filt_est = -1.8457e-04 * cmd_roll;
1244  // gyro_meas_roll = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1245  // gyro_meas_roll = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1246 
1247  // gyro_meas_roll = 1e-04 * parameters[PAR_PRED_ROLL_1] * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1248  // gyro_meas_roll = parameters[PAR_PRED_ROLL_2] * gyro_meas_roll + 1E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x;
1249 
1250  // only flow:
1251  gyro_meas_roll = 2E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x;
1252 
1253  //printf("Predicted roll: %f, real measured roll: %f.\n", gyro_meas_roll, (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f);
1254  }
1255 
1256  innovation[OF_RATE_IND][0] = gyro_meas_roll - Z_expected[OF_RATE_IND];
1257  //innovation[OF_RATE_IND][0] = rates->p - Z_expected[OF_RATE_IND];
1258  DEBUG_PRINT("Expected rate: %f, Real rate: %f.\n", Z_expected[OF_RATE_IND], ins_flow.lp_gyro_roll);
1259  }
1260 
1261  MAKE_MATRIX_PTR(I, innovation, N_MEAS_OF_KF);
1262  DEBUG_PRINT("Innovation:");
1264 
1265  // X_k1_k1 = X_k1_k + K_k1*(i_k1);
1266  float _KI[N_STATES_OF_KF][1];
1267  MAKE_MATRIX_PTR(KI, _KI, N_STATES_OF_KF);
1269 
1270  DEBUG_PRINT("K*innovation:\n");
1272 
1273  DEBUG_PRINT("PRE: v = %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]);
1274  for (int i = 0; i < N_STATES_OF_KF; i++) {
1275  OF_X[i] += KI[i][0];
1276  }
1277  DEBUG_PRINT("POST v: %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]);
1278 
1279  DEBUG_PRINT("Angles (deg): ahrs = %f, ekf = %f.\n", (180.0f / M_PI)*eulers->phi, (180.0f / M_PI)*OF_X[OF_ANGLE_IND]);
1280 
1281  DEBUG_PRINT("P before correction:\n");
1283 
1284  // P_k1_k1 = (eye(Nx) - K_k1*Hx)*P_k1_k*(eye(Nx) - K_k1*Hx)' + K_k1*R*K_k1'; % Joseph form of the covariance update equation
1285  float _KJac[N_STATES_OF_KF][N_STATES_OF_KF];
1286  MAKE_MATRIX_PTR(KJac, _KJac, N_STATES_OF_KF);
1288 
1289  float _eye[N_STATES_OF_KF][N_STATES_OF_KF];
1290  MAKE_MATRIX_PTR(eye, _eye, N_STATES_OF_KF);
1292  DEBUG_PRINT("eye:\n");
1294 
1295  float _eKJac[N_STATES_OF_KF][N_STATES_OF_KF];
1296  MAKE_MATRIX_PTR(eKJac, _eKJac, N_STATES_OF_KF);
1297  float_mat_diff(eKJac, eye, KJac, N_STATES_OF_KF, N_STATES_OF_KF);
1298  DEBUG_PRINT("eKJac:\n");
1300 
1301  float _eKJacT[N_STATES_OF_KF][N_STATES_OF_KF];
1302  MAKE_MATRIX_PTR(eKJacT, _eKJacT, N_STATES_OF_KF);
1304  // (eye(Nx) - K_k1*Hx)*P_k1_k*(eye(Nx) - K_k1*Hx)'
1305  float _P_pre[N_STATES_OF_KF][N_STATES_OF_KF];
1306  MAKE_MATRIX_PTR(P_pre, _P_pre, N_STATES_OF_KF);
1309  DEBUG_PRINT("eKJac * P *eKJacT:\n");
1311 
1312  // K_k1*R*K_k1'
1313  // TODO: check all MAKE_MATRIX that they mention the number of ROWS!
1314  float _KT[N_MEAS_OF_KF][N_STATES_OF_KF];
1315  MAKE_MATRIX_PTR(KT, _KT, N_MEAS_OF_KF);
1317  float _RKT[N_MEAS_OF_KF][N_STATES_OF_KF];
1318  MAKE_MATRIX_PTR(RKT, _RKT, N_MEAS_OF_KF);
1320  float _KRKT[N_STATES_OF_KF][N_STATES_OF_KF];
1321  MAKE_MATRIX_PTR(KRKT, _KRKT, N_STATES_OF_KF);
1323  DEBUG_PRINT("KRKT:\n");
1325 
1326  // summing the two parts:
1328 
1329  DEBUG_PRINT("P corrected:\n");
1331  float trace_P = 0.0f;
1332  for (int i = 0; i < N_STATES_OF_KF; i++) {
1333  trace_P += P[i][i];
1334  }
1335  DEBUG_PRINT("trace P = %f\n", trace_P);
1336 
1337  // indicate that the measurement has been used:
1339  }
1340 
1341  // update the time:
1343 
1344 }
1345 
1346 static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
1347  uint32_t stamp, struct Int32Rates *gyro)
1348 {
1349  ahrs_icq_last_stamp = stamp;
1350 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
1351  PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.")
1352  /* timestamp in usec when last callback was received */
1353  static uint32_t last_stamp = 0;
1354 
1355  if (last_stamp > 0 && ahrs_icq.is_aligned) {
1356  float dt = (float)(stamp - last_stamp) * 1e-6;
1357  ahrs_icq_propagate(gyro, dt);
1359 
1360  // TODO: filter all gyro values
1361  // For now only filter the roll gyro:
1362  float current_rate = ((float)gyro->p); // TODO: is this correct? / INT32_RATE_FRAC;
1363  ins_flow.lp_gyro_roll = lp_factor * ins_flow.lp_gyro_roll + (1 - lp_factor) * current_rate;
1364  current_rate = ((float)gyro->q);
1365  ins_flow.lp_gyro_pitch = lp_factor * ins_flow.lp_gyro_pitch + (1 - lp_factor) * current_rate;
1366  }
1367  last_stamp = stamp;
1368 #else
1369  PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
1370  PRINT_CONFIG_VAR(AHRS_PROPAGATE_FREQUENCY)
1371  if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
1372  const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
1373  ahrs_icq_propagate(gyro, dt);
1375  }
1376 #endif
1377 }
1378 
1379 static void accel_cb(uint8_t __attribute__((unused)) sender_id,
1380  uint32_t __attribute__((unused)) stamp,
1381  struct Int32Vect3 *accel)
1382 {
1383 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
1384  PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
1385  static uint32_t last_stamp = 0;
1386  if (last_stamp > 0 && ahrs_icq.is_aligned) {
1387  float dt = (float)(stamp - last_stamp) * 1e-6;
1388  ahrs_icq_update_accel(accel, dt);
1390  }
1391  last_stamp = stamp;
1392 #else
1393  PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
1394  PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
1395  if (ahrs_icq.is_aligned) {
1396  const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
1397  ahrs_icq_update_accel(accel, dt);
1399  }
1400 #endif
1401 }
1402 
1403 /*
1404 static void mag_cb(uint8_t __attribute__((unused)) sender_id,
1405  uint32_t __attribute__((unused)) stamp,
1406  struct Int32Vect3 *mag)
1407 {
1408 #if USE_AUTO_AHRS_FREQ || !defined(AHRS_MAG_CORRECT_FREQUENCY)
1409  PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat mag update.")
1410  static uint32_t last_stamp = 0;
1411  if (last_stamp > 0 && ahrs_icq.is_aligned) {
1412  float dt = (float)(stamp - last_stamp) * 1e-6;
1413  //ahrs_icq_update_mag(mag, dt);
1414  //set_body_state_from_quat();
1415  }
1416  last_stamp = stamp;
1417 #else
1418  PRINT_CONFIG_MSG("Using fixed AHRS_MAG_CORRECT_FREQUENCY for AHRS int_cmpl_quat mag update.")
1419  PRINT_CONFIG_VAR(AHRS_MAG_CORRECT_FREQUENCY)
1420  if (ahrs_icq.is_aligned) {
1421  const float dt = 1. / (AHRS_MAG_CORRECT_FREQUENCY);
1422  ahrs_icq_update_mag(mag, dt);
1423  set_body_state_from_quat();
1424  }
1425 #endif
1426 }
1427 
1428 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
1429 {
1430  //VECT3_ASSIGN(ahrs_icq.mag_h, MAG_BFP_OF_REAL(h->x), MAG_BFP_OF_REAL(h->y),
1431  // MAG_BFP_OF_REAL(h->z));
1432 }
1433 */
1434 
1436 static void set_body_state_from_quat(void)
1437 {
1438  /* Compute LTP to BODY quaternion */
1439  struct Int32Quat ltp_to_body_quat = ahrs_icq.ltp_to_body_quat;
1440  //struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
1441  //int32_quat_comp_inv(&ltp_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
1442 
1443  if (use_filter < USE_ANGLE) {
1444  // Use the orientation as is:
1445  stateSetNedToBodyQuat_i(&ltp_to_body_quat);
1446  } else {
1447 
1448  // get Euler angles:
1449  struct OrientationReps orient;
1450  orient.status = 1 << ORREP_QUAT_I;
1451  orient.quat_i = ltp_to_body_quat;
1452  struct FloatEulers *eulers = orientationGetEulers_f(&orient);
1453 
1454  // set roll angle with value from the filter:
1455  GT_phi = eulers->phi;
1456  eulers->phi = OF_X[OF_ANGLE_IND];
1457  if (OF_TWO_DIM) {
1458  GT_theta = eulers->theta;
1459  //printf("Real theta = %f, setting theta to %f.\n", eulers->theta, OF_X[OF_THETA_IND]);
1460  eulers->theta = OF_X[OF_THETA_IND];
1461  }
1462 
1463  // Transform the Euler representation to Int32Quat and set the state:
1464  struct OrientationReps orient_euler;
1465  orient_euler.status = 1 << ORREP_EULER_F;
1466  orient_euler.eulers_f = (*eulers);
1467 
1468  struct Int32Quat *quat_i_adapted = orientationGetQuat_i(&orient_euler);
1469  stateSetNedToBodyQuat_i(quat_i_adapted);
1470  }
1471 
1472  /* compute body rates */
1473  struct Int32Rates body_rate = ahrs_icq.body_rate;
1474  // struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
1475  // int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
1476  /* Set state */
1477  stateSetBodyRates_i(&body_rate);
1478 
1479 }
1480 
1481 static void ins_act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
1482 {
1483  ins_flow.RPM_num_act = num_act;
1484  for (int i = 0; i < num_act; i++) {
1485  if (feedback[i].set.rpm) {
1486  ins_flow.RPM[i] = feedback[i].rpm;
1487  }
1488  }
1489 }
1490 
1491 
1492 /* Update INS based on GPS information */
1493 static void gps_cb(uint8_t sender_id __attribute__((unused)),
1494  uint32_t stamp UNUSED,
1495  struct GpsState *gps_s)
1496 {
1497 
1498  if (gps_s->fix < GPS_FIX_3D) {
1499  return;
1500  }
1501  if (!ins_flow.ltp_initialized) {
1503  }
1504 
1505  ahrs_icq_update_gps(gps_s);
1506 
1507  /* simply scale and copy pos/speed from gps */
1508  struct NedCoor_i gps_pos_cm_ned;
1509  ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_flow.ltp_def, &gps_s->ecef_pos);
1510  INT32_VECT3_SCALE_2(ins_flow.ltp_pos, gps_pos_cm_ned,
1512 
1513  if (use_filter >= USE_HEIGHT) {
1514  //struct NedCoor_f* position = stateGetPositionNed_f();
1515  int32_t z_Ned_i_filter = - (int32_t)((OF_X[OF_Z_IND] * INT32_POS_OF_CM_NUM * 100) / INT32_POS_OF_CM_DEN);
1516  //printf("Z true: %f / %d, Z filter: %f / %d. (float / int32_t)\n", position->z, ins_flow.ltp_pos.z, OF_X[OF_Z_IND], z_Ned_i);
1517  ins_flow.ltp_pos.z = z_Ned_i_filter;
1518  }
1519 
1521 
1522 
1523 
1524  struct NedCoor_i gps_speed_cm_s_ned;
1525  ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_flow.ltp_def, &gps_s->ecef_vel);
1526  INT32_VECT3_SCALE_2(ins_flow.ltp_speed, gps_speed_cm_s_ned,
1528 
1529  if (use_filter >= USE_VELOCITY) {
1530  // get NED to body rotation matrix:
1531  struct FloatRMat *NTB = stateGetNedToBodyRMat_f();
1532  // get transpose (inverse):
1533  struct FloatRMat BTN;
1534  float_rmat_inv(&BTN, NTB);
1535 
1536  // the velocities from the filter are rotated from the body to the inertial frame:
1537  struct FloatVect3 NED_velocities, body_velocities;
1538  body_velocities.x = 0.0f; // filter does not determine this yet
1539  body_velocities.y = OF_X[OF_V_IND];
1540  body_velocities.z = 0.0f;
1541  /*
1542  if(CONSTANT_ALT_FILTER) {
1543  body_velocities.z = 0.0f;
1544  }
1545  else {
1546  body_velocities.z = OF_X[OF_Z_DOT_IND];
1547  }*/
1548  float_rmat_vmult(&NED_velocities, &BTN, &body_velocities);
1549  // TODO: also estimate vx, so that we can just use the rotated vector:
1550  // For now, we need to keep the x, and y body axes aligned with the global ones.
1551  // printf("Original speed y = %d, ", ins_flow.ltp_speed.y);
1554  // printf("Changed speed y = %d (%f in float)\n", ins_flow.ltp_speed.y, NED_velocities.y);
1555  }
1556 
1558 
1559  /*
1560  bool vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
1561  uint8_t nsats = gps_s->num_sv;
1562  */
1563 }
1564 
1565 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
1566  uint32_t stamp __attribute__((unused)),
1567  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
1568  struct Int32Vect3 *lp_mag)
1569 {
1570  if (!ahrs_icq.is_aligned) {
1571  if (ahrs_icq_align(lp_gyro, lp_accel, lp_mag)) {
1573  }
1574  }
1575 }
1576 
1577 
1578 /* Save the Body to IMU information */
1580 //static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
1581 // struct FloatQuat *q_b2i_f)
1582 //{
1583 // ahrs_icq_set_body_to_imu_quat(q_b2i_f);
1584 //}
Main include for ABI (AirBorneInterface).
Event structure to store callbacks in a linked list.
Definition: abi_common.h:67
#define AHRS_COMP_ID_FLOW
Definition: ahrs.h:46
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:84
bool ahrs_icq_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt)
struct AhrsIntCmplQuat ahrs_icq
Default Rate filter Low pass.
void ahrs_icq_init(void)
void ahrs_icq_update_gps(struct GpsState *gps_s)
void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
struct Int32Rates gyro_bias
@ AHRS_ICQ_RUNNING
struct Int32Vect3 mag_h
struct Int32Rates body_rate
struct Int32Quat ltp_to_body_quat
enum AhrsICQStatus status
status of the AHRS, AHRS_ICQ_UNINIT or AHRS_ICQ_RUNNING
bool autopilot_in_flight(void)
get in_flight flag
Definition: autopilot.c:340
Core autopilot interface common to all firmwares.
float g
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
uint8_t last_wp UNUSED
if(GpsFixValid() &&e_identification_started)
struct GpsState gps
global GPS state
Definition: gps.c:69
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:91
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:90
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:94
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
uint8_t fix
status of fix
Definition: gps.h:106
data structure for GPS information
Definition: gps.h:86
float q
in rad/s
float phi
in radians
float p
in rad/s
float theta
in radians
static void float_mat_diff(float **o, float **a, float **b, int m, int n)
o = a - b
static void float_mat_sum(float **o, float **a, float **b, int m, int n)
o = a + b
void float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
static void float_mat_mul(float **o, float **a, float **b, int m, int n, int l)
o = a * b
static void float_mat_transpose(float **o, float **a, int n, int m)
transpose non-square matrix
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_mat_invert(float **o, float **mat, int n)
Calculate inverse of any n x n matrix (passed as C array) o = mat^-1 Algorithm verified with Matlab.
static void float_mat_diagonal_scal(float **o, float v, int n)
Make an n x n identity matrix (for matrix passed as array)
void float_rmat_vmult(struct FloatVect3 *vb, struct FloatRMat *m_a2b, struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
euler angles
rotation matrix
angular rates
int32_t p
in rad/s with INT32_RATE_FRAC
int32_t r
in rad/s with INT32_RATE_FRAC
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t q
in rad/s with INT32_RATE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
#define INT32_POS_OF_CM_DEN
#define INT32_SPEED_OF_CM_S_NUM
#define INT32_SPEED_OF_CM_S_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
#define MAG_FLOAT_OF_BFP(_ai)
euler angles
Rotation quaternion.
angular rates
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Down.
int32_t z
in centimeters
struct LlaCoor_i lla
Reference point in lla.
int32_t x
in centimeters
int32_t y
East.
struct EcefCoor_i ecef
Reference point in ecef.
int32_t y
in centimeters
int32_t lon
in degrees*1e7
int32_t x
North.
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
void ned_of_ecef_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Convert a point from ECEF to local NED.
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
#define E
struct FloatEulers eulers_f
Orienation in zyx euler angles.
struct Int32Quat quat_i
Orientation quaternion.
uint8_t status
Holds the status bits for all orientation representations.
#define ORREP_QUAT_I
Quaternion (BFP int)
#define ORREP_EULER_F
zyx Euler (float)
static struct Int32Quat * orientationGetQuat_i(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (int).
static struct FloatEulers * orientationGetEulers_f(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (float).
static struct Int32Quat * stateGetNedToBodyQuat_i(void)
Get vehicle body attitude quaternion (int).
Definition: state.h:1113
static void stateSetNedToBodyQuat_i(struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition: state.h:1075
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1125
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition: state.h:1137
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static void stateSetPositionNed_i(struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition: state.h:531
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1173
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static void stateSetSpeedNed_i(struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition: state.h:763
#define AHRS_PROPAGATE_FREQUENCY
Definition: hf_float.c:55
static float p[2][2]
#define INS_RPM_ID
Definition: ins_flow.c:99
float lp_factor
Definition: ins_flow.c:187
#define DEBUG_INS_FLOW
Definition: ins_flow.c:49
void ins_reset_local_origin(void)
INS local origin reset.
Definition: ins_flow.c:666
#define PAR_P3
Definition: ins_flow.c:277
int moment_ind
Definition: ins_flow.c:175
int use_filter
Definition: ins_flow.c:190
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition: ins_flow.c:1493
float GT_theta
Definition: ins_flow.c:196
#define PAR_P2
Definition: ins_flow.c:276
#define INS_FLOW_GPS_ID
Definition: ins_flow.c:87
float moments[MOMENT_DELAY]
Definition: ins_flow.c:174
static void print_ins_flow_state(void)
Definition: ins_flow.c:694
float parameters[20]
Definition: ins_flow.c:240
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:365
#define PAR_PRED_ROLL_3
Definition: ins_flow.c:284
#define PAR_Q_TB
Definition: ins_flow.c:280
#define PAR_Q4
Definition: ins_flow.c:273
#define MOMENT_DELAY
Definition: ins_flow.c:173
struct NedCoor_i ltp_speed
Definition: ins_flow.c:146
bool run_filter
Definition: ins_flow.c:191
float optical_flow_y
Definition: ins_flow.c:151
static abi_event accel_ev
Definition: ins_flow.c:105
float lp_thrust
Definition: ins_flow.c:165
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition: ins_flow.c:1346
float OF_P[N_STATES_OF_KF][N_STATES_OF_KF]
Definition: ins_flow.c:179
static uint8_t ahrs_flow_id
Component ID for FLOW.
Definition: ins_flow.c:134
void ins_flow_init(void)
Definition: ins_flow.c:537
#define INS_OPTICAL_FLOW_ID
Definition: ins_flow.c:93
uint32_t counter
Definition: ins_flow.c:192
static void print_true_state(void)
Definition: ins_flow.c:719
float OF_X[N_STATES_OF_KF]
Definition: ins_flow.c:177
float thrust_factor
Definition: ins_flow.c:164
#define INS_FLOW_IMU_ID
Definition: ins_flow.c:80
float vision_time
Definition: ins_flow.c:153
#define OF_N_ROTORS
Definition: ins_flow.c:182
static abi_event gyro_ev
Definition: ins_flow.c:104
float of_time
Definition: ins_flow.c:185
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:381
struct NedCoor_i ltp_accel
Definition: ins_flow.c:147
float RPM_FACTORS[OF_N_ROTORS]
Definition: ins_flow.c:183
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:343
static abi_event aligner_ev
Definition: ins_flow.c:109
float lp_gyro_pitch
Definition: ins_flow.c:160
#define PAR_P4
Definition: ins_flow.c:278
uint16_t RPM[8]
Definition: ins_flow.c:157
#define PAR_KD
Definition: ins_flow.c:279
#define PAR_Q3
Definition: ins_flow.c:272
static void ins_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
Definition: ins_flow.c:1481
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:353
#define DEBUG_PRINT(...)
Definition: ins_flow.c:57
struct InsFlow ins_flow
Definition: ins_flow.c:169
static void send_quat(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:304
static abi_event ins_act_feedback_ev
Definition: ins_flow.c:108
uint8_t RPM_num_act
Definition: ins_flow.c:158
float of_prev_time
Definition: ins_flow.c:186
float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF]
Definition: ins_flow.c:178
float thrust_factor
Definition: ins_flow.c:193
static void ins_reset_filter(void)
Definition: ins_flow.c:494
#define PAR_P1
Definition: ins_flow.c:275
#define INS_FLOW_GYRO_ID
Definition: ins_flow.c:68
struct NedCoor_i ltp_pos
Definition: ins_flow.c:145
void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
Definition: ins_flow.c:675
float lp_factor_strong
Definition: ins_flow.c:188
#define PAR_P_TB
Definition: ins_flow.c:281
float lp_gyro_roll
Definition: ins_flow.c:162
#define PAR_K1
Definition: ins_flow.c:264
float lp_gyro_bias_pitch
Definition: ins_flow.c:161
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition: ins_flow.c:1379
void ins_flow_update(void)
Definition: ins_flow.c:732
#define PAR_K0
Definition: ins_flow.c:263
#define PAR_IX
Definition: ins_flow.c:260
#define PAR_Q0
Definition: ins_flow.c:269
struct LtpDef_i ltp_def
Definition: ins_flow.c:141
static uint32_t ahrs_icq_last_stamp
Definition: ins_flow.c:133
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ins_flow.c:1565
static void send_euler(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:320
static void set_body_state_from_quat(void)
Rotate angles and rates from imu to body frame and set state.
Definition: ins_flow.c:1436
bool ltp_initialized
Definition: ins_flow.c:142
bool reset_filter
Definition: ins_flow.c:189
float optical_flow_x
Definition: ins_flow.c:150
static void send_bias(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:336
#define PAR_K2
Definition: ins_flow.c:265
float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF]
Definition: ins_flow.c:180
static abi_event ins_optical_flow_ev
Definition: ins_flow.c:107
#define PAR_R1
Definition: ins_flow.c:268
float lp_roll_command
Definition: ins_flow.c:166
float lp_gyro_bias_roll
Definition: ins_flow.c:163
#define PAR_R0
Definition: ins_flow.c:267
#define PAR_Q2
Definition: ins_flow.c:271
#define PAR_Q1
Definition: ins_flow.c:270
#define PAR_K3
Definition: ins_flow.c:266
#define DEBUG_MAT_PRINT(...)
Definition: ins_flow.c:58
float divergence
Definition: ins_flow.c:152
#define AHRS_ICQ_OUTPUT_ENABLED
Definition: ins_flow.c:62
#define PAR_P0
Definition: ins_flow.c:274
#define INS_FLOW_ACCEL_ID
Definition: ins_flow.c:74
static abi_event gps_ev
Definition: ins_flow.c:106
bool new_flow_measurement
Definition: ins_flow.c:154
float GT_phi
Definition: ins_flow.c:195
#define PAR_MASS
Definition: ins_flow.c:261
static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
Definition: ins_flow.c:395
#define OF_THRUST_BIAS
Definition: ins_flow.h:40
#define CONSTANT_ALT_FILTER
Definition: ins_flow.h:35
#define OF_V_IND
Definition: ins_flow.h:76
#define USE_HEIGHT
Definition: ins_flow.h:145
#define USE_VELOCITY
Definition: ins_flow.h:144
#define OF_THETA_IND
Definition: ins_flow.h:79
#define OF_DIV_FLOW_IND
Definition: ins_flow.h:133
#define OF_USE_GYROS
Definition: ins_flow.h:42
#define OF_ANGLE_DOT_IND
Definition: ins_flow.h:81
#define OF_LAT_FLOW_IND
Definition: ins_flow.h:132
#define PREDICT_GYROS
Definition: ins_flow.h:44
#define OF_LAT_FLOW_X_IND
Definition: ins_flow.h:136
#define OF_ANGLE_IND
Definition: ins_flow.h:77
#define OF_DRAG
Definition: ins_flow.h:36
#define N_MEAS_OF_KF
Definition: ins_flow.h:90
#define OF_THRUST_BIAS_IND
Definition: ins_flow.h:85
#define OF_Z_IND
Definition: ins_flow.h:78
#define OF_Z_DOT_IND
Definition: ins_flow.h:97
#define OF_VX_IND
Definition: ins_flow.h:80
#define OF_TWO_DIM
Definition: ins_flow.h:38
#define USE_ANGLE
Definition: ins_flow.h:143
#define OF_RATE_IND
Definition: ins_flow.h:134
#define N_STATES_OF_KF
Definition: ins_flow.h:75
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static struct FloatVect3 H
int32_t rpm
RPM.
Definition: actuators.h:51
Motor Mixing.
Paparazzi floating point algebra.
float z
in meters
float x
in meters
float y
in meters
vector in North East Down coordinates Units: meters
Simple matrix helper macros.
struct Stabilization stabilization
Definition: stabilization.c:41
General stabilization interface for rotorcrafts.
int32_t cmd[COMMANDS_NB]
output command vector, range from [-MAX_PPRZ:MAX_PPRZ] (store for messages)
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
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
uint16_t val[TCOUPLE_NB]
static float P[3][3]
Definition: trilateration.c:31
static float K[9]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98