Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
35
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
65
66/* default Gyro to use in INS */
67#ifndef INS_FLOW_GYRO_ID
68#define INS_FLOW_GYRO_ID ABI_BROADCAST
69#endif
71
72/* default Accelerometer to use in INS */
73#ifndef INS_FLOW_ACCEL_ID
74#define INS_FLOW_ACCEL_ID ABI_BROADCAST
75#endif
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
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
90
91/* Use optical flow estimates */
92#ifndef INS_OPTICAL_FLOW_ID
93#define INS_OPTICAL_FLOW_ID ABI_BROADCAST
94#endif
96
97// reading RPMs:
98#ifndef INS_RPM_ID
99#define INS_RPM_ID ABI_BROADCAST
100#endif
102
103/* All registered ABI events */
111
112/* All ABI callbacks */
113static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro);
114static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel);
115static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
116void ins_optical_flow_cb(uint8_t sender_id, uint32_t stamp, int32_t flow_x,
117 int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence);
118static void ins_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act);
119static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
120 uint32_t stamp __attribute__((unused)),
121 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
122 struct Int32Vect3 *lp_mag);
123static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag);
124
125static void print_ins_flow_state(void);
126static void print_true_state(void);
127/* Static local functions */
130static void set_body_state_from_quat(void);
131static void ins_reset_filter(void);
132
133struct InsFlow {
134
135 // data elements for gps passthrough:
138
139 /* output LTP NED */
143
144 // vision measurements:
148 float vision_time; // perhaps better to use microseconds (us) instead of float in seconds
150
151 // RPMs:
152 uint16_t RPM[8]; // max an octocopter
154
156 float lp_gyro_bias_pitch; // determine the bias before take-off
158 float lp_gyro_bias_roll; // determine the bias before take-off
159 float thrust_factor; // determine the additional required scale factor to have unbiased thrust estimates
162
163};
165
166// Kalman filter parameters and variables:
167
168#define MOMENT_DELAY 20
169float moments[MOMENT_DELAY] = {0.};
171
172float OF_X[N_STATES_OF_KF] = {0.};
176
177#define OF_N_ROTORS 4
179
189
190float GT_phi;
192
193
194#define USE_STANDARD_PARAMS 0
195
196#if USE_STANDARD_PARAMS
197// Note that not all of the values are used.
198// Moment of Inertia, mass, distance motors from center of gravity, 4 params for thrust and moment generation,
199// measurement noise R (2), actuation noise Q(5),
200// initial P (5), linear drag factor
201#if USE_NPS
202float parameters[20] = {0.0018244, 0.400, 0.085, 0.152163; 0.170734; 0.103436; 0.122109,
203 0.1, 0.1, 0.1, 1.0f * M_PI / 180.0f, 100.0f * M_PI / 180.0f, 0.1f, 3.0f,
204 1.0f, 10.0f * M_PI / 180.0f, 10.0f * M_PI / 180.0f, 1.0f, 1.0f, 0.5f
205 };
206#else
207float parameters[20] = {0.0018244, 0.400, 0.085, 0.108068 0.115448 0.201207 0.208834,
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#endif
212#else
213// Define parameters for the filter, fitted in MATLAB:
214#if USE_NPS
215#if N_MEAS_OF_KF == 3
216// with rate measurement:
217float 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};
218#else
219// without rate state / measurement:
220#if CONSTANT_ALT_FILTER
221#if OF_DRAG
222float 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};
223#else
224float 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};
225#endif
226#else
227float 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};
228#endif
229#endif
230#else
231// TODO: train constant alt filter without drag, also with and without measuring the gyro.
232#if CONSTANT_ALT_FILTER
233#if OF_DRAG
234// 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};
235float 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};
236#endif
237#else
238#if N_MEAS_OF_KF == 3
239#if PREDICT_GYROS == 0
240// with rate measurement (last two values copied from the last condition)
241float 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};
242#else
243// Estimate gyro directly instead of moment:
244float 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};
245#endif
246#else
247// without rate measurement:
248// 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};
249float 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};
250#endif
251#endif
252#endif
253#endif
254// parameter indices (TODO: how important are these numbers? Some are not used, others, like P may be not so important).
255#define PAR_IX 0
256#define PAR_MASS 1
257#define PAR_BASE 2
258#define PAR_K0 3
259#define PAR_K1 4
260#define PAR_K2 5
261#define PAR_K3 6
262#define PAR_R0 7
263#define PAR_R1 8
264#define PAR_Q0 9
265#define PAR_Q1 10
266#define PAR_Q2 11
267#define PAR_Q3 12
268#define PAR_Q4 13
269#define PAR_P0 14
270#define PAR_P1 15
271#define PAR_P2 16
272#define PAR_P3 17
273#define PAR_P4 18
274#define PAR_KD 19
275#define PAR_Q_TB 20
276#define PAR_P_TB 21
277#define PAR_PRED_ROLL_1 22
278#define PAR_PRED_ROLL_2 23
279#define PAR_PRED_ROLL_3 24
280
281
282
283/*
284struct InsFlowState {
285 // vector representation of state:
286 // v, angle, angle_dot, z, z_dot
287
288
289};
290struct InsFlowState ins_flow_state;
291*/
292
293#if PERIODIC_TELEMETRY
295#include "mcu_periph/sys_time.h"
296#include "state.h"
297
298// attitude part:
299static void send_quat(struct transport_tx *trans, struct link_device *dev)
300{
301 struct Int32Quat *quat = stateGetNedToBodyQuat_i();
302 pprz_msg_send_AHRS_QUAT_INT(trans, dev, AC_ID,
303 &ahrs_icq.weight,
304 &ahrs_icq.ltp_to_body_quat.qi,
305 &ahrs_icq.ltp_to_body_quat.qx,
306 &ahrs_icq.ltp_to_body_quat.qy,
307 &ahrs_icq.ltp_to_body_quat.qz,
308 &(quat->qi),
309 &(quat->qx),
310 &(quat->qy),
311 &(quat->qz),
312 &ahrs_flow_id);
313}
314
315static void send_euler(struct transport_tx *trans, struct link_device *dev)
316{
317 struct Int32Eulers ltp_to_imu_euler;
318 int32_eulers_of_quat(&ltp_to_imu_euler, &ahrs_icq.ltp_to_body_quat);
319 struct Int32Eulers *eulers = stateGetNedToBodyEulers_i();
320 pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
321 &ltp_to_imu_euler.phi,
322 &ltp_to_imu_euler.theta,
323 &ltp_to_imu_euler.psi,
324 &(eulers->phi),
325 &(eulers->theta),
326 &(eulers->psi),
327 &ahrs_flow_id);
328
329}
330
331static void send_bias(struct transport_tx *trans, struct link_device *dev)
332{
333 pprz_msg_send_AHRS_GYRO_BIAS_INT(trans, dev, AC_ID,
334 &ahrs_icq.gyro_bias.p, &ahrs_icq.gyro_bias.q,
335 &ahrs_icq.gyro_bias.r, &ahrs_flow_id);
336}
337
338static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
339{
340 struct FloatVect3 h_float;
341 h_float.x = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.x);
342 h_float.y = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.y);
343 h_float.z = MAG_FLOAT_OF_BFP(ahrs_icq.mag_h.z);
344 pprz_msg_send_GEO_MAG(trans, dev, AC_ID,
345 &h_float.x, &h_float.y, &h_float.z, &ahrs_flow_id);
346}
347
348static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
349{
350 uint8_t mde = 3;
351 uint16_t val = 0;
352 if (!ahrs_icq.is_aligned) { mde = 2; }
354 /* set lost if no new gyro measurements for 50ms */
355 if (t_diff > 50000) { mde = 5; }
356 pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &ahrs_flow_id, &mde, &val);
357}
358
359// ins part
360static void send_ins(struct transport_tx *trans, struct link_device *dev)
361{
362 pprz_msg_send_INS(trans, dev, AC_ID,
363 &ins_flow.ltp_pos.x, &ins_flow.ltp_pos.y, &ins_flow.ltp_pos.z,
364 &ins_flow.ltp_speed.x, &ins_flow.ltp_speed.y, &ins_flow.ltp_speed.z,
365 &ins_flow.ltp_accel.x, &ins_flow.ltp_accel.y, &ins_flow.ltp_accel.z);
366}
367
368/*static void send_ins_z(struct transport_tx *trans, struct link_device *dev)
369{
370 static float fake_baro_z = 0.0;
371 pprz_msg_send_INS_Z(trans, dev, AC_ID,
372 (float *)&fake_baro_z, &ins_flow.ltp_pos.z,
373 &ins_flow.ltp_speed.z, &ins_flow.ltp_accel.z);
374}*/
375
376static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
377{
378 static float fake_qfe = 0.0;
379 if (ins_flow.ltp_initialized) {
380 pprz_msg_send_INS_REF(trans, dev, AC_ID,
381 &ins_flow.ltp_def.ecef.x, &ins_flow.ltp_def.ecef.y, &ins_flow.ltp_def.ecef.z,
382 &ins_flow.ltp_def.lla.lat, &ins_flow.ltp_def.lla.lon, &ins_flow.ltp_def.lla.alt,
383 &ins_flow.ltp_def.hmsl, (float *)&fake_qfe);
384 }
385}
386
387
388#endif
389
390static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
391{
392 // TODO: add sending of theta:
393 struct FloatEulers *eulers = stateGetNedToBodyEulers_f();
394 struct NedCoor_f *position = stateGetPositionNed_f();
395 struct NedCoor_f *velocities = stateGetSpeedNed_f();
396 struct FloatRates *rates = stateGetBodyRates_f();
397
398 float phi = (180.0 / M_PI) * OF_X[OF_ANGLE_IND];
399 float theta;
400 if (OF_TWO_DIM) {
401 theta = (180.0 / M_PI) * OF_X[OF_THETA_IND];
402 } else {
403 // if not filtering the second dimension, just take the ground truth
404 theta = (180.0 / M_PI) * eulers->theta;
405 }
406
407 float phi_dot = 0.0f;
408 float z_dot = 0.0f;
409 if (!CONSTANT_ALT_FILTER) {
410 phi_dot = (180.0 / M_PI) * OF_X[OF_ANGLE_DOT_IND];
411 z_dot = OF_X[OF_Z_DOT_IND];
412 }
413
414 struct FloatRMat *NTB = stateGetNedToBodyRMat_f();
415 struct FloatVect3 NED_velocities, body_velocities;
416 NED_velocities.x = velocities->x;
417 NED_velocities.y = velocities->y;
418 NED_velocities.z = velocities->z;
419 float_rmat_vmult(&body_velocities, NTB, &NED_velocities);
420
421 float vy_GT = body_velocities.y;
422
423 float phi_GT;
424 if (use_filter < USE_ANGLE) {
425 phi_GT = (180.0 / M_PI) * eulers->phi;
426 } else {
427 phi_GT = (180.0 / M_PI) * GT_phi;
428 }
429 float vx_GT = body_velocities.x;
430 float theta_GT;
431 if (use_filter < USE_ANGLE) {
432 theta_GT = (180.0 / M_PI) * eulers->theta;
433 } else if (OF_TWO_DIM) {
434 theta_GT = (180.0 / M_PI) * GT_theta;
435 }
436 float p_GT = rates->p;
437 float q_GT = rates->q;
438 float z_GT = -position->z;
439 float vz_GT = -velocities->z;
440
441 float vy = OF_X[OF_V_IND];
442 float vx;
443 if (OF_TWO_DIM) {
444 vx = OF_X[OF_VX_IND];
445 } else {
446 vx = vx_GT;
447 }
448 float z = OF_X[OF_Z_IND];
449 float p, q;
450 if (!CONSTANT_ALT_FILTER) {
451 // normally:
452 p = phi_dot;
453 // when estimating the gyros:
454 // // p = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
455 // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
456 // TODO: expand the full filter later as well, to include q:
457 q = q_GT;
458 } else {
459 p = ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll;
460 // p = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
461 q = ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch;
462 }
463
464 float thrust_bias;
466 thrust_bias = 0.0f;
467 } else {
468 thrust_bias = OF_X[OF_THRUST_BIAS_IND];
469 }
470 // This code is to actually compare the unbiased thrust with gravity:
471 // TODO: code copied from below, put in a function?
472 float thrust = 0.0f;
473 for (int i = 0; i < OF_N_ROTORS; i++) {
474 thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
475 }
476 thrust *= thrust_factor; // ins_flow.thrust_factor;
477 thrust -= thrust_bias;
478 float mass = parameters[PAR_MASS];
479 float g = 9.81;
480 float actual_lp_thrust = mass * g;
481 thrust_bias = thrust - actual_lp_thrust;
482
483 pprz_msg_send_INS_FLOW_INFO(trans, dev, AC_ID,
484 &vy, &phi, &p, &vx, &theta, &q, &z, &z_dot,
485 &vy_GT, &phi_GT, &p_GT, &vx_GT, &theta_GT, &q_GT,
486 &z_GT, &vz_GT, &thrust_bias, &use_filter);
487}
488
490{
491
492 // (re-)initialize the state:
493 for (int i = 0; i < N_STATES_OF_KF; i++) {
494 OF_X[i] = 0.0f;
495 }
496 OF_X[OF_Z_IND] = 1.0; // nonzero z
497
498 // P-matrix:
499 for (int i = 0; i < N_STATES_OF_KF; i++) {
500 for (int j = 0; j < N_STATES_OF_KF; j++) {
501 OF_P[i][j] = 0.0f;
502 }
503 }
504 if (CONSTANT_ALT_FILTER == 1) {
509 if (OF_TWO_DIM) {
512 }
513 } else {
519 if (OF_THRUST_BIAS) {
521 }
522 }
523
524 counter = 0;
525
526 // TODO: what to do with thrust, gyro bias, and low-passed roll command?
527
528}
529
530
531/* Initialize the flow ins */
533{
534
536
537 struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
538 llh_nav0.lat = NAV_LAT0;
539 llh_nav0.lon = NAV_LON0;
540 /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
541 llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
542 struct EcefCoor_i ecef_nav0;
543 ecef_of_lla_i(&ecef_nav0, &llh_nav0);
544 struct LtpDef_i ltp_def;
545 ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
546
547 ltp_def_from_ecef_i(&ins_flow.ltp_def, &ecef_nav0);
548 ins_flow.ltp_def.hmsl = NAV_ALT0;
549 stateSetLocalOrigin_i(MODULE_INS_FLOW_ID, &ins_flow.ltp_def);
550 ins_flow.ltp_initialized = true;
551 ins_flow.new_flow_measurement = false;
552 ins_flow.lp_gyro_pitch = 0.0f;
553 ins_flow.lp_gyro_bias_pitch = 0.0f;
554 ins_flow.lp_gyro_roll = 0.0f;
555 ins_flow.lp_gyro_bias_roll = 0.0f;
556 ins_flow.thrust_factor = 1.0f;
557 ins_flow.lp_thrust = 0.0f;
558 ins_flow.lp_roll_command = 0.0f;
559
560 lp_factor = 0.95;
561 lp_factor_strong = 1 - 1E-3;
562
563 GT_phi = 0.0f;
564 GT_theta = 0.0f;
565
566 // Extended Kalman filter:
567 // reset the state and P matrix:
569
570 // R-matrix, measurement noise (TODO: make params)
573 if (OF_TWO_DIM) {
575 } else if (N_MEAS_OF_KF == 3) {
576 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
577 }
578 // Q-matrix, actuation noise (TODO: make params)
583 if (!CONSTANT_ALT_FILTER) {
585 } else if (OF_TWO_DIM) {
588 }
589 if (OF_THRUST_BIAS) {
591 }
592
593
594 // based on a fit, factor * rpm^2:
595 // TODO: with the parameters, we don't need this if / else any more.
596#if USE_NPS
597 // K = [0.152163; 0.170734; 0.103436; 0.122109] * 1E-7;
598 // K = [0.222949; 0.160458; 0.114227; 0.051396] * 1E-7;
599 // rpm:
600 // [2708.807954; 2587.641476; -379.728916; -501.203388]
601 RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7;
602 RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7;
603 RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7;
604 RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7;
605#else
606 // % Bebop 2, #45
607 // From fit_TM_2 script:
608 // K = [0.108068; 0.115448; 0.201207; 0.208834] * 1E-7
609 RPM_FACTORS[0] = parameters[PAR_K0] * 1E-7;
610 RPM_FACTORS[1] = parameters[PAR_K1] * 1E-7;
611 RPM_FACTORS[2] = parameters[PAR_K2] * 1E-7;
612 RPM_FACTORS[3] = parameters[PAR_K3] * 1E-7;
613#endif
614
615 reset_filter = false;
616 use_filter = 0;
617 run_filter = false;
618
621
622 // align the AHRS:
624 // set the initial attitude:
626
627#if PERIODIC_TELEMETRY
628 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_QUAT_INT, send_quat);
629 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER_INT, send_euler);
630 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_GYRO_BIAS_INT, send_bias);
632 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
634 //register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_Z, send_ins_z);
636#endif
637
638 /*
639 * Subscribe to scaled IMU measurements and attach callbacks
640 */
641// TODO: warning: passing argument 3 of ‘AbiBindMsgIMU_GYRO_INT’ from incompatible pointer type
642 AbiBindMsgIMU_GYRO(INS_FLOW_GYRO_ID, &gyro_ev, gyro_cb);
643 AbiBindMsgIMU_ACCEL(INS_FLOW_ACCEL_ID, &accel_ev, accel_cb);
644 AbiBindMsgGPS(INS_FLOW_GPS_ID, &gps_ev, gps_cb);
646 AbiBindMsgACT_FEEDBACK(INS_RPM_ID, &ins_act_feedback_ev, ins_act_feedback_cb);
647 AbiBindMsgIMU_LOWPASSED(INS_FLOW_IMU_ID, &aligner_ev, aligner_cb);
648 AbiBindMsgINS_RESET(ABI_BROADCAST, &reset_ev, reset_cb);
649
650 // Telemetry:
651 register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_INS_FLOW_INFO, send_ins_flow);
652
653 moment_ind = 0;
654
655}
656
657static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
658{
659 if (flag == INS_RESET_REF) {
660 struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
661 ltp_def_from_ecef_i(&ins_flow.ltp_def, &ecef_pos);
662 ins_flow.ltp_def.lla.alt = gps.lla_pos.alt;
663 ins_flow.ltp_def.hmsl = gps.hmsl;
664 stateSetLocalOrigin_i(MODULE_INS_FLOW_ID, &ins_flow.ltp_def);
665 ins_flow.ltp_initialized = true;
666 }
667}
668
670 int32_t flow_y UNUSED,
671 int32_t flow_der_x UNUSED, int32_t flow_der_y UNUSED, float quality UNUSED, float size_divergence)
672{
673
674 // TODO: make parameters:
675 float subpixel_factor = 10.0f;
676 float focal_x = 347.22;
677 float new_time = ((float)stamp) / 1e6;
678 float fps = 1.0f / (new_time - ins_flow.vision_time);
679 ins_flow.optical_flow_x = (((float)flow_x) * fps) / (subpixel_factor * focal_x);
680 ins_flow.optical_flow_y = (((float)flow_y) * fps) / (subpixel_factor * focal_x);
681 ins_flow.divergence = 1.27 * size_divergence * fps;
682 //printf("Reading %f, %f, %f\n", ins_flow.optical_flow_x, ins_flow.optical_flow_y, ins_flow.divergence);
683 ins_flow.vision_time = new_time;
684 ins_flow.new_flow_measurement = true;
685
686}
687
689{
691 if (!OF_TWO_DIM) {
692 printf("v = %f, angle = %f, angle_dot = %f, z = %f.\n",
694 } else {
695 printf("v = %f, angle = %f, angle_dot = %f, z = %f, vx = %f, theta = %f.\n",
697 }
698
699 } else {
700 if (!OF_THRUST_BIAS) {
701 printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
703 } else {
704 printf("v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f, thrust bias = %f.\n",
707 }
708
709 }
710
711}
712
714{
715 // TODO: rotate velocities to body frame:
716 // TODO: add also the theta axis:
717 struct FloatEulers *eulers = stateGetNedToBodyEulers_f();
718 struct NedCoor_f *position = stateGetPositionNed_f();
719 struct NedCoor_f *velocities = stateGetSpeedNed_f();
720 struct FloatRates *rates = stateGetBodyRates_f();
721
722 printf("True: v = %f, angle = %f, angle_dot = %f, z = %f, z_dot = %f.\n",
723 velocities->y, eulers->phi, rates->p, -position->z, -velocities->z);
724}
725
727{
728 float mass = parameters[PAR_MASS]; // 0.400;
729 float moment = 0.0f; // for now assumed to be 0
730 float Ix = parameters[PAR_IX]; // 0.0018244;
731 //float b = parameters[PAR_BASE];
732 float g = 9.81; // TODO: get a more accurate definition from pprz
733 float kd = parameters[PAR_KD]; // 0.5
734 float drag = 0.0f;
735
736 if (reset_filter) {
738 reset_filter = false;
739 }
740
741 // get ground truth data:
742 //struct FloatEulers* eulers = stateGetNedToBodyEulers_f();
743 //struct NedCoor_f* position = stateGetPositionNed_f();
744 //struct NedCoor_f *velocities = stateGetSpeedNed_f();
745 //struct FloatRates *rates = stateGetBodyRates_f();
746
747 // TODO: record when starting from the ground: does that screw up the filter? Yes it does : )
748
749
750 // assuming that the typical case is no rotation, we can estimate the (initial) bias of the gyro:
751 ins_flow.lp_gyro_bias_roll = lp_factor_strong * ins_flow.lp_gyro_bias_roll + (1 - lp_factor_strong) *
752 ins_flow.lp_gyro_roll;
753 if (OF_TWO_DIM) {
754 ins_flow.lp_gyro_bias_pitch = lp_factor_strong * ins_flow.lp_gyro_bias_pitch + (1 - lp_factor_strong) *
755 ins_flow.lp_gyro_pitch;
756 }
757 // same assumption for the roll command: assuming a close-to-hover situation and roll trim for staying in place:
758 ins_flow.lp_roll_command = lp_factor_strong * ins_flow.lp_roll_command + (1 - lp_factor_strong) *
760
761 // only start estimation when flying (and above 1 meter: || position->z > -1.0f )
762 // 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.
763 if (!autopilot_in_flight()) {
764 return;
765 }
766
767 if (!run_filter) {
768
769 // TODO: should we do this if we have a thrust bias?
770
771 // Drone is flying but not yet running the filter, in order to obtain unbiased thrust estimates we estimate an additional factor.
772 // Assumption is that the drone is hovering, which means that over a longer period of time, the thrust should equal gravity.
773 // If the low pass thrust is lower than the one expected by gravity, then it needs to be increased and viceversa.
774 float thrust = 0.0f;
775 for (int i = 0; i < OF_N_ROTORS; i++) {
776 thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
777 }
778 if (ins_flow.lp_thrust < 1E-3) {
779 // first time directly initialize with thrust to get quicker convergence:
780 ins_flow.lp_thrust = thrust;
781 } else {
782 ins_flow.lp_thrust = lp_factor_strong * ins_flow.lp_thrust + (1 - lp_factor_strong) * thrust;
783 }
784 float actual_lp_thrust = mass * g;
785 ins_flow.thrust_factor = actual_lp_thrust / ins_flow.lp_thrust;
786 thrust_factor = ins_flow.thrust_factor;
787 //printf("Low pass predicted thrust = %f. Expected thrust = %f. Thrust factor = %f.\n", ins_flow.lp_thrust, actual_lp_thrust, ins_flow.thrust_factor);
788 // don't run the filter just yet:
789 return;
790 }
791
793
794 // in the sim, the gyro bias wanders so fast, that this does not seem to be useful:
795 // TODO: verify how this is in reality, and if not useful, remove all code to estimate this bias (or do it differently)
796 // ins_flow.lp_gyro_bias_roll = 0.0f;
797
798 // 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.
799 // However, the updates of the filter themselves should be slower:
800 /*counter++;
801 if(counter < 5) {
802 return;
803 }
804 else {
805 counter = 0;
806 }*/
807
808 // get the new time:
810 float dt = of_time - of_prev_time;
811 //printf("dt = %f.\n", dt);
812 if (dt > 1.0f) {
813 dt = 0.01f;
814 }
815
816 // predict the thrust and moment:
817 float thrust = 0.0f;
818 for (int i = 0; i < OF_N_ROTORS; i++) {
819 thrust += RPM_FACTORS[i] * ins_flow.RPM[i] * ins_flow.RPM[i];
820 }
821 thrust *= thrust_factor; // ins_flow.thrust_factor;
823 thrust -= OF_X[OF_THRUST_BIAS_IND];
824 }
825 DEBUG_PRINT("Thrust acceleration = %f, g = %f\n", thrust / mass, g);
826
827 // TODO: do we have an optimization that used the moment?
828 /*moment = b * RPM_FACTORS[0] * ins_flow.RPM[0]*ins_flow.RPM[0] -
829 b * RPM_FACTORS[1] * ins_flow.RPM[1]*ins_flow.RPM[1] -
830 b * RPM_FACTORS[2] * ins_flow.RPM[2]*ins_flow.RPM[2] +
831 b * RPM_FACTORS[3] * ins_flow.RPM[3]*ins_flow.RPM[3];*/
832 // M_est = Ix * (-0.000553060716181365 * cmd_roll(k) -3.23315441805895 * Xe(3, k));
833#if USE_NPS
834 // TODO: moment in simulation is very easy to estimate with the roll command, so add that:
835 moment = 0;
836#else
837
838 /*
839 moments[moment_ind] = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
840
841 int select_ind = moment_ind - MOMENT_DELAY;
842 if(select_ind < 0) {
843 select_ind += MOMENT_DELAY;
844 }
845
846 // current moment is a delayed version:
847 moment = moments[select_ind];
848
849 // update the moment's ind:
850 moment_ind++;
851 if(moment_ind >= MOMENT_DELAY) {
852 moment_ind = 0;
853 }
854 */
855 // moment = Ix *(-0.000553060716181365 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command) -3.23315441805895 * OF_X[OF_ANGLE_DOT_IND]);
856 moment = 0;
857#endif
858
859 // 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);
860
861
862 // propagate the state with Euler integration:
863 DEBUG_PRINT("Before prediction: ");
866 OF_X[OF_V_IND] += dt * (g * tan(OF_X[OF_ANGLE_IND]));
867 if (OF_DRAG) {
868 // quadratic drag acceleration:
869 drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass;
870 // apply it in the right direction:
871 if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; }
872 else { OF_X[OF_V_IND] += drag; }
873 }
874
875 /* // if we use gyros here, the angle dot estimate is ignored:
876 * if(OF_USE_GYROS) {
877 // 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...
878 } */
879
880 // temporary insertion of gyro estimate here, for quicker effect:
881 // OF_X[OF_ANGLE_IND] += dt * -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
882
884 OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix);
885
886 if (OF_TWO_DIM) {
887 // Second axis, decoupled formulation:
888 OF_X[OF_VX_IND] += dt * (g * tan(OF_X[OF_THETA_IND]));
889 if (OF_DRAG) {
890 // quadratic drag acceleration:
891 drag = dt * kd * (OF_X[OF_VX_IND] * OF_X[OF_VX_IND]) / mass;
892 // apply it in the right direction:
893 if (OF_X[OF_VX_IND] > 0) { OF_X[OF_VX_IND] -= drag; }
894 else { OF_X[OF_VX_IND] += drag; }
895 }
896 // TODO: here also a moment estimate?
897 // TODO: add a THETA_DOT_IND
898 OF_X[OF_THETA_IND] += dt * (ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch) *
899 (M_PI / 180.0f) / 74.0f; // Code says scaled by 12, but... that does not fit...
900 }
901
902 DEBUG_PRINT("Rate p = %f, gyro p = %f\n", rates->p,
903 (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f);
904 } else {
905 // make sure that the right hand state terms appear before they change:
906 OF_X[OF_V_IND] += dt * (thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
907 OF_X[OF_Z_IND] += dt * OF_X[OF_Z_DOT_IND];
908 OF_X[OF_Z_DOT_IND] += dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass - g);
910 /*
911 * // 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.
912 else {
913 OF_X[OF_ANGLE_IND] += dt * (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI/180.0f) / 74.0f;
914 }*/
915 OF_X[OF_ANGLE_DOT_IND] += dt * (moment / Ix);
916
917 // thrust bias does not change over time according to our model
918
919 if (OF_DRAG) {
920 // quadratic drag acceleration:
921 drag = dt * kd * (OF_X[OF_V_IND] * OF_X[OF_V_IND]) / mass;
922 // apply it in the right direction:
923 if (OF_X[OF_V_IND] > 0) { OF_X[OF_V_IND] -= drag; }
924 else { OF_X[OF_V_IND] += drag; }
925 }
926 }
927
928 // ensure that z is not 0 (or lower)
929 if (OF_X[OF_Z_IND] < 1e-2) {
930 OF_X[OF_Z_IND] = 1e-2;
931 }
932
933 DEBUG_PRINT("After prediction: ");
935
936 // prepare the update and correction step:
937 // we have to recompute these all the time, as they depend on the state:
938 // discrete version of state transition matrix F: (ignoring t^2)
939 float F[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}};
940 for (int i = 0; i < N_STATES_OF_KF; i++) {
941 F[i][i] = 1.0f;
942 }
943
945 F[OF_V_IND][OF_ANGLE_IND] = dt * (g / (cosf(OF_X[OF_ANGLE_IND]) * cosf(OF_X[OF_ANGLE_IND])));
946 F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
947 if (OF_TWO_DIM) {
948 F[OF_VX_IND][OF_THETA_IND] = dt * (g / (cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND])));
949 }
950 } else {
951 F[OF_V_IND][OF_ANGLE_IND] = dt * (thrust * cosf(OF_X[OF_ANGLE_IND]) / mass);
952 F[OF_ANGLE_IND][OF_ANGLE_DOT_IND] = dt * 1.0f;
953 F[OF_Z_IND][OF_Z_DOT_IND] = dt * 1.0f;
954 F[OF_Z_DOT_IND][OF_ANGLE_IND] = dt * (-thrust * sinf(OF_X[OF_ANGLE_IND]) / mass);
955 if (OF_THRUST_BIAS) {
956 F[OF_V_IND][OF_THRUST_BIAS_IND] = -dt * sinf(OF_X[OF_ANGLE_IND]) / mass;
957 F[OF_Z_DOT_IND][OF_THRUST_BIAS_IND] = -dt * cosf(OF_X[OF_ANGLE_IND]) / mass;
958 }
959 }
960
961 if (OF_DRAG) {
962 // In MATLAB: -sign(v)*2*kd*v/m (always minus, whether v is positive or negative):
963 F[OF_V_IND][OF_V_IND] -= dt * 2 * kd * fabs(OF_X[OF_V_IND]) / mass;
964 if (OF_TWO_DIM) {
965 F[OF_VX_IND][OF_VX_IND] -= dt * 2 * kd * fabs(OF_X[OF_VX_IND]) / mass;
966 }
967 }
968
969 // G matrix (whatever it may be):
970 float G[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}};
971 // TODO: we miss an off-diagonal element here (compare with MATLAB)
972 for (int i = 0; i < N_STATES_OF_KF; i++) {
973 G[i][i] = dt;
974 }
975
976 // Jacobian observation matrix H:
977 float H[N_MEAS_OF_KF][N_STATES_OF_KF] = {{0.}};
978
980 // Hx = [-cosf(theta)^2/z, (v*sinf(theta))/ z, (v* cosf(theta)^2)/z^2];
981 // lateral flow:
987 // divergence:
988 H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
992
993 if (OF_TWO_DIM) {
994 // divergence measurement couples the two axes actually...:
995 H[OF_DIV_FLOW_IND][OF_VX_IND] = -sinf(2 * OF_X[OF_THETA_IND]) / (2 * OF_X[OF_Z_IND]);
997
998 // lateral flow in x direction:
1002 (OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
1003 }
1004 } else {
1005 // lateral flow:
1008 + OF_X[OF_Z_DOT_IND] * cosf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
1012 - OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND] * OF_X[OF_Z_IND]);
1014 // divergence:
1015 H[OF_DIV_FLOW_IND][OF_V_IND] = -sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
1017 + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / OF_X[OF_Z_IND];
1022 }
1023
1024 // rate measurement:
1025 if (OF_USE_GYROS) {
1026 H[OF_RATE_IND][OF_V_IND] = 0.0f;
1027 H[OF_RATE_IND][OF_ANGLE_IND] = 0.0f;
1029 H[OF_RATE_IND][OF_Z_IND] = 0.0f;
1030 H[OF_RATE_IND][OF_Z_DOT_IND] = 0.0f;
1031 }
1032
1033 // propagate uncertainty:
1034 // TODO: make pointers that don't change to init:
1041
1042 DEBUG_PRINT("Phi:\n");
1044
1045 DEBUG_PRINT("P:\n");
1047
1048 DEBUG_PRINT("Gamma:\n");
1050
1051 DEBUG_PRINT("Q:\n");
1053
1054 DEBUG_PRINT("R:\n");
1056
1057 DEBUG_PRINT("Jacobian:\n");
1059
1060 // Corresponding MATLAB statement: :O
1061 // P_k1_k = Phi_k1_k*P*Phi_k1_k' + Gamma_k1_k*Q*Gamma_k1_k';
1062 float _PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1063 MAKE_MATRIX_PTR(PhiT, _PhiT, N_STATES_OF_KF);
1064 float _P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1065 MAKE_MATRIX_PTR(PPhiT, _P_PhiT, N_STATES_OF_KF);
1066 float _Phi_P_PhiT[N_STATES_OF_KF][N_STATES_OF_KF];
1067 MAKE_MATRIX_PTR(PhiPPhiT, _Phi_P_PhiT, N_STATES_OF_KF);
1068
1072
1073 DEBUG_PRINT("Phi*P*PhiT:\n");
1075
1076 float _GT[N_STATES_OF_KF][N_STATES_OF_KF];
1078 float _Q_GT[N_STATES_OF_KF][N_STATES_OF_KF];
1079 MAKE_MATRIX_PTR(QGT, _Q_GT, N_STATES_OF_KF);
1080 float _G_Q_GT[N_STATES_OF_KF][N_STATES_OF_KF];
1081 MAKE_MATRIX_PTR(GQGT, _G_Q_GT, N_STATES_OF_KF);
1082
1086
1087 DEBUG_PRINT("Gamma*Q*GammaT:\n");
1089
1090 float_mat_sum(P, PhiPPhiT, GQGT, N_STATES_OF_KF, N_STATES_OF_KF);
1091 DEBUG_PRINT("P:\n");
1093
1094 // correct state when there is a new vision measurement:
1095 if (ins_flow.new_flow_measurement) {
1096
1097 DEBUG_PRINT("*********************\n");
1098 DEBUG_PRINT(" NEW MEASUREMENT \n");
1099 DEBUG_PRINT("*********************\n");
1100
1101 // determine Kalman gain:
1102 // MATLAB statement:
1103 // S_k = Hx*P_k1_k*Hx' + R;
1104 float _JacT[N_STATES_OF_KF][N_MEAS_OF_KF];
1105 MAKE_MATRIX_PTR(JacT, _JacT, N_STATES_OF_KF);
1106 float _P_JacT[N_STATES_OF_KF][N_MEAS_OF_KF];
1107 MAKE_MATRIX_PTR(PJacT, _P_JacT, N_STATES_OF_KF);
1108 float _Jac_P_JacT[N_MEAS_OF_KF][N_MEAS_OF_KF];
1109 MAKE_MATRIX_PTR(JacPJacT, _Jac_P_JacT, N_MEAS_OF_KF);
1110
1113 DEBUG_PRINT("P*JacT:\n");
1115
1116 float_mat_mul(JacPJacT, Jac, PJacT, N_MEAS_OF_KF, N_STATES_OF_KF, N_MEAS_OF_KF);
1117
1118 DEBUG_PRINT("Jac*P*JacT:\n");
1120
1121 float _S[N_MEAS_OF_KF][N_MEAS_OF_KF];
1123 float_mat_sum(S, JacPJacT, R, N_MEAS_OF_KF, N_MEAS_OF_KF);
1124
1125 DEBUG_PRINT("S:\n");
1127
1128 // MATLAB statement:
1129 // K_k1 = P_k1_k*Hx' * inv(S_k);
1130 float _K[N_STATES_OF_KF][N_MEAS_OF_KF];
1132 float _INVS[N_MEAS_OF_KF][N_MEAS_OF_KF];
1133 MAKE_MATRIX_PTR(INVS, _INVS, N_MEAS_OF_KF);
1135 if (DEBUG_INS_FLOW) {
1136 // This should be the identity matrix:
1137 float _SINVS[N_MEAS_OF_KF][N_MEAS_OF_KF];
1138 MAKE_MATRIX_PTR(SINVS, _SINVS, N_MEAS_OF_KF);
1140 DEBUG_PRINT("S*Inv(S):\n");
1142 }
1143
1145 DEBUG_PRINT("K:\n");
1147
1148 // Correct the state:
1149 // MATLAB:
1150 // Z_expected = [-v*cosf(theta)*cosf(theta)/z + zd*sinf(2*theta)/(2*z) + thetad;
1151 // (-v*sinf(2*theta)/(2*z)) - zd*cosf(theta)*cosf(theta)/z];
1152
1153 float Z_expected[N_MEAS_OF_KF];
1154
1155 // TODO: take this var out? It was meant for debugging...
1156 //float Z_expect_GT_angle;
1157
1158 if (CONSTANT_ALT_FILTER) {
1159 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]
1160 + OF_X[OF_ANGLE_DOT_IND]; // TODO: Currently, no p works better than using p here. Analyze!
1161
1162
1163 /* TODO: remove later, just for debugging:
1164 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];
1165 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];
1166 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,
1167 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);
1168 if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) {
1169 printf("NO RATE WINS!");
1170 }
1171 printf("\n");*/
1172
1173 Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND]);
1174
1175 if (OF_TWO_DIM) {
1176 Z_expected[OF_LAT_FLOW_X_IND] = -OF_X[OF_VX_IND] * cosf(OF_X[OF_THETA_IND]) * cosf(OF_X[OF_THETA_IND]) /
1177 OF_X[OF_Z_IND]; // TODO: no q?
1178 }
1179
1180 //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND];
1181
1182 if (OF_USE_GYROS) {
1183 Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
1184 }
1185 } else {
1186 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]
1187 + OF_X[OF_Z_DOT_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
1188 + OF_X[OF_ANGLE_DOT_IND]; // TODO: We first had this rate term but not for the constant alt filter.
1189 // Simulation and data analysis from real flights shows that including it is better. CHECK IN REALITY!
1190
1191 Z_expected[OF_DIV_FLOW_IND] = -OF_X[OF_V_IND] * sinf(2 * OF_X[OF_ANGLE_IND]) / (2 * OF_X[OF_Z_IND])
1193
1194 //Z_expect_GT_angle = -OF_X[OF_V_IND]*cosf(eulers->phi)*cosf(eulers->phi)/OF_X[OF_Z_IND]
1195 // + OF_X[OF_Z_DOT_IND]*sinf(2*eulers->phi)/(2*OF_X[OF_Z_IND]);
1196 //+ OF_X[OF_ANGLE_DOT_IND];
1197 if (N_MEAS_OF_KF == 3) {
1198 Z_expected[OF_RATE_IND] = OF_X[OF_ANGLE_DOT_IND]; // TODO: is this even in the right direction?
1199 }
1200
1201 /*
1202 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]
1203 + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND]);
1204 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]
1205 + OF_X[OF_Z_DOT_IND]*sinf(2*OF_X[OF_ANGLE_IND])/(2*OF_X[OF_Z_IND])
1206 + OF_X[OF_ANGLE_DOT_IND];
1207 */
1208 /*
1209 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,
1210 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);
1211 if(fabs(ins_flow.optical_flow_x - Z_exp_no_rate) < fabs(ins_flow.optical_flow_x - Z_exp_with_rate)) {
1212 printf("NO RATE WINS!");
1213 }
1214 printf("\n");*/
1215 }
1216
1217 // i_k1 = Z - Z_expected;
1218 float innovation[N_MEAS_OF_KF][1];
1219 //print_ins_flow_state();
1220 innovation[OF_LAT_FLOW_IND][0] = ins_flow.optical_flow_x - Z_expected[OF_LAT_FLOW_IND];
1221 DEBUG_PRINT("Expected flow filter: %f, Expected flow ground truth = %f, Real flow x: %f, Real flow y: %f.\n",
1222 Z_expected[OF_LAT_FLOW_IND], Z_expect_GT_angle, ins_flow.optical_flow_x, ins_flow.optical_flow_y);
1223 innovation[OF_DIV_FLOW_IND][0] = ins_flow.divergence - Z_expected[OF_DIV_FLOW_IND];
1224 DEBUG_PRINT("Expected div: %f, Real div: %f.\n", Z_expected[OF_DIV_FLOW_IND], ins_flow.divergence);
1226 innovation[OF_LAT_FLOW_X_IND][0] = ins_flow.optical_flow_y - Z_expected[OF_LAT_FLOW_X_IND];
1227 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",
1228 Z_expected[OF_LAT_FLOW_X_IND], ins_flow.optical_flow_y, ins_flow.lp_gyro_pitch - ins_flow.lp_gyro_bias_pitch,
1229 OF_X[OF_VX_IND], velocities->x, OF_X[OF_THETA_IND], eulers->theta);
1230 }
1231 if (OF_USE_GYROS) {
1232 float gyro_meas_roll;
1233 if (!PREDICT_GYROS) {
1234 gyro_meas_roll = (ins_flow.lp_gyro_roll - ins_flow.lp_gyro_bias_roll) * (M_PI / 180.0f) / 74.0f;
1235 } else {
1236 // TODO: You can fake gyros here by estimating them as follows:
1237 // rate_p_filt_est = -1.8457e-04 * cmd_roll;
1238 // gyro_meas_roll = -1.8457e-04 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1239 // gyro_meas_roll = -2.0e-03 * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1240
1241 // gyro_meas_roll = 1e-04 * parameters[PAR_PRED_ROLL_1] * (stabilization.cmd[COMMAND_ROLL]-ins_flow.lp_roll_command);
1242 // gyro_meas_roll = parameters[PAR_PRED_ROLL_2] * gyro_meas_roll + 1E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x;
1243
1244 // only flow:
1245 gyro_meas_roll = 2E-3 * parameters[PAR_PRED_ROLL_3] * ins_flow.optical_flow_x;
1246
1247 //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);
1248 }
1249
1250 innovation[OF_RATE_IND][0] = gyro_meas_roll - Z_expected[OF_RATE_IND];
1251 //innovation[OF_RATE_IND][0] = rates->p - Z_expected[OF_RATE_IND];
1252 DEBUG_PRINT("Expected rate: %f, Real rate: %f.\n", Z_expected[OF_RATE_IND], ins_flow.lp_gyro_roll);
1253 }
1254
1255 MAKE_MATRIX_PTR(I, innovation, N_MEAS_OF_KF);
1256 DEBUG_PRINT("Innovation:");
1258
1259 // X_k1_k1 = X_k1_k + K_k1*(i_k1);
1260 float _KI[N_STATES_OF_KF][1];
1263
1264 DEBUG_PRINT("K*innovation:\n");
1266
1267 DEBUG_PRINT("PRE: v = %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]);
1268 for (int i = 0; i < N_STATES_OF_KF; i++) {
1269 OF_X[i] += KI[i][0];
1270 }
1271 DEBUG_PRINT("POST v: %f, angle = %f\n", OF_X[OF_V_IND], OF_X[OF_ANGLE_IND]);
1272
1273 DEBUG_PRINT("Angles (deg): ahrs = %f, ekf = %f.\n", (180.0f / M_PI)*eulers->phi, (180.0f / M_PI)*OF_X[OF_ANGLE_IND]);
1274
1275 DEBUG_PRINT("P before correction:\n");
1277
1278 // 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
1279 float _KJac[N_STATES_OF_KF][N_STATES_OF_KF];
1280 MAKE_MATRIX_PTR(KJac, _KJac, N_STATES_OF_KF);
1282
1283 float _eye[N_STATES_OF_KF][N_STATES_OF_KF];
1284 MAKE_MATRIX_PTR(eye, _eye, N_STATES_OF_KF);
1286 DEBUG_PRINT("eye:\n");
1288
1289 float _eKJac[N_STATES_OF_KF][N_STATES_OF_KF];
1290 MAKE_MATRIX_PTR(eKJac, _eKJac, N_STATES_OF_KF);
1291 float_mat_diff(eKJac, eye, KJac, N_STATES_OF_KF, N_STATES_OF_KF);
1292 DEBUG_PRINT("eKJac:\n");
1294
1295 float _eKJacT[N_STATES_OF_KF][N_STATES_OF_KF];
1296 MAKE_MATRIX_PTR(eKJacT, _eKJacT, N_STATES_OF_KF);
1298 // (eye(Nx) - K_k1*Hx)*P_k1_k*(eye(Nx) - K_k1*Hx)'
1299 float _P_pre[N_STATES_OF_KF][N_STATES_OF_KF];
1300 MAKE_MATRIX_PTR(P_pre, _P_pre, N_STATES_OF_KF);
1303 DEBUG_PRINT("eKJac * P *eKJacT:\n");
1305
1306 // K_k1*R*K_k1'
1307 // TODO: check all MAKE_MATRIX that they mention the number of ROWS!
1308 float _KT[N_MEAS_OF_KF][N_STATES_OF_KF];
1309 MAKE_MATRIX_PTR(KT, _KT, N_MEAS_OF_KF);
1311 float _RKT[N_MEAS_OF_KF][N_STATES_OF_KF];
1312 MAKE_MATRIX_PTR(RKT, _RKT, N_MEAS_OF_KF);
1314 float _KRKT[N_STATES_OF_KF][N_STATES_OF_KF];
1315 MAKE_MATRIX_PTR(KRKT, _KRKT, N_STATES_OF_KF);
1317 DEBUG_PRINT("KRKT:\n");
1319
1320 // summing the two parts:
1322
1323 DEBUG_PRINT("P corrected:\n");
1325 float trace_P = 0.0f;
1326 for (int i = 0; i < N_STATES_OF_KF; i++) {
1327 trace_P += P[i][i];
1328 }
1329 DEBUG_PRINT("trace P = %f\n", trace_P);
1330
1331 // indicate that the measurement has been used:
1332 ins_flow.new_flow_measurement = false;
1333 }
1334
1335 // update the time:
1337
1338}
1339
1340static void gyro_cb(uint8_t __attribute__((unused)) sender_id,
1341 uint32_t stamp, struct Int32Rates *gyro)
1342{
1343 ahrs_icq_last_stamp = stamp;
1344#if USE_AUTO_AHRS_FREQ || !defined(AHRS_PROPAGATE_FREQUENCY)
1345 PRINT_CONFIG_MSG("Calculating dt for AHRS_ICQ propagation.")
1346 /* timestamp in usec when last callback was received */
1347 static uint32_t last_stamp = 0;
1348
1349 if (last_stamp > 0 && ahrs_icq.is_aligned) {
1350 float dt = (float)(stamp - last_stamp) * 1e-6;
1351 ahrs_icq_propagate(gyro, dt);
1353
1354 // TODO: filter all gyro values
1355 // For now only filter the roll gyro:
1356 float current_rate = ((float)gyro->p); // TODO: is this correct? / INT32_RATE_FRAC;
1357 ins_flow.lp_gyro_roll = lp_factor * ins_flow.lp_gyro_roll + (1 - lp_factor) * current_rate;
1358 current_rate = ((float)gyro->q);
1359 ins_flow.lp_gyro_pitch = lp_factor * ins_flow.lp_gyro_pitch + (1 - lp_factor) * current_rate;
1360 }
1361 last_stamp = stamp;
1362#else
1363 PRINT_CONFIG_MSG("Using fixed AHRS_PROPAGATE_FREQUENCY for AHRS_ICQ propagation.")
1365 if (ahrs_icq.status == AHRS_ICQ_RUNNING) {
1366 const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
1367 ahrs_icq_propagate(gyro, dt);
1369 }
1370#endif
1371}
1372
1373static void accel_cb(uint8_t __attribute__((unused)) sender_id,
1374 uint32_t __attribute__((unused)) stamp,
1375 struct Int32Vect3 *accel)
1376{
1377#if USE_AUTO_AHRS_FREQ || !defined(AHRS_CORRECT_FREQUENCY)
1378 PRINT_CONFIG_MSG("Calculating dt for AHRS int_cmpl_quat accel update.")
1379 static uint32_t last_stamp = 0;
1380 if (last_stamp > 0 && ahrs_icq.is_aligned) {
1381 float dt = (float)(stamp - last_stamp) * 1e-6;
1382 ahrs_icq_update_accel(accel, dt);
1384 }
1385 last_stamp = stamp;
1386#else
1387 PRINT_CONFIG_MSG("Using fixed AHRS_CORRECT_FREQUENCY for AHRS int_cmpl_quat accel update.")
1388 PRINT_CONFIG_VAR(AHRS_CORRECT_FREQUENCY)
1389 if (ahrs_icq.is_aligned) {
1390 const float dt = 1. / (AHRS_CORRECT_FREQUENCY);
1391 ahrs_icq_update_accel(accel, dt);
1393 }
1394#endif
1395}
1396
1397
1400{
1401 /* Compute LTP to BODY quaternion */
1402 struct Int32Quat ltp_to_body_quat = ahrs_icq.ltp_to_body_quat;
1403 //struct Int32Quat *body_to_imu_quat = orientationGetQuat_i(&ahrs_icq.body_to_imu);
1404 //int32_quat_comp_inv(&ltp_to_body_quat, &ahrs_icq.ltp_to_imu_quat, body_to_imu_quat);
1405
1406 if (use_filter < USE_ANGLE) {
1407 // Use the orientation as is:
1408 stateSetNedToBodyQuat_i(MODULE_INS_FLOW_ID, &ltp_to_body_quat);
1409 } else {
1410
1411 // get Euler angles:
1412 struct OrientationReps orient;
1413 orient.status = 1 << ORREP_QUAT_I;
1414 orient.quat_i = ltp_to_body_quat;
1415 struct FloatEulers *eulers = orientationGetEulers_f(&orient);
1416
1417 // set roll angle with value from the filter:
1418 GT_phi = eulers->phi;
1419 eulers->phi = OF_X[OF_ANGLE_IND];
1420 if (OF_TWO_DIM) {
1421 GT_theta = eulers->theta;
1422 //printf("Real theta = %f, setting theta to %f.\n", eulers->theta, OF_X[OF_THETA_IND]);
1423 eulers->theta = OF_X[OF_THETA_IND];
1424 }
1425
1426 // Transform the Euler representation to Int32Quat and set the state:
1427 struct OrientationReps orient_euler;
1428 orient_euler.status = 1 << ORREP_EULER_F;
1429 orient_euler.eulers_f = (*eulers);
1430
1431 struct Int32Quat *quat_i_adapted = orientationGetQuat_i(&orient_euler);
1432 stateSetNedToBodyQuat_i(MODULE_INS_FLOW_ID, quat_i_adapted);
1433 }
1434
1435 /* compute body rates */
1436 struct Int32Rates body_rate = ahrs_icq.body_rate;
1437 // struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu);
1438 // int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate);
1439 /* Set state */
1440 stateSetBodyRates_i(MODULE_INS_FLOW_ID, &body_rate);
1441
1442}
1443
1444static void ins_act_feedback_cb(uint8_t sender_id UNUSED, struct act_feedback_t *feedback, uint8_t num_act)
1445{
1446 ins_flow.RPM_num_act = num_act;
1447 for (int i = 0; i < num_act; i++) {
1448 if (feedback[i].set.rpm) {
1449 ins_flow.RPM[i] = feedback[i].rpm;
1450 }
1451 }
1452}
1453
1454
1455/* Update INS based on GPS information */
1456static void gps_cb(uint8_t sender_id __attribute__((unused)),
1457 uint32_t stamp UNUSED,
1458 struct GpsState *gps_s)
1459{
1460
1461 if (gps_s->fix < GPS_FIX_3D) {
1462 return;
1463 }
1464 if (!ins_flow.ltp_initialized) {
1466 }
1467
1468 ahrs_icq_update_gps(gps_s);
1469
1470 /* simply scale and copy pos/speed from gps */
1471 struct NedCoor_i gps_pos_cm_ned;
1472 struct EcefCoor_i gps_ecef_pos = ecef_int_from_gps(gps_s);
1473 ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_flow.ltp_def, &gps_ecef_pos);
1474 INT32_VECT3_SCALE_2(ins_flow.ltp_pos, gps_pos_cm_ned,
1476
1477 if (use_filter >= USE_HEIGHT) {
1478 //struct NedCoor_f* position = stateGetPositionNed_f();
1479 int32_t z_Ned_i_filter = - (int32_t)((OF_X[OF_Z_IND] * INT32_POS_OF_CM_NUM * 100) / INT32_POS_OF_CM_DEN);
1480 //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);
1481 ins_flow.ltp_pos.z = z_Ned_i_filter;
1482 }
1483
1484 stateSetPositionNed_i(MODULE_INS_FLOW_ID, &ins_flow.ltp_pos);
1485
1486
1487
1488 struct NedCoor_i gps_speed_cm_s_ned;
1489 struct EcefCoor_i gps_ecef_vel = ecef_vel_int_from_gps(gps_s);
1490 ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_flow.ltp_def, &gps_ecef_vel);
1491 INT32_VECT3_SCALE_2(ins_flow.ltp_speed, gps_speed_cm_s_ned,
1493
1494 if (use_filter >= USE_VELOCITY) {
1495 // get NED to body rotation matrix:
1496 struct FloatRMat *NTB = stateGetNedToBodyRMat_f();
1497 // get transpose (inverse):
1498 struct FloatRMat BTN;
1499 float_rmat_inv(&BTN, NTB);
1500
1501 // the velocities from the filter are rotated from the body to the inertial frame:
1502 struct FloatVect3 NED_velocities, body_velocities;
1503 body_velocities.x = 0.0f; // filter does not determine this yet
1504 body_velocities.y = OF_X[OF_V_IND];
1505 body_velocities.z = 0.0f;
1506 /*
1507 if(CONSTANT_ALT_FILTER) {
1508 body_velocities.z = 0.0f;
1509 }
1510 else {
1511 body_velocities.z = OF_X[OF_Z_DOT_IND];
1512 }*/
1513 float_rmat_vmult(&NED_velocities, &BTN, &body_velocities);
1514 // TODO: also estimate vx, so that we can just use the rotated vector:
1515 // For now, we need to keep the x, and y body axes aligned with the global ones.
1516 // printf("Original speed y = %d, ", ins_flow.ltp_speed.y);
1517 ins_flow.ltp_speed.y = (int32_t)((NED_velocities.y * INT32_SPEED_OF_CM_S_NUM * 100) / INT32_SPEED_OF_CM_S_DEN);
1518 if (!CONSTANT_ALT_FILTER) { ins_flow.ltp_speed.z = -(int32_t)((NED_velocities.z * INT32_SPEED_OF_CM_S_NUM * 100) / INT32_SPEED_OF_CM_S_DEN); }
1519 // printf("Changed speed y = %d (%f in float)\n", ins_flow.ltp_speed.y, NED_velocities.y);
1520 }
1521
1522 stateSetSpeedNed_i(MODULE_INS_FLOW_ID, &ins_flow.ltp_speed);
1523
1524 /*
1525 bool vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT);
1526 uint8_t nsats = gps_s->num_sv;
1527 */
1528}
1529
1530static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
1531 uint32_t stamp __attribute__((unused)),
1532 struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
1533 struct Int32Vect3 *lp_mag)
1534{
1535 if (!ahrs_icq.is_aligned) {
1536 if (ahrs_icq_align(lp_gyro, lp_accel, lp_mag)) {
1538 }
1539 }
1540}
1541
1542
Main include for ABI (AirBorneInterface).
struct abi_struct abi_event
Definition abi_common.h:101
#define ABI_BROADCAST
Broadcast address.
Definition abi_common.h:80
#define AHRS_COMP_ID_FLOW
Definition ahrs.h:49
void ahrs_aligner_init(void)
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static abi_event gyro_ev
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
static abi_event aligner_ev
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void send_euler(struct transport_tx *trans, struct link_device *dev)
static void send_bias(struct transport_tx *trans, struct link_device *dev)
static abi_event gps_ev
static void set_body_state_from_quat(void)
Compute body orientation and rates from imu orientation and rates.
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)
@ AHRS_ICQ_RUNNING
static void send_quat(struct transport_tx *trans, struct link_device *dev)
static uint32_t ahrs_icq_last_stamp
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
Core autopilot interface common to all firmwares.
#define UNUSED(x)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
#define COMMAND_ROLL
static abi_event accel_ev
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct GpsState gps
global GPS state
Definition gps.c:74
struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (integer) Converted on the fly if not available.
Definition gps.c:536
struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s)
Get GPS ecef pos (integer) Converted on the fly if not available.
Definition gps.c:507
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
uint8_t fix
status of fix
Definition gps.h:110
data structure for GPS information
Definition gps.h:90
float q
in rad/s
float phi
in radians
float p
in rad/s
float theta
in radians
void float_mat_invert(float **o, float **mat, const 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_transpose(float **o, float **a, const int n, const int m)
transpose non-square matrix
static void float_mat_mul(float **o, float **a, float **b, const int m, const int n, const int l)
o = a * b
static void float_mat_diagonal_scal(float **o, float v, const int n)
Make an n x n identity matrix (for matrix passed as array).
void float_rmat_vmult(struct FloatVect3 *vb, const struct FloatRMat *m_a2b, const struct FloatVect3 *va)
rotate 3D vector by rotation matrix.
static void float_mat_sum(float **o, float **a, float **b, const int m, const int n)
o = a + b
#define MAKE_MATRIX_PTR(_ptr, _mat, _rows)
Make a pointer to a matrix of _rows lines.
void float_rmat_inv(struct FloatRMat *m_b2a, const struct FloatRMat *m_a2b)
Inverse/transpose of a rotation matrix.
static void float_mat_diff(float **o, float **a, float **b, const int m, const int n)
o = a - b
euler angles
rotation matrix
angular rates
int32_t p
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 alt
in millimeters above WGS84 reference ellipsoid
int32_t lon
in degrees*1e7
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:1295
static struct FloatRMat * stateGetNedToBodyRMat_f(void)
Get vehicle body attitude rotation matrix (float).
Definition state.h:1319
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition state.h:1307
static void stateSetNedToBodyQuat_i(uint16_t id, struct Int32Quat *ned_to_body_quat)
Set vehicle body attitude from quaternion (int).
Definition state.h:1251
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static void stateSetLocalOrigin_i(uint16_t id, struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition state.h:530
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static void stateSetPositionNed_i(uint16_t id, struct NedCoor_i *ned_pos)
Set position from local NED coordinates (int).
Definition state.h:649
static void stateSetBodyRates_i(uint16_t id, struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition state.h:1361
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
static void stateSetSpeedNed_i(uint16_t id, struct NedCoor_i *ned_speed)
Set ground speed in local NED coordinates (int).
Definition state.h:903
#define AHRS_PROPAGATE_FREQUENCY
Definition hf_float.c:55
#define INS_RESET_REF
flags for INS reset
Definition ins.h:36
static void reset_cb(uint8_t sender_id, uint8_t flag)
static abi_event reset_ev
static float p[2][2]
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:563
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_ekf2.cpp:608
#define INS_RPM_ID
Definition ins_flow.c:99
float lp_factor
Definition ins_flow.c:182
#define DEBUG_INS_FLOW
Definition ins_flow.c:49
#define PAR_P3
Definition ins_flow.c:272
int moment_ind
Definition ins_flow.c:170
int use_filter
Definition ins_flow.c:185
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
Definition ins_flow.c:1456
float GT_theta
Definition ins_flow.c:191
#define PAR_P2
Definition ins_flow.c:271
#define INS_FLOW_GPS_ID
Definition ins_flow.c:87
float moments[MOMENT_DELAY]
Definition ins_flow.c:169
static void print_ins_flow_state(void)
Definition ins_flow.c:688
static void send_ins(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:360
#define PAR_PRED_ROLL_3
Definition ins_flow.c:279
#define PAR_Q_TB
Definition ins_flow.c:275
#define PAR_Q4
Definition ins_flow.c:268
#define MOMENT_DELAY
Definition ins_flow.c:168
struct NedCoor_i ltp_speed
Definition ins_flow.c:141
bool run_filter
Definition ins_flow.c:186
float optical_flow_y
Definition ins_flow.c:146
float lp_thrust
Definition ins_flow.c:160
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Definition ins_flow.c:1340
float OF_P[N_STATES_OF_KF][N_STATES_OF_KF]
Definition ins_flow.c:174
static uint8_t ahrs_flow_id
Component ID for FLOW.
Definition ins_flow.c:129
void ins_flow_init(void)
Definition ins_flow.c:532
#define INS_OPTICAL_FLOW_ID
Definition ins_flow.c:93
uint32_t counter
Definition ins_flow.c:187
static void print_true_state(void)
Definition ins_flow.c:713
float OF_X[N_STATES_OF_KF]
Definition ins_flow.c:172
float thrust_factor
Definition ins_flow.c:159
#define INS_FLOW_IMU_ID
Definition ins_flow.c:80
float vision_time
Definition ins_flow.c:148
#define OF_N_ROTORS
Definition ins_flow.c:177
float of_time
Definition ins_flow.c:180
static void send_ins_ref(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:376
struct NedCoor_i ltp_accel
Definition ins_flow.c:142
static void reset_cb(uint8_t sender_id UNUSED, uint8_t flag)
Definition ins_flow.c:657
float RPM_FACTORS[OF_N_ROTORS]
Definition ins_flow.c:178
static void send_geo_mag(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:338
float lp_gyro_pitch
Definition ins_flow.c:155
#define PAR_P4
Definition ins_flow.c:273
uint16_t RPM[8]
Definition ins_flow.c:152
#define PAR_KD
Definition ins_flow.c:274
#define PAR_Q3
Definition ins_flow.c:267
static void ins_act_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
Definition ins_flow.c:1444
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:348
#define DEBUG_PRINT(...)
Definition ins_flow.c:57
struct InsFlow ins_flow
Definition ins_flow.c:164
static void send_quat(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:299
static abi_event ins_act_feedback_ev
Definition ins_flow.c:108
uint8_t RPM_num_act
Definition ins_flow.c:153
float of_prev_time
Definition ins_flow.c:181
float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF]
Definition ins_flow.c:173
float thrust_factor
Definition ins_flow.c:188
static void ins_reset_filter(void)
Definition ins_flow.c:489
#define PAR_P1
Definition ins_flow.c:270
#define INS_FLOW_GYRO_ID
Definition ins_flow.c:68
struct NedCoor_i ltp_pos
Definition ins_flow.c:140
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:669
float lp_factor_strong
Definition ins_flow.c:183
#define PAR_P_TB
Definition ins_flow.c:276
float lp_gyro_roll
Definition ins_flow.c:157
#define PAR_K1
Definition ins_flow.c:259
float lp_gyro_bias_pitch
Definition ins_flow.c:156
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
Definition ins_flow.c:1373
void ins_flow_update(void)
Definition ins_flow.c:726
#define PAR_K0
Definition ins_flow.c:258
#define PAR_IX
Definition ins_flow.c:255
#define PAR_Q0
Definition ins_flow.c:264
struct LtpDef_i ltp_def
Definition ins_flow.c:136
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:1530
static void send_euler(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:315
static void set_body_state_from_quat(void)
Rotate angles and rates from imu to body frame and set state.
Definition ins_flow.c:1399
bool ltp_initialized
Definition ins_flow.c:137
bool reset_filter
Definition ins_flow.c:184
float optical_flow_x
Definition ins_flow.c:145
static void send_bias(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:331
#define PAR_K2
Definition ins_flow.c:260
float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF]
Definition ins_flow.c:175
static abi_event ins_optical_flow_ev
Definition ins_flow.c:107
#define PAR_R1
Definition ins_flow.c:263
float lp_roll_command
Definition ins_flow.c:161
float lp_gyro_bias_roll
Definition ins_flow.c:158
#define PAR_R0
Definition ins_flow.c:262
#define PAR_Q2
Definition ins_flow.c:266
#define PAR_Q1
Definition ins_flow.c:265
#define PAR_K3
Definition ins_flow.c:261
#define DEBUG_MAT_PRINT(...)
Definition ins_flow.c:58
float parameters[22]
Definition ins_flow.c:249
float divergence
Definition ins_flow.c:147
#define AHRS_ICQ_OUTPUT_ENABLED
Definition ins_flow.c:62
#define PAR_P0
Definition ins_flow.c:269
#define INS_FLOW_ACCEL_ID
Definition ins_flow.c:74
bool new_flow_measurement
Definition ins_flow.c:149
float GT_phi
Definition ins_flow.c:190
#define PAR_MASS
Definition ins_flow.c:256
static void send_ins_flow(struct transport_tx *trans, struct link_device *dev)
Definition ins_flow.c:390
#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:107
#define USE_HEIGHT
Definition ins_flow.h:145
#define USE_VELOCITY
Definition ins_flow.h:144
#define OF_THETA_IND
Definition ins_flow.h:113
#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:109
#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:138
#define OF_ANGLE_IND
Definition ins_flow.h:108
#define OF_DRAG
Definition ins_flow.h:36
#define N_MEAS_OF_KF
Definition ins_flow.h:122
#define OF_THRUST_BIAS_IND
Definition ins_flow.h:101
#define OF_Z_IND
Definition ins_flow.h:110
#define OF_Z_DOT_IND
Definition ins_flow.h:111
#define OF_VX_IND
Definition ins_flow.h:114
#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:100
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
static struct FloatVect3 H
int32_t rpm
RPM.
Definition actuators.h:54
Motor Mixing.
static float g
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
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
General stabilization interface for rotorcrafts.
API to get/set the generic vehicle states.
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition sys_time.h:241
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
uint16_t val[TCOUPLE_NB]
static float P[3][3]
static float K[9]
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.