Paparazzi UAS  v5.15_devel-99-g2ff7410
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_mekf_wind_wrapper.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  */
20 
30 #if USE_AHRS_ALIGNER
32 #endif
33 #include "subsystems/ins.h"
34 #include "subsystems/abi.h"
35 #include "math/pprz_isa.h"
36 #include "state.h"
37 
38 #include "generated/airframe.h"
39 #include "generated/flight_plan.h"
40 
41 #define MEKF_WIND_USE_UTM TRUE
42 #if MEKF_WIND_USE_UTM
44 #endif
45 
46 #ifndef INS_MEKF_WIND_FILTER_ID
47 #define INS_MEKF_WIND_FILTER_ID 3
48 #endif
49 
51 
55 
57 static void set_state_from_ins(void);
58 
60 #if LOG_MEKF_WIND
61 #ifndef SITL
63 #define PrintLog sdLogWriteLog
64 #define LogFileIsOpen() (pprzLogFile != -1)
65 #else // SITL: print in a file
66 #include <stdio.h>
67 #define PrintLog fprintf
68 #define LogFileIsOpen() (pprzLogFile != NULL)
69 static FILE* pprzLogFile = NULL;
70 #endif
71 #endif
72 
74 #if PERIODIC_TELEMETRY
76 #include "mcu_periph/sys_time.h"
77 
78 static void send_euler(struct transport_tx *trans, struct link_device *dev)
79 {
80  struct FloatEulers ltp_to_imu_euler;
81  struct FloatQuat quat = ins_mekf_wind_get_quat();
82  float_eulers_of_quat(&ltp_to_imu_euler, &quat);
84  pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
85  &ltp_to_imu_euler.phi,
86  &ltp_to_imu_euler.theta,
87  &ltp_to_imu_euler.psi,
88  &id);
89 }
90 
91 static void send_wind(struct transport_tx *trans, struct link_device *dev)
92 {
93  struct NedCoor_f wind_ned = ins_mekf_wind_get_wind_ned();
94  struct EnuCoor_f wind_enu;
95  ENU_OF_TO_NED(wind_enu, wind_ned);
97  uint8_t flags = 7; // 3D wind + airspeed
98  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags,
99  &wind_enu.x, &wind_enu.y, &wind_enu.z, &va);
100 }
101 
102 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
103 {
104  uint8_t mde = 3; // OK
105  uint16_t val = 0;
106  if (!ins_mekf_wind.is_aligned) {
107  mde = 2; // ALIGN
109  mde = 1; // INIT
110  }
112  /* set lost if no new gyro measurements for 50ms */
113  if (t_diff > 50000) {
114  mde = 5; // IMU_LOST
115  }
117  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
118 }
119 
120 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
121 {
122  struct FloatEulers eulers;
123  struct FloatQuat quat = ins_mekf_wind_get_quat();
124  float_eulers_of_quat(&eulers, &quat);
125  //struct FloatRates rates = ins_mekf_wind_get_body_rates();
126  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
127  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
128  //struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
131  float airspeed = ins_mekf_wind_get_airspeed_norm();
132  pprz_msg_send_INV_FILTER(trans, dev,
133  AC_ID,
134  &quat.qi,
135  &eulers.phi,
136  &eulers.theta,
137  &eulers.psi,
138  &speed.x,
139  &speed.y,
140  &speed.z,
141  &pos.x,
142  &pos.y,
143  &pos.z,
144  &rb.p,
145  &rb.q,
146  &rb.r,
147  &ab.x,
148  &ab.y,
149  &ab.z,
150  &airspeed);
151 }
152 #endif
153 
154 /*
155  * ABI bindings
156  */
157 
159 #ifndef INS_MEKF_WIND_AIRSPEED_ID
160 #define INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST
161 #endif
162 PRINT_CONFIG_VAR(INS_MEKF_WIND_AIRSPEED_ID)
163 
164 
165 #ifndef INS_MEKF_WIND_INCIDENCE_ID
166 #define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST
167 #endif
168 PRINT_CONFIG_VAR(INS_MEKF_WIND_INCIDENCE_ID)
169 
170 
171 #ifndef INS_MEKF_WIND_BARO_ID
172 #if USE_BARO_BOARD
173 #define INS_MEKF_WIND_BARO_ID BARO_BOARD_SENDER_ID
174 #else
175 #define INS_MEKF_WIND_BARO_ID ABI_BROADCAST
176 #endif
177 #endif
178 PRINT_CONFIG_VAR(INS_MEKF_WIND_BARO_ID)
179 
180 
181 #ifndef INS_MEKF_WIND_IMU_ID
182 #define INS_MEKF_WIND_IMU_ID ABI_BROADCAST
183 #endif
184 PRINT_CONFIG_VAR(INS_MEKF_WIND_IMU_ID)
185 
186 
187 #ifndef INS_MEKF_WIND_MAG_ID
188 #define INS_MEKF_WIND_MAG_ID ABI_BROADCAST
189 #endif
190 PRINT_CONFIG_VAR(INS_MEKF_WIND_MAG_ID)
191 
192 
195 #ifndef INS_MEKF_WIND_GPS_ID
196 #define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID
197 #endif
198 PRINT_CONFIG_VAR(INS_MEKF_WIND_GPS_ID)
199 
211 
212 
213 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
214 {
215  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
216  static float alpha = 10.0f;
217  static int32_t i = 1;
218  static float baro_moy = 0.0f;
219  static float baro_prev = 0.0f;
220 
222  // try to find a stable qfe
223  // TODO generic function in pprz_isa ?
224  if (i == 1) {
225  baro_moy = pressure;
226  baro_prev = pressure;
227  }
228  baro_moy = (baro_moy * (i - 1) + pressure) / i;
229  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
230  baro_prev = baro_moy;
231  // test stop condition
232  if (fabs(alpha) < 0.1f) {
233  ins_qfe = baro_moy;
235  }
236  if (i == 250) {
237  ins_qfe = pressure;
239  }
240  i++;
241  } else { /* normal update with baro measurement */
243  float baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
244  ins_mekf_wind_update_baro(baro_alt);
245 
246 #if LOG_MEKF_WIND
247  if (LogFileIsOpen()) {
248  PrintLog(pprzLogFile, "%.3f baro %.3f \n", get_sys_time_float(), baro_alt);
249  }
250 #endif
251  }
252  }
253 }
254 
255 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pdyn)
256 {
258  float airspeed = tas_from_dynamic_pressure(pdyn);
260 
261 #if LOG_MEKF_WIND
262  if (LogFileIsOpen()) {
263  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
264  }
265 #endif
266  }
267 }
268 
269 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float airspeed)
270 {
273 
274 #if LOG_MEKF_WIND
275  if (LogFileIsOpen()) {
276  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
277  }
278 #endif
279  }
280 }
281 
282 static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
283 {
284  if (ins_mekf_wind.is_aligned && bit_is_set(flag, 0) && bit_is_set(flag, 1)) {
285  ins_mekf_wind_update_incidence(aoa, sideslip);
286 
287 #if LOG_MEKF_WIND
288  if (LogFileIsOpen()) {
289  PrintLog(pprzLogFile, "%.3f incidence %.3f %.3f\n", get_sys_time_float(), aoa, sideslip);
290  }
291 #endif
292  }
293 }
294 
300 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
301  uint32_t stamp, struct Int32Rates *gyro)
302 {
304  struct FloatRates gyro_f, gyro_body;
305  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
306  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
307  // new values in body frame
308  float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
309 
310 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
311  PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
312 
313  if (last_imu_stamp > 0) {
314  float dt = (float)(stamp - last_imu_stamp) * 1e-6;
317  } else {
319  }
320  }
321 #else
322  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS MEKF_WIND propagation.")
323  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
324  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
327  } else {
329  }
330 #endif
331 
332  // update state interface
334 
335 #if LOG_MEKF_WIND
336  if (LogFileIsOpen()) {
337  float time = get_sys_time_float();
338 
339  PrintLog(pprzLogFile,
340  "%.3f gyro_accel %.3f %.3f %.3f %.3f %.3f %.3f \n",
341  time,
342  gyro_body.p, gyro_body.q, gyro_body.r,
346 
347  struct FloatEulers eulers;
348  struct FloatQuat quat = ins_mekf_wind_get_quat();
349  float_eulers_of_quat(&eulers, &quat);
350  struct FloatRates rates = ins_mekf_wind_get_body_rates();
351  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
352  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
353  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
356  float bb = ins_mekf_wind_get_baro_bias();
357  struct NedCoor_f wind = ins_mekf_wind_get_wind_ned();
358  float airspeed = ins_mekf_wind_get_airspeed_norm();
359  PrintLog(pprzLogFile,
360  "%.3f output %.4f %.4f %.4f %.3f %.3f %.3f ",
361  time,
362  eulers.phi, eulers.theta, eulers.psi,
363  rates.p, rates.q, rates.r);
364  PrintLog(pprzLogFile,
365  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f ",
366  pos.x, pos.y, pos.z,
367  speed.x, speed.y, speed.z,
368  accel.x, accel.y, accel.z);
369  PrintLog(pprzLogFile,
370  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
371  ab.x, ab.y, ab.z, rb.p, rb.q, rb.r, bb,
372  wind.x, wind.y, wind.z, airspeed);
373  }
374 #endif
375  }
376 
377  /* timestamp in usec when last callback was received */
378  last_imu_stamp = stamp;
379 }
380 
381 static void accel_cb(uint8_t sender_id __attribute__((unused)),
382  uint32_t stamp __attribute__((unused)),
383  struct Int32Vect3 *accel)
384 {
385  struct FloatVect3 accel_f;
386  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
387  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
388  // new values in body frame
389  float_rmat_transp_vmult(&ins_mekf_wind_accel, body_to_imu_rmat, &accel_f);
390 }
391 
392 static void mag_cb(uint8_t sender_id __attribute__((unused)),
393  uint32_t stamp __attribute__((unused)),
394  struct Int32Vect3 *mag)
395 {
397  struct FloatVect3 mag_f, mag_body;
398  MAGS_FLOAT_OF_BFP(mag_f, *mag);
399  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
400  // new values in body frame
401  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
402  // only correct attitude if GPS is not initialized
404 
405 #if LOG_MEKF_WIND
406  if (LogFileIsOpen()) {
407  PrintLog(pprzLogFile,
408  "%.3f magneto %.3f %.3f %.3f\n",
410  mag_body.x, mag_body.y, mag_body.z);
411  }
412 #endif
413  }
414 }
415 
416 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
417  uint32_t stamp __attribute__((unused)),
418  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
419  struct Int32Vect3 *lp_mag)
420 {
421  if (!ins_mekf_wind.is_aligned) {
422  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
423 
424  struct FloatRates gyro_f, gyro_body;
425  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
426  float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
427 
428  struct FloatVect3 accel_f, accel_body;
429  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
430  float_rmat_transp_vmult(&accel_body, body_to_imu_rmat, &accel_f);
431 
432  struct FloatVect3 mag_f, mag_body;
433  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
434  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
435 
436  struct FloatQuat quat;
437  ahrs_float_get_quat_from_accel_mag(&quat, &accel_body, &mag_body);
438  ins_mekf_wind_align(&gyro_body, &quat);
439  // udate state interface
441 
442  // ins and ahrs are now running
443  ins_mekf_wind.is_aligned = true;
444  }
445 }
446 
447 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
448  struct FloatQuat *q_b2i)
449 {
451  if (!ins_mekf_wind.is_aligned) {
452  // set ltp_to_imu so that body is zero
453  ins_mekf_wind_set_quat(q_b2i);
454  }
455 }
456 
457 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
458 {
460 }
461 
462 static void gps_cb(uint8_t sender_id __attribute__((unused)),
463  uint32_t stamp __attribute__((unused)),
464  struct GpsState *gps_s)
465 {
466  if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) {
467 
468 #if MEKF_WIND_USE_UTM
469  if (state.utm_initialized_f) {
470  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
471  struct FloatVect3 pos, speed;
472  // position (local ned)
473  pos.x = utm.north - state.utm_origin_f.north;
474  pos.y = utm.east - state.utm_origin_f.east;
475  pos.z = state.utm_origin_f.alt - utm.alt;
476  // speed
477  speed.x = gps_s->ned_vel.x / 100.0f;
478  speed.y = gps_s->ned_vel.y / 100.0f;
479  speed.z = gps_s->ned_vel.z / 100.0f;
481  ins_mekf_wind_set_pos_ned((struct NedCoor_f*)(&pos));
482  ins_mekf_wind_set_speed_ned((struct NedCoor_f*)(&speed));
484  }
485  ins_mekf_wind_update_pos_speed(&pos, &speed);
486 
487 #if LOG_MEKFW_FILTER
488  if (LogFileIsOpen()) {
489  PrintLog(pprzLogFile,
490  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
492  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
493  }
494 #endif
495  }
496 
497 #else
498  if (state.ned_initialized_f) {
499  struct FloatVect3 pos, speed;
500  struct NedCoor_i gps_pos_cm_ned, ned_pos;
501  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
503  NED_FLOAT_OF_BFP(pos, ned_pos);
504  struct EcefCoor_f ecef_vel;
505  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
506  ned_of_ecef_vect_f(&speed, &state.ned_origin_f, &ecef_vel);
507  ins_mekf_wind_update_pos_speed(&pos, &speed);
508 
509 #if LOG_MEKFW_FILTER
510  if (LogFileIsOpen()) {
511  PrintLog(pprzLogFile,
512  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
514  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
515  }
516 #endif
517  }
518 #endif
519  }
520 }
521 
525 static void set_state_from_ins(void)
526 {
527  struct FloatQuat quat = ins_mekf_wind_get_quat();
529 
530  struct FloatRates rates = ins_mekf_wind_get_body_rates();
531  stateSetBodyRates_f(&rates);
532 
533  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
534  stateSetPositionNed_f(&pos);
535 
536  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
537  stateSetSpeedNed_f(&speed);
538 
539  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
540  stateSetAccelNed_f(&accel);
541 }
542 
547 {
548  // init position
549 #if MEKF_WIND_USE_UTM
550  struct UtmCoor_f utm0;
551  utm0.north = (float)nav_utm_north0;
552  utm0.east = (float)nav_utm_east0;
553  utm0.alt = GROUND_ALT;
554  utm0.zone = nav_utm_zone0;
556  stateSetPositionUtm_f(&utm0);
557 #else
558  struct LlaCoor_i llh_nav0;
559  llh_nav0.lat = NAV_LAT0;
560  llh_nav0.lon = NAV_LON0;
561  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
562  struct EcefCoor_i ecef_nav0;
563  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
564  struct LtpDef_i ltp_def;
565  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
566  ltp_def.hmsl = NAV_ALT0;
567  stateSetLocalOrigin_i(&ltp_def);
568 #endif
569 
570  // reset flags
571  ins_mekf_wind.is_aligned = false;
572  ins_mekf_wind.reset = false;
575 
576  // aligner
577 #if USE_AHRS_ALIGNER
579 #endif
580 
581  // init filter
583  const struct FloatVect3 mag_h = { INS_H_X, INS_H_Y, INS_H_Z };
584  ins_mekf_wind_set_mag_h(&mag_h);
585 
586  // Bind to ABI messages
587  AbiBindMsgBARO_ABS(INS_MEKF_WIND_BARO_ID, &baro_ev, baro_cb);
589  AbiBindMsgAIRSPEED(INS_MEKF_WIND_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
590  AbiBindMsgINCIDENCE(INS_MEKF_WIND_INCIDENCE_ID, &incidence_ev, incidence_cb);
591  AbiBindMsgIMU_MAG_INT32(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb);
592  AbiBindMsgIMU_GYRO_INT32(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
593  AbiBindMsgIMU_ACCEL_INT32(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
594  AbiBindMsgIMU_LOWPASSED(INS_MEKF_WIND_IMU_ID, &aligner_ev, aligner_cb);
595  AbiBindMsgBODY_TO_IMU_QUAT(INS_MEKF_WIND_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
596  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
597  AbiBindMsgGPS(INS_MEKF_WIND_GPS_ID, &gps_ev, gps_cb);
598 
599 #if PERIODIC_TELEMETRY
600  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler);
601  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
602  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
604 #endif
605 
606  // init log
607 #if LOG_MEKF_WIND && SITL
608  // open log file for writing
609  // path should be specified in airframe file
610  uint32_t counter = 0;
611  char filename[512];
612  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), counter);
613  // check availale name
614  while ((pprzLogFile = fopen(filename, "r"))) {
615  fclose(pprzLogFile);
616  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), ++counter);
617  }
618  pprzLogFile = fopen(filename, "w");
619  if (pprzLogFile == NULL) {
620  printf("Failed to open WE log file '%s'\n",filename);
621  } else {
622  printf("Opening WE log file '%s'\n",filename);
623  }
624 #endif
625 
626 }
627 
633 {
634 #if MEKF_WIND_USE_UTM
635  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
636  // reset state UTM ref
638 #else
639  struct LtpDef_i ltp_def;
640  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
641  ltp_def.hmsl = gps.hmsl;
642  stateSetLocalOrigin_i(&ltp_def);
643 #endif
645  //ins_mekf_wind.gps_initialized = false;
646 }
647 
649 {
650 #if MEKF_WIND_USE_UTM
651  struct UtmCoor_f utm = state.utm_origin_f;
652  utm.alt = gps.hmsl / 1000.0f;
654 #else
655  struct LlaCoor_i lla = {
657  .lon = state.ned_origin_i.lla.lon,
658  .alt = gps.lla_pos.alt
659  };
660  struct LtpDef_i ltp_def;
661  ltp_def_from_lla_i(&ltp_def, &lla);
662  ltp_def.hmsl = gps.hmsl;
663  stateSetLocalOrigin_i(&ltp_def);
664 #endif
665 }
666 
static uint32_t last_imu_stamp
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
static void set_state_from_ins(void)
update state interface
Interface to align the AHRS via low-passed measurements at startup.
unsigned short uint16_t
Definition: types.h:16
void ins_mekf_wind_init(void)
Init function.
angular rates
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
static abi_event gps_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_mekf_wind_propagate on new gyro measurements.
float y
in meters
float east
in meters
float phi
in radians
static abi_event mag_ev
definition of the local (flat earth) coordinate system
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
float north
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
void ins_mekf_wind_set_quat(struct FloatQuat *quat)
Periodic telemetry system header (includes downlink utility and generated code).
#define NED_FLOAT_OF_BFP(_o, _i)
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
vector in EarthCenteredEarthFixed coordinates
vector in EarthCenteredEarthFixed coordinates
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
#define ins_mekf_wind_wrapper_Reset(_v)
void ins_mekf_wind_update_baro(float baro_alt)
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
vector in East North Up coordinates Units: meters
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
float r
in rad/s
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
float alpha
Definition: textons.c:107
Main include for ABI (AirBorneInterface).
Utility functions for floating point AHRS implementations.
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
static abi_event accel_ev
uint8_t nav_utm_zone0
Definition: common_nav.c:44
float psi
in radians
Integrated Navigation System interface.
#define INS_MEKF_WIND_IMU_ID
IMU (gyro, accel)
static void send_wind(struct transport_tx *trans, struct link_device *dev)
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
position in UTM coordinates Units: meters
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
static void airspeed_cb(uint8_t sender_id, float airspeed)
vector in Latitude, Longitude and Altitude
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
float q
in rad/s
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
float p
in rad/s
int32_t z
Down.
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
static abi_event incidence_ev
static void body_to_imu_cb(uint8_t sender_id, struct FloatQuat *q_b2i)
int32_t hmsl
Height above mean sea level in mm.
Multiplicative Extended Kalman Filter in rotation matrix formulation.
static abi_event baro_ev
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
static abi_event gyro_ev
euler angles
float z
in meters
struct InsMekfWind ins_mekf_wind
Roation quaternion.
int32_t y
East.
float ins_mekf_wind_get_airspeed_norm(void)
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition: air_data.c:338
float ins_mekf_wind_get_baro_bias(void)
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
float theta
in radians
static abi_event airspeed_ev
struct FloatQuat ins_mekf_wind_get_quat(void)
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
filter structure
static abi_event pressure_diff_ev
#define INS_MEKF_WIND_AIRSPEED_ID
airspeed (Pitot tube)
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
Paparazzi specific wrapper to run MEKF-Wind INS filter.
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
void float_rmat_transp_vmult(struct FloatVect3 *vb, struct FloatRMat *m_b2a, struct FloatVect3 *va)
rotate 3D vector by transposed rotation matrix.
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
int32_t nav_utm_north0
Definition: common_nav.c:43
vector in North East Down coordinates Units: meters
float x
in meters
Architecture independent timing functions.
data structure for GPS information
Definition: gps.h:87
#define INT32_POS_OF_CM_NUM
#define INS_MEKF_WIND_MAG_ID
magnetometer
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
#define INS_MEKF_WIND_BARO_ID
baro
static struct FloatVect3 ins_mekf_wind_accel
last accel measurement
Paparazzi atmospheric pressure conversion utilities.
uint16_t val[TCOUPLE_NB]
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition: air_data.c:365
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
static abi_event aligner_ev
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
int32_t counter
struct LlaCoor_i lla
Reference point in lla.
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
struct OrientationReps body_to_imu
int32_t lon
in degrees*1e7
FileDes pprzLogFile
Definition: sdlog_chibios.c:85
struct FloatRates ins_mekf_wind_get_rates_bias(void)
static void aligner_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
uint8_t zone
UTM zone number.
static void stateSetPositionNed_f(struct NedCoor_f *ned_pos)
Set position from local NED coordinates (float).
Definition: state.h:598
struct UtmCoor_f utm_origin_f
Definition of the origin of Utm coordinate system.
Definition: state.h:233
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
#define INS_MEKF_WIND_FILTER_ID
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
signed long int32_t
Definition: types.h:19
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
void ins_mekf_wind_wrapper_init(void)
Init function.
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
struct FloatRates ins_mekf_wind_get_body_rates(void)
#define INT32_POS_OF_CM_DEN
unsigned char uint8_t
Definition: types.h:14
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
Convenience functions to get utm position from GPS state.
Definition: gps.c:389
API to get/set the generic vehicle states.
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
void float_rmat_transp_ratemult(struct FloatRates *rb, struct FloatRMat *m_b2a, struct FloatRates *ra)
rotate anglular rates by transposed rotation matrix.
static void send_euler(struct transport_tx *trans, struct link_device *dev)
logging functions
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.
vector in North East Down coordinates
int32_t nav_utm_east0
Definition: common_nav.c:42
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static abi_event geo_mag_ev
float z
in meters
void ins_mekf_wind_update_airspeed(float airspeed)
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
rotation matrix
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:56
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
static abi_event body_to_imu_ev
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in)
Convert a LLA to ECEF.
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
#define INS_MEKF_WIND_GPS_ID
ABI binding for gps data.
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
static float pprz_isa_height_of_pressure(float pressure, float ref_p)
Get relative altitude from pressure (using simplified equation).
Definition: pprz_isa.h:102
#define ECEF_FLOAT_OF_BFP(_o, _i)
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
#define INS_MEKF_WIND_INCIDENCE_ID
incidence angles
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
static void pressure_diff_cb(uint8_t sender_id, float pdyn)
int32_t lat
in degrees*1e7
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
uint8_t fix
status of fix
Definition: gps.h:107
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
void ins_reset_altitude_ref(void)
INS altitude reference reset.
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:81
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
float y
in meters
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
void ins_mekf_wind_update_incidence(float aoa, float aos)
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
struct GpsState gps
global GPS state
Definition: gps.c:69
angular rates
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
float x
in meters
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.