Paparazzi UAS  v6.3_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/ins/ins.h"
35 #include "modules/core/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 #if FIXEDWING_FIRMWARE
44 #endif
45 
46 #ifndef INS_MEKF_WIND_FILTER_ID
47 #define INS_MEKF_WIND_FILTER_ID 3
48 #endif
49 
51 
53 static struct FloatVect3 ins_mekf_wind_accel;
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 
210 
211 
212 static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
213 {
214  static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
215  static float alpha = 10.0f;
216  static int32_t i = 1;
217  static float baro_moy = 0.0f;
218  static float baro_prev = 0.0f;
219 
221  // try to find a stable qfe
222  // TODO generic function in pprz_isa ?
223  if (i == 1) {
224  baro_moy = pressure;
225  baro_prev = pressure;
226  }
227  baro_moy = (baro_moy * (i - 1) + pressure) / i;
228  alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
229  baro_prev = baro_moy;
230  // test stop condition
231  if (fabs(alpha) < 0.1f) {
232  ins_qfe = baro_moy;
234  }
235  if (i == 250) {
236  ins_qfe = pressure;
238  }
239  i++;
240  } else { /* normal update with baro measurement */
242  float baro_alt = -pprz_isa_height_of_pressure(pressure, ins_qfe); // Z down
244 
245 #if LOG_MEKF_WIND
246  if (LogFileIsOpen()) {
247  PrintLog(pprzLogFile, "%.3f baro %.3f \n", get_sys_time_float(), baro_alt);
248  }
249 #endif
250  }
251  }
252 }
253 
254 static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pdyn)
255 {
257  float airspeed = tas_from_dynamic_pressure(pdyn);
259 
260 #if LOG_MEKF_WIND
261  if (LogFileIsOpen()) {
262  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
263  }
264 #endif
265  }
266 }
267 
268 static void airspeed_cb(uint8_t __attribute__((unused)) sender_id, float airspeed)
269 {
272 
273 #if LOG_MEKF_WIND
274  if (LogFileIsOpen()) {
275  PrintLog(pprzLogFile, "%.3f airspeed %.3f\n", get_sys_time_float(), airspeed);
276  }
277 #endif
278  }
279 }
280 
281 static void incidence_cb(uint8_t __attribute__((unused)) sender_id, uint8_t flag, float aoa, float sideslip)
282 {
283  if (ins_mekf_wind.is_aligned && bit_is_set(flag, 0) && bit_is_set(flag, 1)) {
284  ins_mekf_wind_update_incidence(aoa, sideslip);
285 
286 #if LOG_MEKF_WIND
287  if (LogFileIsOpen()) {
288  PrintLog(pprzLogFile, "%.3f incidence %.3f %.3f\n", get_sys_time_float(), aoa, sideslip);
289  }
290 #endif
291  }
292 }
293 
299 static void gyro_cb(uint8_t sender_id __attribute__((unused)),
300  uint32_t stamp, struct Int32Rates *gyro)
301 {
303  struct FloatRates gyro_body;
304  RATES_FLOAT_OF_BFP(gyro_body, *gyro);
305 
306 #if USE_AUTO_INS_FREQ || !defined(INS_PROPAGATE_FREQUENCY)
307  PRINT_CONFIG_MSG("Calculating dt for INS MEKF_WIND propagation.")
308 
309  if (last_imu_stamp > 0) {
310  float dt = (float)(stamp - last_imu_stamp) * 1e-6;
313  } else {
315  }
316  }
317 #else
318  PRINT_CONFIG_MSG("Using fixed INS_PROPAGATE_FREQUENCY for INS MEKF_WIND propagation.")
319  PRINT_CONFIG_VAR(INS_PROPAGATE_FREQUENCY)
320  const float dt = 1. / (INS_PROPAGATE_FREQUENCY);
323  } else {
325  }
326 #endif
327 
328  // update state interface
330 
331 #if LOG_MEKF_WIND
332  if (LogFileIsOpen()) {
333  float time = get_sys_time_float();
334 
335  PrintLog(pprzLogFile,
336  "%.3f gyro_accel %.3f %.3f %.3f %.3f %.3f %.3f \n",
337  time,
338  gyro_body.p, gyro_body.q, gyro_body.r,
342 
343  struct FloatEulers eulers;
344  struct FloatQuat quat = ins_mekf_wind_get_quat();
345  float_eulers_of_quat(&eulers, &quat);
346  struct FloatRates rates = ins_mekf_wind_get_body_rates();
347  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
348  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
349  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
352  float bb = ins_mekf_wind_get_baro_bias();
353  struct NedCoor_f wind = ins_mekf_wind_get_wind_ned();
354  float airspeed = ins_mekf_wind_get_airspeed_norm();
355  PrintLog(pprzLogFile,
356  "%.3f output %.4f %.4f %.4f %.3f %.3f %.3f ",
357  time,
358  eulers.phi, eulers.theta, eulers.psi,
359  rates.p, rates.q, rates.r);
360  PrintLog(pprzLogFile,
361  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f ",
362  pos.x, pos.y, pos.z,
363  speed.x, speed.y, speed.z,
364  accel.x, accel.y, accel.z);
365  PrintLog(pprzLogFile,
366  "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
367  ab.x, ab.y, ab.z, rb.p, rb.q, rb.r, bb,
368  wind.x, wind.y, wind.z, airspeed);
369  }
370 #endif
371  }
372 
373  /* timestamp in usec when last callback was received */
374  last_imu_stamp = stamp;
375 }
376 
377 static void accel_cb(uint8_t sender_id __attribute__((unused)),
378  uint32_t stamp __attribute__((unused)),
379  struct Int32Vect3 *accel)
380 {
382 }
383 
384 static void mag_cb(uint8_t sender_id __attribute__((unused)),
385  uint32_t stamp __attribute__((unused)),
386  struct Int32Vect3 *mag)
387 {
389  struct FloatVect3 mag_body;
390  MAGS_FLOAT_OF_BFP(mag_body, *mag);
391 
392  // only correct attitude if GPS is not initialized
394 
395 #if LOG_MEKF_WIND
396  if (LogFileIsOpen()) {
397  PrintLog(pprzLogFile,
398  "%.3f magneto %.3f %.3f %.3f\n",
400  mag_body.x, mag_body.y, mag_body.z);
401  }
402 #endif
403  }
404 }
405 
406 static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
407  uint32_t stamp __attribute__((unused)),
408  struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
409  struct Int32Vect3 *lp_mag)
410 {
411  if (!ins_mekf_wind.is_aligned) {
412  struct FloatRates gyro_body;
413  RATES_FLOAT_OF_BFP(gyro_body, *lp_gyro);
414 
415  struct FloatVect3 accel_body;
416  ACCELS_FLOAT_OF_BFP(accel_body, *lp_accel);
417 
418  struct FloatVect3 mag_body;
419  MAGS_FLOAT_OF_BFP(mag_body, *lp_mag);
420 
421  struct FloatQuat quat;
422  ahrs_float_get_quat_from_accel_mag(&quat, &accel_body, &mag_body);
423  ins_mekf_wind_align(&gyro_body, &quat);
424  // udate state interface
426 
427  // ins and ahrs are now running
428  ins_mekf_wind.is_aligned = true;
429  }
430 }
431 
432 static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
433 {
435 }
436 
437 static void gps_cb(uint8_t sender_id __attribute__((unused)),
438  uint32_t stamp __attribute__((unused)),
439  struct GpsState *gps_s)
440 {
441  if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) {
442 
443 #if FIXEDWING_FIRMWARE
444  if (state.utm_initialized_f) {
445  struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
446  struct NedCoor_f pos, speed;
447  // position (local ned)
448  pos.x = utm.north - state.utm_origin_f.north;
449  pos.y = utm.east - state.utm_origin_f.east;
450  pos.z = state.utm_origin_f.alt - utm.alt;
451  // speed
452  speed = ned_vel_float_from_gps(gps_s);
457  }
458  ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
459 
460 #if LOG_MEKFW_FILTER
461  if (LogFileIsOpen()) {
462  PrintLog(pprzLogFile,
463  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
465  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
466  }
467 #endif
468  }
469 
470 #else
471  if (state.ned_initialized_f) {
472  struct NedCoor_f pos, speed;
473  struct NedCoor_i gps_pos_cm_ned, ned_pos;
474  struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s);
475  ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i);
477  NED_FLOAT_OF_BFP(pos, ned_pos);
478  struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s);
479  ned_of_ecef_vect_f(&speed, &state.ned_origin_f, &ecef_vel);
480  ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed));
481 
482 #if LOG_MEKFW_FILTER
483  if (LogFileIsOpen()) {
484  PrintLog(pprzLogFile,
485  "%.3f gps %.3f %.3f %.3f %.3f %.3f %.3f \n",
487  pos.x, pos.y, pos.z, speed.x, speed.y, speed.z);
488  }
489 #endif
490  }
491 #endif
492  }
493 }
494 
498 static void set_state_from_ins(void)
499 {
500  struct FloatQuat quat = ins_mekf_wind_get_quat();
502 
503  struct FloatRates rates = ins_mekf_wind_get_body_rates();
504  stateSetBodyRates_f(&rates);
505 
506  struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
507  stateSetPositionNed_f(&pos);
508 
509  struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
510  stateSetSpeedNed_f(&speed);
511 
512  struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
513  stateSetAccelNed_f(&accel);
514 }
515 
520 {
521  // init position
522 #if FIXEDWING_FIRMWARE
523  struct UtmCoor_f utm0;
524  utm0.north = (float)nav_utm_north0;
525  utm0.east = (float)nav_utm_east0;
526  utm0.alt = GROUND_ALT;
527  utm0.zone = nav_utm_zone0;
529  stateSetPositionUtm_f(&utm0);
530 #else
531  struct LlaCoor_i llh_nav0;
532  llh_nav0.lat = NAV_LAT0;
533  llh_nav0.lon = NAV_LON0;
534  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
535  struct EcefCoor_i ecef_nav0;
536  ecef_of_lla_i(&ecef_nav0, &llh_nav0);
537  struct LtpDef_i ltp_def;
538  ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
539  ltp_def.hmsl = NAV_ALT0;
541 #endif
542 
543  // reset flags
544  ins_mekf_wind.is_aligned = false;
545  ins_mekf_wind.reset = false;
548 
549  // aligner
550 #if USE_AHRS_ALIGNER
552 #endif
553 
554  // init filter
556  const struct FloatVect3 mag_h = { INS_H_X, INS_H_Y, INS_H_Z };
557  ins_mekf_wind_set_mag_h(&mag_h);
558 
559  // Bind to ABI messages
560  AbiBindMsgBARO_ABS(INS_MEKF_WIND_BARO_ID, &baro_ev, baro_cb);
562  AbiBindMsgAIRSPEED(INS_MEKF_WIND_AIRSPEED_ID, &airspeed_ev, airspeed_cb);
563  AbiBindMsgINCIDENCE(INS_MEKF_WIND_INCIDENCE_ID, &incidence_ev, incidence_cb);
564  AbiBindMsgIMU_MAG(INS_MEKF_WIND_MAG_ID, &mag_ev, mag_cb);
565  AbiBindMsgIMU_GYRO(INS_MEKF_WIND_IMU_ID, &gyro_ev, gyro_cb);
566  AbiBindMsgIMU_ACCEL(INS_MEKF_WIND_IMU_ID, &accel_ev, accel_cb);
567  AbiBindMsgIMU_LOWPASSED(ABI_BROADCAST, &aligner_ev, aligner_cb);
568  AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
569  AbiBindMsgGPS(INS_MEKF_WIND_GPS_ID, &gps_ev, gps_cb);
570 
571 #if PERIODIC_TELEMETRY
572  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AHRS_EULER, send_euler);
573  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_WIND_INFO_RET, send_wind);
574  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STATE_FILTER_STATUS, send_filter_status);
576 #endif
577 
578  // init log
579 #if LOG_MEKF_WIND && SITL
580  // open log file for writing
581  // path should be specified in airframe file
582  uint32_t counter = 0;
583  char filename[512];
584  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), counter);
585  // check availale name
586  while ((pprzLogFile = fopen(filename, "r"))) {
587  fclose(pprzLogFile);
588  snprintf(filename, 512, "%s/mekf_wind_%05d.csv", STRINGIFY(MEKF_WIND_LOG_PATH), ++counter);
589  }
590  pprzLogFile = fopen(filename, "w");
591  if (pprzLogFile == NULL) {
592  printf("Failed to open WE log file '%s'\n",filename);
593  } else {
594  printf("Opening WE log file '%s'\n",filename);
595  }
596 #endif
597 
598 }
599 
605 {
606 #if FIXEDWING_FIRMWARE
607  struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
608  // reset state UTM ref
610 #else
611  struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps);
612  struct LlaCoor_i lla_pos = lla_int_from_gps(&gps);
613  struct LtpDef_i ltp_def;
614  ltp_def_from_ecef_i(&ltp_def, &ecef_pos);
615  ltp_def.lla.alt = lla_pos.alt;
616  ltp_def.hmsl = gps.hmsl;
618 #endif
620  //ins_mekf_wind.gps_initialized = false;
621 }
622 
624 {
625 #if FIXEDWING_FIRMWARE
626  struct UtmCoor_f utm = state.utm_origin_f;
627  utm.alt = gps.hmsl / 1000.0f;
629 #else
630  struct LlaCoor_i lla = {
632  .lon = state.ned_origin_i.lla.lon,
633  .alt = gps.lla_pos.alt
634  };
635  struct LtpDef_i ltp_def;
637  ltp_def.hmsl = gps.hmsl;
639 #endif
640 }
641 
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:57
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
void ahrs_aligner_init(void)
Definition: ahrs_aligner.c:84
Interface to align the AHRS via low-passed measurements at startup.
Utility functions for floating point AHRS implementations.
static void ahrs_float_get_quat_from_accel_mag(struct FloatQuat *q, struct FloatVect3 *accel, struct FloatVect3 *mag)
float tas_from_dynamic_pressure(float q)
Calculate true airspeed from dynamic pressure.
Definition: air_data.c:418
float tas_from_eas(float eas)
Calculate true airspeed from equivalent airspeed.
Definition: air_data.c:389
Air Data interface.
int32_t counter
float baro_alt
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
int32_t nav_utm_east0
Definition: common_nav.c:43
uint8_t nav_utm_zone0
Definition: common_nav.c:45
int32_t nav_utm_north0
Definition: common_nav.c:44
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:539
struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ned velocity (float) Converted on the fly if not available.
Definition: gps.c:506
struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s)
Get GPS ecef velocity (float) Converted on the fly if not available.
Definition: gps.c:471
struct GpsState gps
global GPS state
Definition: gps.c:69
struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s)
Get GPS lla (integer) Converted on the fly if not available.
Definition: gps.c:426
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:455
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:95
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
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 r
in rad/s
float theta
in radians
float psi
in radians
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
euler angles
Roation quaternion.
angular rates
#define MAGS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:807
#define RATES_FLOAT_OF_BFP(_rf, _ri)
Definition: pprz_algebra.h:759
#define ACCELS_FLOAT_OF_BFP(_ef, _ei)
Definition: pprz_algebra.h:795
#define INT32_POS_OF_CM_DEN
#define INT32_POS_OF_CM_NUM
#define INT32_VECT3_SCALE_2(_a, _b, _num, _den)
angular rates
#define ENU_OF_TO_NED(_po, _pi)
Definition: pprz_geodetic.h:41
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
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)
#define NED_FLOAT_OF_BFP(_o, _i)
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 ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
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 PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
static void stateSetAccelNed_f(struct NedCoor_f *ned_accel)
Set acceleration in NED coordinates (float).
Definition: state.h:1002
static void stateSetNedToBodyQuat_f(struct FloatQuat *ned_to_body_quat)
Set vehicle body attitude from quaternion (float).
Definition: state.h:1093
struct State state
Definition: state.c:36
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
bool ned_initialized_f
True if local float coordinate frame is initialsed.
Definition: state.h:223
bool utm_initialized_f
True if utm origin (float) coordinate frame is initialsed.
Definition: state.h:236
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
static void stateSetPositionUtm_f(struct UtmCoor_f *utm_pos)
Set position from UTM coordinates (float).
Definition: state.h:582
static void stateSetLocalOrigin_i(struct LtpDef_i *ltp_def)
Set the local (flat earth) coordinate frame origin (int).
Definition: state.h:457
static void stateSetLocalUtmOrigin_f(struct UtmCoor_f *utm_def)
Set the local (flat earth) coordinate frame origin from UTM (float).
Definition: state.h:477
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
Definition: state.h:1181
static void stateSetSpeedNed_f(struct NedCoor_f *ned_speed)
Set ground speed in local NED coordinates (float).
Definition: state.h:809
Integrated Navigation System interface.
float ins_mekf_wind_get_baro_bias(void)
struct NedCoor_f ins_mekf_wind_get_wind_ned(void)
void ins_mekf_wind_update_incidence(float aoa, float aos)
void ins_mekf_wind_set_pos_ned(struct NedCoor_f *p)
float ins_mekf_wind_get_airspeed_norm(void)
void ins_mekf_wind_update_baro(float baro_alt)
struct NedCoor_f ins_mekf_wind_get_accel_ned(void)
void ins_mekf_wind_init(void)
Init function.
void ins_mekf_wind_set_mag_h(const struct FloatVect3 *mag_h)
struct NedCoor_f ins_mekf_wind_get_pos_ned(void)
Getter/Setter functions.
struct FloatRates ins_mekf_wind_get_body_rates(void)
void ins_mekf_wind_set_speed_ned(struct NedCoor_f *s)
void ins_mekf_wind_update_mag(struct FloatVect3 *mag, bool attitude_only)
struct FloatVect3 ins_mekf_wind_get_accel_bias(void)
struct FloatQuat ins_mekf_wind_get_quat(void)
void ins_mekf_wind_propagate(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
Full INS propagation.
void ins_mekf_wind_align(struct FloatRates *gyro_bias, struct FloatQuat *quat)
void ins_mekf_wind_propagate_ahrs(struct FloatRates *gyro, struct FloatVect3 *acc, float dt)
AHRS-only propagation + accel correction.
struct FloatRates ins_mekf_wind_get_rates_bias(void)
struct NedCoor_f ins_mekf_wind_get_speed_ned(void)
void ins_mekf_wind_update_airspeed(float airspeed)
void ins_mekf_wind_update_pos_speed(struct FloatVect3 *pos, struct FloatVect3 *speed)
Multiplicative Extended Kalman Filter in rotation matrix formulation.
static void airspeed_cb(uint8_t sender_id, float airspeed)
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
void ins_reset_local_origin(void)
local implemetation of the ins_reset functions
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
static abi_event airspeed_ev
#define INS_MEKF_WIND_IMU_ID
IMU (gyro, accel)
static void send_wind(struct transport_tx *trans, struct link_device *dev)
static abi_event mag_ev
static abi_event accel_ev
static void gyro_cb(uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
Call ins_mekf_wind_propagate on new gyro measurements.
static void mag_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *mag)
#define INS_MEKF_WIND_INCIDENCE_ID
incidence angles
static abi_event pressure_diff_ev
static abi_event gyro_ev
#define INS_MEKF_WIND_FILTER_ID
#define INS_MEKF_WIND_AIRSPEED_ID
airspeed (Pitot tube)
static abi_event aligner_ev
static uint32_t last_imu_stamp
void ins_mekf_wind_wrapper_init(void)
Init function.
static abi_event baro_ev
static abi_event geo_mag_ev
static void set_state_from_ins(void)
update state interface
static void send_filter_status(struct transport_tx *trans, struct link_device *dev)
static void incidence_cb(uint8_t sender_id, uint8_t flag, float aoa, float sideslip)
static void baro_cb(uint8_t sender_id, uint32_t stamp, float pressure)
static struct FloatVect3 ins_mekf_wind_accel
last accel measurement
void ins_reset_altitude_ref(void)
INS altitude reference reset.
static abi_event incidence_ev
#define INS_MEKF_WIND_BARO_ID
baro
static void accel_cb(uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
struct InsMekfWind ins_mekf_wind
static void geo_mag_cb(uint8_t sender_id, struct FloatVect3 *h)
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)
logging functions
#define INS_MEKF_WIND_GPS_ID
ABI binding for gps data.
static void pressure_diff_cb(uint8_t sender_id, float pdyn)
#define INS_MEKF_WIND_MAG_ID
magnetometer
static abi_event gps_ev
Paparazzi specific wrapper to run MEKF-Wind INS filter.
#define ins_mekf_wind_wrapper_Reset(_v)
filter structure
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Fixedwing Navigation library.
void ned_of_ecef_vect_f(struct NedCoor_f *ned, struct LtpDef_f *def, struct EcefCoor_f *ecef)
float y
in meters
float x
in meters
float alt
in meters (above WGS84 reference ellipsoid or above MSL)
uint8_t zone
UTM zone number.
float z
in meters
float x
in meters
float east
in meters
float north
in meters
float z
in meters
float y
in meters
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in North East Down coordinates Units: meters
position in UTM coordinates Units: meters
Paparazzi atmospheric pressure conversion utilities.
FileDes pprzLogFile
Definition: sdlog_chibios.c:86
API to get/set the generic vehicle states.
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
Definition: telemetry.h:66
uint16_t val[TCOUPLE_NB]
float alpha
Definition: textons.c:107
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98