Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
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 
31 #if USE_AHRS_ALIGNER
33 #endif
34 #include "subsystems/ins.h"
35 #include "subsystems/abi.h"
36 #include "math/pprz_isa.h"
37 #include "state.h"
38 
39 #include "generated/airframe.h"
40 #include "generated/flight_plan.h"
41 
42 #define MEKF_WIND_USE_UTM TRUE
43 #if MEKF_WIND_USE_UTM
45 #endif
46 
47 #ifndef INS_MEKF_WIND_FILTER_ID
48 #define INS_MEKF_WIND_FILTER_ID 3
49 #endif
50 
52 
56 
58 static void set_state_from_ins(void);
59 
61 #if LOG_MEKF_WIND
62 #ifndef SITL
64 #define PrintLog sdLogWriteLog
65 #define LogFileIsOpen() (pprzLogFile != -1)
66 #else // SITL: print in a file
67 #include <stdio.h>
68 #define PrintLog fprintf
69 #define LogFileIsOpen() (pprzLogFile != NULL)
70 static FILE* pprzLogFile = NULL;
71 #endif
72 #endif
73 
75 #if PERIODIC_TELEMETRY
77 #include "mcu_periph/sys_time.h"
78 
79 static void send_euler(struct transport_tx *trans, struct link_device *dev)
80 {
81  struct FloatEulers ltp_to_imu_euler;
82  struct FloatQuat quat = ins_mekf_wind_get_quat();
83  float_eulers_of_quat(&ltp_to_imu_euler, &quat);
85  pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
86  &ltp_to_imu_euler.phi,
87  &ltp_to_imu_euler.theta,
88  &ltp_to_imu_euler.psi,
89  &id);
90 }
91 
92 static void send_wind(struct transport_tx *trans, struct link_device *dev)
93 {
94  struct NedCoor_f wind_ned = ins_mekf_wind_get_wind_ned();
95  struct EnuCoor_f wind_enu;
96  ENU_OF_TO_NED(wind_enu, wind_ned);
98  uint8_t flags = 7; // 3D wind + airspeed
99  pprz_msg_send_WIND_INFO_RET(trans, dev, AC_ID, &flags,
100  &wind_enu.x, &wind_enu.y, &wind_enu.z, &va);
101 }
102 
103 static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
104 {
105  uint8_t mde = 3; // OK
106  uint16_t val = 0;
107  if (!ins_mekf_wind.is_aligned) {
108  mde = 2; // ALIGN
110  mde = 1; // INIT
111  }
113  /* set lost if no new gyro measurements for 50ms */
114  if (t_diff > 50000) {
115  mde = 5; // IMU_LOST
116  }
118  pprz_msg_send_STATE_FILTER_STATUS(trans, dev, AC_ID, &id, &mde, &val);
119 }
120 
121 static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
122 {
123  struct FloatEulers eulers;
124  struct FloatQuat quat = ins_mekf_wind_get_quat();
125  float_eulers_of_quat(&eulers, &quat);
126  //struct FloatRates rates = ins_mekf_wind_get_body_rates();
127  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
128  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
129  //struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
132  float airspeed = ins_mekf_wind_get_airspeed_norm();
133  pprz_msg_send_INV_FILTER(trans, dev,
134  AC_ID,
135  &quat.qi,
136  &eulers.phi,
137  &eulers.theta,
138  &eulers.psi,
139  &speed.x,
140  &speed.y,
141  &speed.z,
142  &pos.x,
143  &pos.y,
144  &pos.z,
145  &rb.p,
146  &rb.q,
147  &rb.r,
148  &ab.x,
149  &ab.y,
150  &ab.z,
151  &airspeed);
152 }
153 #endif
154 
155 /*
156  * ABI bindings
157  */
158 
160 #ifndef INS_MEKF_WIND_AIRSPEED_ID
161 #define INS_MEKF_WIND_AIRSPEED_ID ABI_BROADCAST
162 #endif
163 PRINT_CONFIG_VAR(INS_MEKF_WIND_AIRSPEED_ID)
164 
165 
166 #ifndef INS_MEKF_WIND_INCIDENCE_ID
167 #define INS_MEKF_WIND_INCIDENCE_ID ABI_BROADCAST
168 #endif
169 PRINT_CONFIG_VAR(INS_MEKF_WIND_INCIDENCE_ID)
170 
171 
172 #ifndef INS_MEKF_WIND_BARO_ID
173 #if USE_BARO_BOARD
174 #define INS_MEKF_WIND_BARO_ID BARO_BOARD_SENDER_ID
175 #else
176 #define INS_MEKF_WIND_BARO_ID ABI_BROADCAST
177 #endif
178 #endif
179 PRINT_CONFIG_VAR(INS_MEKF_WIND_BARO_ID)
180 
181 
182 #ifndef INS_MEKF_WIND_IMU_ID
183 #define INS_MEKF_WIND_IMU_ID ABI_BROADCAST
184 #endif
185 PRINT_CONFIG_VAR(INS_MEKF_WIND_IMU_ID)
186 
187 
188 #ifndef INS_MEKF_WIND_MAG_ID
189 #define INS_MEKF_WIND_MAG_ID ABI_BROADCAST
190 #endif
191 PRINT_CONFIG_VAR(INS_MEKF_WIND_MAG_ID)
192 
193 
196 #ifndef INS_MEKF_WIND_GPS_ID
197 #define INS_MEKF_WIND_GPS_ID GPS_MULTI_ID
198 #endif
199 PRINT_CONFIG_VAR(INS_MEKF_WIND_GPS_ID)
200 
212 
213 
214 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
215 {
216  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
217  static float alpha = 10.0f;
218  static int32_t i = 1;
219  static float baro_moy = 0.0f;
220  static float baro_prev = 0.0f;
221 
223  // try to find a stable qfe
224  // TODO generic function in pprz_isa ?
225  if (i == 1) {
226  baro_moy = pressure;
227  baro_prev = pressure;
228  }
229  baro_moy = (baro_moy * (i - 1) + pressure) / i;
230  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
231  baro_prev = baro_moy;
232  // test stop condition
233  if (fabs(alpha) < 0.1f) {
234  ins_qfe = baro_moy;
236  }
237  if (i == 250) {
238  ins_qfe = pressure;
240  }
241  i++;
242  } else { /* normal update with baro measurement */
244  float baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
245  ins_mekf_wind_update_baro(baro_alt);
246 
247 #if LOG_MEKF_WIND
248  if (LogFileIsOpen()) {
249  PrintLog(pprzLogFile, "%.3f baro %.3f \n", get_sys_time_float(), baro_alt);
250  }
251 #endif
252  }
253  }
254 }
255 
256 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pdyn)
257 {
259  float airspeed = tas_from_dynamic_pressure(pdyn);
261 
262 #if LOG_MEKF_WIND
263  if (LogFileIsOpen()) {
264  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
265  }
266 #endif
267  }
268 }
269 
270 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float airspeed)
271 {
274 
275 #if LOG_MEKF_WIND
276  if (LogFileIsOpen()) {
277  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
278  }
279 #endif
280  }
281 }
282 
283 static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
284 {
285  if (ins_mekf_wind.is_aligned && bit_is_set(flag, 0) && bit_is_set(flag, 1)) {
286  ins_mekf_wind_update_incidence(aoa, sideslip);
287 
288 #if LOG_MEKF_WIND
289  if (LogFileIsOpen()) {
290  PrintLog(pprzLogFile, "%.3f incidence %.3f %.3f\n", get_sys_time_float(), aoa, sideslip);
291  }
292 #endif
293  }
294 }
295 
301 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
302  uint32_t stamp, struct Int32Rates *gyro)
303 {
305  struct FloatRates gyro_f, gyro_body;
306  RATES_FLOAT_OF_BFP(gyro_f, *gyro);
307  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
308  // new values in body frame
309  float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
310 
311 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
312  PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
313 
314  if (last_imu_stamp > 0) {
315  float dt = (float)(stamp - last_imu_stamp) * 1e-6;
318  } else {
320  }
321  }
322 #else
323  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS MEKF_WIND propagation.")
324  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
325  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
328  } else {
330  }
331 #endif
332 
333  // update state interface
335 
336 #if LOG_MEKF_WIND
337  if (LogFileIsOpen()) {
338  float time = get_sys_time_float();
339 
340  PrintLog(pprzLogFile,
341  "%.3f gyro_accel %.3f %.3f %.3f %.3f %.3f %.3f \n",
342  time,
343  gyro_body.p, gyro_body.q, gyro_body.r,
347 
348  struct FloatEulers eulers;
349  struct FloatQuat quat = ins_mekf_wind_get_quat();
350  float_eulers_of_quat(&eulers, &quat);
351  struct FloatRates rates = ins_mekf_wind_get_body_rates();
352  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
353  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
354  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
357  float bb = ins_mekf_wind_get_baro_bias();
358  struct NedCoor_f wind = ins_mekf_wind_get_wind_ned();
359  float airspeed = ins_mekf_wind_get_airspeed_norm();
360  PrintLog(pprzLogFile,
361  "%.3f output %.4f %.4f %.4f %.3f %.3f %.3f ",
362  time,
363  eulers.phi, eulers.theta, eulers.psi,
364  rates.p, rates.q, rates.r);
365  PrintLog(pprzLogFile,
366  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f ",
367  pos.x, pos.y, pos.z,
368  speed.x, speed.y, speed.z,
369  accel.x, accel.y, accel.z);
370  PrintLog(pprzLogFile,
371  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
372  ab.x, ab.y, ab.z, rb.p, rb.q, rb.r, bb,
373  wind.x, wind.y, wind.z, airspeed);
374  }
375 #endif
376  }
377 
378  /* timestamp in usec when last callback was received */
379  last_imu_stamp = stamp;
380 }
381 
382 static void accel_cb(uint8_t sender_id __attribute__((unused)),
383  uint32_t stamp __attribute__((unused)),
384  struct Int32Vect3 *accel)
385 {
386  struct FloatVect3 accel_f;
387  ACCELS_FLOAT_OF_BFP(accel_f, *accel);
388  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
389  // new values in body frame
390  float_rmat_transp_vmult(&ins_mekf_wind_accel, body_to_imu_rmat, &accel_f);
391 }
392 
393 static void mag_cb(uint8_t sender_id __attribute__((unused)),
394  uint32_t stamp __attribute__((unused)),
395  struct Int32Vect3 *mag)
396 {
398  struct FloatVect3 mag_f, mag_body;
399  MAGS_FLOAT_OF_BFP(mag_f, *mag);
400  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
401  // new values in body frame
402  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
403  // only correct attitude if GPS is not initialized
405 
406 #if LOG_MEKF_WIND
407  if (LogFileIsOpen()) {
408  PrintLog(pprzLogFile,
409  "%.3f magneto %.3f %.3f %.3f\n",
411  mag_body.x, mag_body.y, mag_body.z);
412  }
413 #endif
414  }
415 }
416 
417 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
418  uint32_t stamp __attribute__((unused)),
419  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
420  struct Int32Vect3 *lp_mag)
421 {
422  if (!ins_mekf_wind.is_aligned) {
423  struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ins_mekf_wind.body_to_imu);
424 
425  struct FloatRates gyro_f, gyro_body;
426  RATES_FLOAT_OF_BFP(gyro_f, *lp_gyro);
427  float_rmat_transp_ratemult(&gyro_body, body_to_imu_rmat, &gyro_f);
428 
429  struct FloatVect3 accel_f, accel_body;
430  ACCELS_FLOAT_OF_BFP(accel_f, *lp_accel);
431  float_rmat_transp_vmult(&accel_body, body_to_imu_rmat, &accel_f);
432 
433  struct FloatVect3 mag_f, mag_body;
434  MAGS_FLOAT_OF_BFP(mag_f, *lp_mag);
435  float_rmat_transp_vmult(&mag_body, body_to_imu_rmat, &mag_f);
436 
437  struct FloatQuat quat;
438  ahrs_float_get_quat_from_accel_mag(&quat, &accel_body, &mag_body);
439  ins_mekf_wind_align(&gyro_body, &quat);
440  // udate state interface
442 
443  // ins and ahrs are now running
444  ins_mekf_wind.is_aligned = true;
445  }
446 }
447 
448 static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
449  struct FloatQuat *q_b2i)
450 {
452  if (!ins_mekf_wind.is_aligned) {
453  // set ltp_to_imu so that body is zero
454  ins_mekf_wind_set_quat(q_b2i);
455  }
456 }
457 
458 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
459 {
461 }
462 
463 static void gps_cb(uint8_t sender_id __attribute__((unused)),
464  uint32_t stamp __attribute__((unused)),
465  struct GpsState *gps_s)
466 {
467  if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) {
468 
469 #if MEKF_WIND_USE_UTM
470  if (state.utm_initialized_f) {
471  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
472  struct FloatVect3 pos, speed;
473  // position (local ned)
474  pos.x = utm.north - state.utm_origin_f.north;
475  pos.y = utm.east - state.utm_origin_f.east;
476  pos.z = state.utm_origin_f.alt - utm.alt;
477  // speed
478  speed.x = gps_s->ned_vel.x / 100.0f;
479  speed.y = gps_s->ned_vel.y / 100.0f;
480  speed.z = gps_s->ned_vel.z / 100.0f;
482  ins_mekf_wind_set_pos_ned((struct NedCoor_f*)(&pos));
483  ins_mekf_wind_set_speed_ned((struct NedCoor_f*)(&speed));
485  }
486  ins_mekf_wind_update_pos_speed(&pos, &speed);
487 
488 #if LOG_MEKFW_FILTER
489  if (LogFileIsOpen()) {
490  PrintLog(pprzLogFile,
491  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
493  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
494  }
495 #endif
496  }
497 
498 #else
499  if (state.ned_initialized_f) {
500  struct FloatVect3 pos, speed;
501  struct NedCoor_i gps_pos_cm_ned, ned_pos;
502  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos);
504  NED_FLOAT_OF_BFP(pos, ned_pos);
505  struct EcefCoor_f ecef_vel;
506  ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel);
507  ned_of_ecef_vect_f(&speed, &state.ned_origin_f, &ecef_vel);
508  ins_mekf_wind_update_pos_speed(&pos, &speed);
509 
510 #if LOG_MEKFW_FILTER
511  if (LogFileIsOpen()) {
512  PrintLog(pprzLogFile,
513  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
515  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
516  }
517 #endif
518  }
519 #endif
520  }
521 }
522 
526 static void set_state_from_ins(void)
527 {
528  struct FloatQuat quat = ins_mekf_wind_get_quat();
530 
531  struct FloatRates rates = ins_mekf_wind_get_body_rates();
532  stateSetBodyRates_f(&rates);
533 
534  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
535  stateSetPositionNed_f(&pos);
536 
537  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
538  stateSetSpeedNed_f(&speed);
539 
540  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
541  stateSetAccelNed_f(&accel);
542 }
543 
548 {
549  // init position
550 #if MEKF_WIND_USE_UTM
551  struct UtmCoor_f utm0;
552  utm0.north = (float)nav_utm_north0;
553  utm0.east = (float)nav_utm_east0;
554  utm0.alt = GROUND_ALT;
555  utm0.zone = nav_utm_zone0;
557  stateSetPositionUtm_f(&utm0);
558 #else
559  struct LlaCoor_i llh_nav0;
560  llh_nav0.lat = NAV_LAT0;
561  llh_nav0.lon = NAV_LON0;
562  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
563  struct EcefCoor_i ecef_nav0;
564  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
565  struct LtpDef_i ltp_def;
566  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
567  ltp_def.hmsl = NAV_ALT0;
568  stateSetLocalOrigin_i(&ltp_def);
569 #endif
570 
571  // reset flags
572  ins_mekf_wind.is_aligned = false;
573  ins_mekf_wind.reset = false;
576 
577  // aligner
578 #if USE_AHRS_ALIGNER
580 #endif
581 
582  // init filter
584  const struct FloatVect3 mag_h = { INS_H_X, INS_H_Y, INS_H_Z };
585  ins_mekf_wind_set_mag_h(&mag_h);
586 
587  // Bind to ABI messages
588  AbiBindMsgBARO_ABS(INS_MEKF_WIND_BARO_ID, &baro_ev, baro_cb);
590  AbiBindMsgAIRSPEED(INS_MEKF_WIND_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
591  AbiBindMsgINCIDENCE(INS_MEKF_WIND_INCIDENCE_ID, &incidence_ev, incidence_cb);
592  AbiBindMsgIMU_MAG_INT32(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb);
593  AbiBindMsgIMU_GYRO_INT32(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
594  AbiBindMsgIMU_ACCEL_INT32(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
595  AbiBindMsgIMU_LOWPASSED(INS_MEKF_WIND_IMU_ID, &aligner_ev, aligner_cb);
596  AbiBindMsgBODY_TO_IMU_QUAT(INS_MEKF_WIND_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
597  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
598  AbiBindMsgGPS(INS_MEKF_WIND_GPS_ID, &gps_ev, gps_cb);
599 
600 #if PERIODIC_TELEMETRY
601  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler);
602  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
603  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
605 #endif
606 
607  // init log
608 #if LOG_MEKF_WIND && SITL
609  // open log file for writing
610  // path should be specified in airframe file
611  uint32_t counter = 0;
612  char filename[512];
613  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), counter);
614  // check availale name
615  while ((pprzLogFile = fopen(filename, "r"))) {
616  fclose(pprzLogFile);
617  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), ++counter);
618  }
619  pprzLogFile = fopen(filename, "w");
620  if (pprzLogFile == NULL) {
621  printf("Failed to open WE log file '%s'\n",filename);
622  } else {
623  printf("Opening WE log file '%s'\n",filename);
624  }
625 #endif
626 
627 }
628 
634 {
635 #if MEKF_WIND_USE_UTM
636  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
637  // reset state UTM ref
639 #else
640  struct LtpDef_i ltp_def;
641  ltp_def_from_ecef_i(&ltp_def, &gps.ecef_pos);
642  ltp_def.hmsl = gps.hmsl;
643  stateSetLocalOrigin_i(&ltp_def);
644 #endif
646  //ins_mekf_wind.gps_initialized = false;
647 }
648 
650 {
651 #if MEKF_WIND_USE_UTM
652  struct UtmCoor_f utm = state.utm_origin_f;
653  utm.alt = gps.hmsl / 1000.0f;
655 #else
656  struct LlaCoor_i lla = {
658  .lon = state.ned_origin_i.lla.lon,
659  .alt = gps.lla_pos.alt
660  };
661  struct LtpDef_i ltp_def;
662  ltp_def_from_lla_i(&ltp_def, &lla);
663  ltp_def.hmsl = gps.hmsl;
664  stateSetLocalOrigin_i(&ltp_def);
665 #endif
666 }
667 
static uint32_t last_imu_stamp
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
#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.
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)
float baro_alt
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:134
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.
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
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
Air Data interface.
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:86
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.
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
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
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:390
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:71
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
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
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
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.