PX4 Firmware
PX4 Autopilot Software http://px4.io
estimator_interface.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name ECL nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file estimator_interface.h
36  * Definition of base class for attitude estimators
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  *
40  */
41 
42 #pragma once
43 
44 #include <ecl.h>
45 #include "common.h"
46 #include "RingBuffer.h"
47 #include "AlphaFilter.hpp"
48 
49 #include <geo/geo.h>
50 #include <matrix/math.hpp>
51 #include <mathlib/mathlib.h>
52 
53 using namespace estimator;
54 
56 {
57 
58 public:
60 
61  EstimatorInterface() = default;
62  virtual ~EstimatorInterface() = default;
63 
64  virtual bool init(uint64_t timestamp) = 0;
65  virtual void reset(uint64_t timestamp) = 0;
66  virtual void resetStates() = 0;
67  virtual void resetStatesAndCovariances() = 0;
68  virtual bool update() = 0;
69 
70  // gets the innovations of velocity and position measurements
71  // 0-2 vel, 3-5 pos
72  virtual void get_vel_pos_innov(float vel_pos_innov[6]) = 0;
73 
74  // gets the innovations for of the NE auxiliary velocity measurement
75  virtual void get_aux_vel_innov(float aux_vel_innov[2]) = 0;
76 
77  // gets the innovations of the earth magnetic field measurements
78  virtual void get_mag_innov(float mag_innov[3]) = 0;
79 
80  // gets the innovation of airspeed measurement
81  virtual void get_airspeed_innov(float *airspeed_innov) = 0;
82 
83  // gets the innovation of the synthetic sideslip measurement
84  virtual void get_beta_innov(float *beta_innov) = 0;
85 
86  // gets the innovations of the heading measurement
87  virtual void get_heading_innov(float *heading_innov) = 0;
88 
89  // gets the innovation variances of velocity and position measurements
90  // 0-2 vel, 3-5 pos
91  virtual void get_vel_pos_innov_var(float vel_pos_innov_var[6]) = 0;
92 
93  // gets the innovation variances of the earth magnetic field measurements
94  virtual void get_mag_innov_var(float mag_innov_var[3]) = 0;
95 
96  // gets the innovation variance of the airspeed measurement
97  virtual void get_airspeed_innov_var(float *get_airspeed_innov_var) = 0;
98 
99  // gets the innovation variance of the synthetic sideslip measurement
100  virtual void get_beta_innov_var(float *get_beta_innov_var) = 0;
101 
102  // gets the innovation variance of the heading measurement
103  virtual void get_heading_innov_var(float *heading_innov_var) = 0;
104 
105  virtual void get_state_delayed(float *state) = 0;
106 
107  virtual void get_wind_velocity(float *wind) = 0;
108 
109  virtual void get_wind_velocity_var(float *wind_var) = 0;
110 
111  virtual void get_true_airspeed(float *tas) = 0;
112 
113  // gets the variances for the NED velocity states
114  virtual void get_vel_var(Vector3f &vel_var) = 0;
115 
116  // gets the variances for the NED position states
117  virtual void get_pos_var(Vector3f &pos_var) = 0;
118 
119  // gets the innovation variance of the flow measurement
120  virtual void get_flow_innov_var(float flow_innov_var[2]) = 0;
121 
122  // gets the innovation of the flow measurement
123  virtual void get_flow_innov(float flow_innov[2]) = 0;
124 
125  // gets the innovation variance of the drag specific force measurement
126  virtual void get_drag_innov_var(float drag_innov_var[2]) = 0;
127 
128  // gets the innovation of the drag specific force measurement
129  virtual void get_drag_innov(float drag_innov[2]) = 0;
130 
131  virtual void getHaglInnovVar(float *hagl_innov_var) = 0;
132  virtual void getHaglInnov(float *hagl_innov) = 0;
133  //[[deprecated("Replaced by getHaglInnovVar")]]
134  void get_hagl_innov_var(float *hagl_innov_var) { getHaglInnovVar(hagl_innov_var); }
135  //[[deprecated("Replaced by getHaglInnov")]]
136  void get_hagl_innov(float *hagl_innov) { getHaglInnov(hagl_innov); }
137 
138  // return an array containing the output predictor angular, velocity and position tracking
139  // error magnitudes (rad), (m/s), (m)
140  virtual void get_output_tracking_error(float error[3]) = 0;
141 
142  /*
143  Returns following IMU vibration metrics in the following array locations
144  0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle)
145  1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle)
146  2 : Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity)
147  */
148  virtual void get_imu_vibe_metrics(float vibe[3]) = 0;
149 
150  /*
151  First argument returns GPS drift metrics in the following array locations
152  0 : Horizontal position drift rate (m/s)
153  1 : Vertical position drift rate (m/s)
154  2 : Filtered horizontal velocity (m/s)
155  Second argument returns true when IMU movement is blocking the drift calculation
156  Function returns true if the metrics have been updated and not returned previously by this function
157  */
158  virtual bool get_gps_drift_metrics(float drift[3], bool *blocked) = 0;
159 
160  // get the ekf WGS-84 origin position and height and the system time it was last set
161  // return true if the origin is valid
162  virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
163 
164  // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
165  virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
166 
167  // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
168  virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
169 
170  // get the 1-sigma horizontal and vertical velocity uncertainty
171  virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0;
172 
173  // get the vehicle control limits required by the estimator to keep within sensor limitations
174  virtual void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) = 0;
175 
176  // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
177  virtual bool collect_gps(const gps_message &gps) = 0;
178 
179  // accumulate and downsample IMU data to the EKF prediction rate
180  virtual bool collect_imu(const imuSample &imu) = 0;
181 
182  // set delta angle imu data
183  void setIMUData(const imuSample &imu_sample);
184 
185  // legacy interface for compatibility (2018-09-14)
186  void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]);
187 
188  // set magnetometer data
189  void setMagData(uint64_t time_usec, float (&data)[3]);
190 
191  // set gps data
192  void setGpsData(uint64_t time_usec, const gps_message &gps);
193 
194  // set baro data
195  void setBaroData(uint64_t time_usec, float data);
196 
197  // set airspeed data
198  void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas);
199 
200  // set range data
201  void setRangeData(uint64_t time_usec, float data, int8_t quality);
202 
203  // set optical flow data
204  // if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
205  void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
206 
207  // set external vision position and attitude data
208  void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata);
209 
210  // set auxiliary velocity data
211  void setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2]);
212 
213  // return a address to the parameters struct
214  // in order to give access to the application
215  parameters *getParamHandle() {return &_params;}
216 
217  // set vehicle landed status data
218  void set_in_air_status(bool in_air) {_control_status.flags.in_air = in_air;}
219 
220  /*
221  Reset all IMU bias states and covariances to initial alignment values.
222  Use when the IMU sensor has changed.
223  Returns true if reset performed, false if rejected due to less than 10 seconds lapsed since last reset.
224  */
225  virtual bool reset_imu_bias() = 0;
226 
227  // return true if the attitude is usable
228  bool attitude_valid() { return ISFINITE(_output_new.quat_nominal(0)) && _control_status.flags.tilt_align; }
229 
230  // get vehicle landed status data
231  bool get_in_air_status() {return _control_status.flags.in_air;}
232 
233  // get wind estimation status
234  bool get_wind_status() { return _control_status.flags.wind; }
235 
236  // set vehicle is fixed wing status
237  void set_is_fixed_wing(bool is_fixed_wing) {_control_status.flags.fixed_wing = is_fixed_wing;}
238 
239  // set flag if synthetic sideslip measurement should be fused
240  void set_fuse_beta_flag(bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
241 
242  // set flag if static pressure rise due to ground effect is expected
243  // use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
244  // flag will clear after GNDEFFECT_TIMEOUT uSec
245  void set_gnd_effect_flag(bool gnd_effect)
246  {
247  _control_status.flags.gnd_effect = gnd_effect;
248  _time_last_gnd_effect_on = _time_last_imu;
249  }
250 
251  // set air density used by the multi-rotor specific drag force fusion
252  void set_air_density(float air_density) {_air_density = air_density;}
253 
254  // set sensor limitations reported by the rangefinder
255  void set_rangefinder_limits(float min_distance, float max_distance)
256  {
257  _rng_valid_min_val = min_distance;
258  _rng_valid_max_val = max_distance;
259  }
260 
261  // set sensor limitations reported by the optical flow sensor
262  void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
263  {
264  _flow_max_rate = max_flow_rate;
265  _flow_min_distance = min_distance;
266  _flow_max_distance = max_distance;
267  }
268 
269  // return true if the global position estimate is valid
270  virtual bool global_position_is_valid() = 0;
271 
272  // return true if the EKF is dead reckoning the position using inertial data only
273  bool inertial_dead_reckoning() {return _is_dead_reckoning;}
274 
275  virtual bool isTerrainEstimateValid() const = 0;
276  //[[deprecated("Replaced by isTerrainEstimateValid")]]
277  bool get_terrain_valid() { return isTerrainEstimateValid(); }
278 
279  // get the estimated terrain vertical position relative to the NED origin
280  virtual void getTerrainVertPos(float *ret) = 0;
281  //[[deprecated("Replaced by getTerrainVertPos")]]
282  void get_terrain_vert_pos(float *ret) { getTerrainVertPos(ret); }
283 
284  // return true if the local position estimate is valid
285  bool local_position_is_valid();
286 
287  const matrix::Quatf &get_quaternion() const { return _output_new.quat_nominal; }
288 
289  // return the quaternion defining the rotation from the EKF to the External Vision reference frame
290  virtual void get_ev2ekf_quaternion(float *quat) = 0;
291 
292  // get the velocity of the body frame origin in local NED earth frame
293  void get_velocity(float *vel)
294  {
295  Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
296 
297  for (unsigned i = 0; i < 3; i++) {
298  vel[i] = vel_earth(i);
299  }
300  }
301 
302  // get the NED velocity derivative in earth frame
303  void get_vel_deriv_ned(float *vel_deriv)
304  {
305  for (unsigned i = 0; i < 3; i++) {
306  vel_deriv[i] = _vel_deriv_ned(i);
307  }
308  }
309 
310  // get the derivative of the vertical position of the body frame origin in local NED earth frame
311  void get_pos_d_deriv(float *pos_d_deriv)
312  {
313  float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
314  *pos_d_deriv = var;
315  }
316 
317  // get the position of the body frame origin in local NED earth frame
318  void get_position(float *pos)
319  {
320  // rotate the position of the IMU relative to the boy origin into earth frame
321  Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
322 
323  // subtract from the EKF position (which is at the IMU) to get position at the body origin
324  for (unsigned i = 0; i < 3; i++) {
325  pos[i] = _output_new.pos(i) - pos_offset_earth(i);
326  }
327  }
328  void copy_timestamp(uint64_t *time_us)
329  {
330  *time_us = _time_last_imu;
331  }
332 
333  // Get the value of magnetic declination in degrees to be saved for use at the next startup
334  // Returns true when the declination can be saved
335  // At the next startup, set param.mag_declination_deg to the value saved
336  bool get_mag_decl_deg(float *val)
337  {
338  *val = 0.0f;
339  if (_NED_origin_initialised && (_params.mag_declination_source & MASK_SAVE_GEO_DECL)) {
340  *val = math::degrees(_mag_declination_gps);
341  return true;
342 
343  } else {
344  return false;
345  }
346  }
347 
348  virtual void get_accel_bias(float bias[3]) = 0;
349  virtual void get_gyro_bias(float bias[3]) = 0;
350 
351  // get EKF mode status
352  void get_control_mode(uint32_t *val)
353  {
354  *val = _control_status.value;
355  }
356 
357  // get EKF internal fault status
358  void get_filter_fault_status(uint16_t *val)
359  {
360  *val = _fault_status.value;
361  }
362 
363  bool isVehicleAtRest() const { return _vehicle_at_rest; }
364 
365  // get GPS check status
366  virtual void get_gps_check_status(uint16_t *val) = 0;
367 
368  // return the amount the local vertical position changed in the last reset and the number of reset events
369  virtual void get_posD_reset(float *delta, uint8_t *counter) = 0;
370 
371  // return the amount the local vertical velocity changed in the last reset and the number of reset events
372  virtual void get_velD_reset(float *delta, uint8_t *counter) = 0;
373 
374  // return the amount the local horizontal position changed in the last reset and the number of reset events
375  virtual void get_posNE_reset(float delta[2], uint8_t *counter) = 0;
376 
377  // return the amount the local horizontal velocity changed in the last reset and the number of reset events
378  virtual void get_velNE_reset(float delta[2], uint8_t *counter) = 0;
379 
380  // return the amount the quaternion has changed in the last reset and the number of reset events
381  virtual void get_quat_reset(float delta_quat[4], uint8_t *counter) = 0;
382 
383  // get EKF innovation consistency check status information comprising of:
384  // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
385  // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
386  // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
387  // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
388  virtual void get_innovation_test_status(uint16_t *status, float *mag, float *vel, float *pos, float *hgt, float *tas, float *hagl, float *beta) = 0;
389 
390  // return a bitmask integer that describes which state estimates can be used for flight control
391  virtual void get_ekf_soln_status(uint16_t *status) = 0;
392 
393  // Getter for the average imu update period in s
394  float get_dt_imu_avg() const { return _dt_imu_avg; }
395 
396  // Getter for the imu sample on the delayed time horizon
398  {
399  return _imu_sample_delayed;
400  }
401 
402  // Getter for the baro sample on the delayed time horizon
404  {
405  return _baro_sample_delayed;
406  }
407 
408  void print_status();
409 
410  static constexpr unsigned FILTER_UPDATE_PERIOD_MS{8}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta
411  static constexpr float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
412 
413 protected:
414 
415  parameters _params; // filter parameters
416 
417  /*
418  OBS_BUFFER_LENGTH defines how many observations (non-IMU measurements) we can buffer
419  which sets the maximum frequency at which we can process non-IMU measurements. Measurements that
420  arrive too soon after the previous measurement will not be processed.
421  max freq (Hz) = (OBS_BUFFER_LENGTH - 1) / (IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_S)
422  This can be adjusted to match the max sensor data rate plus some margin for jitter.
423  */
424  uint8_t _obs_buffer_length{0};
425 
426  /*
427  IMU_BUFFER_LENGTH defines how many IMU samples we buffer which sets the time delay from current time to the
428  EKF fusion time horizon and therefore the maximum sensor time offset relative to the IMU that we can compensate for.
429  max sensor time offet (msec) = IMU_BUFFER_LENGTH * FILTER_UPDATE_PERIOD_MS
430  This can be adjusted to a value that is FILTER_UPDATE_PERIOD_MS longer than the maximum observation time delay.
431  */
432  uint8_t _imu_buffer_length{0};
433 
434  unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
435 
436  float _dt_imu_avg{0.0f}; // average imu update period in s
437 
438  imuSample _imu_sample_delayed{}; // captures the imu sample on the delayed time horizon
439 
440  // measurement samples capturing measurements on the delayed time horizon
441  magSample _mag_sample_delayed{};
442  baroSample _baro_sample_delayed{};
443  gpsSample _gps_sample_delayed{};
444  rangeSample _range_sample_delayed{};
445  airspeedSample _airspeed_sample_delayed{};
446  flowSample _flow_sample_delayed{};
447  extVisionSample _ev_sample_delayed{};
448  dragSample _drag_sample_delayed{};
449  dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate)
450  auxVelSample _auxvel_sample_delayed{};
451 
452  // Used by the multi-rotor specific drag force fusion
453  uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate
454  float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec)
455  float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3)
456 
457  // Sensor limitations
458  float _rng_valid_min_val{0.0f}; ///< minimum distance that the rangefinder can measure (m)
459  float _rng_valid_max_val{0.0f}; ///< maximum distance that the rangefinder can measure (m)
460  float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
461  float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m)
462  float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m)
463 
464  // Output Predictor
465  outputSample _output_sample_delayed{}; // filter output on the delayed time horizon
466  outputSample _output_new{}; // filter output on the non-delayed time horizon
467  outputVert _output_vert_delayed{}; // vertical filter output on the delayed time horizon
468  outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon
469  imuSample _imu_sample_new{}; // imu sample capturing the newest imu data
470  Matrix3f _R_to_earth_now; // rotation matrix from body to earth frame at current time
471  Vector3f _vel_imu_rel_body_ned; // velocity of IMU relative to body origin in NED earth frame
472  Vector3f _vel_deriv_ned; // velocity derivative at the IMU in NED earth frame (m/s/s)
473 
474  bool _imu_updated{false}; // true if the ekf should update (completed downsampling process)
475  bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized
476 
477  bool _NED_origin_initialised{false};
478  bool _gps_speed_valid{false};
479  float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin
480  float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin
481  struct map_projection_reference_s _pos_ref {}; // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
482  struct map_projection_reference_s _gps_pos_prev {}; // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
483  float _gps_alt_prev{0.0f}; // height from the previous GPS message (m)
484  float _gps_yaw_offset{0.0f}; // Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
485 
486  // innovation consistency check monitoring ratios
487  float _yaw_test_ratio{0.0f}; // yaw innovation consistency check ratio
488  float _mag_test_ratio[3] {}; // magnetometer XYZ innovation consistency check ratios
489  float _vel_pos_test_ratio[6] {}; // velocity and position innovation consistency check ratios
490  float _tas_test_ratio{0.0f}; // tas innovation consistency check ratio
491  float _terr_test_ratio{0.0f}; // height above terrain measurement innovation consistency check ratio
492  float _beta_test_ratio{0.0f}; // sideslip innovation consistency check ratio
493  float _drag_test_ratio[2] {}; // drag innovation consistency check ratio
494  innovation_fault_status_u _innov_check_fail_status{};
495 
496  bool _is_dead_reckoning{false}; // true if we are no longer fusing measurements that constrain horizontal velocity drift
497  bool _deadreckon_time_exceeded{true}; // true if the horizontal nav solution has been deadreckoning for too long and is invalid
498  bool _is_wind_dead_reckoning{false}; // true if we are navigationg reliant on wind relative measurements
499 
500  // IMU vibration and movement monitoring
501  Vector3f _delta_ang_prev; // delta angle from the previous IMU measurement
502  Vector3f _delta_vel_prev; // delta velocity from the previous IMU measurement
503  float _vibe_metrics[3] {}; // IMU vibration metrics
504  // [0] Level of coning vibration in the IMU delta angles (rad^2)
505  // [1] high frequency vibration level in the IMU delta angle data (rad)
506  // [2] high frequency vibration level in the IMU delta velocity data (m/s)
507  float _gps_drift_metrics[3] {}; // Array containing GPS drift metrics
508  // [0] Horizontal position drift rate (m/s)
509  // [1] Vertical position drift rate (m/s)
510  // [2] Filtered horizontal velocity (m/s)
511  bool _vehicle_at_rest{false}; // true when the vehicle is at rest
512  uint64_t _time_last_move_detect_us{0}; // timestamp of last movement detection event in microseconds
513  bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval
514 
515  // data buffer instances
528 
529  // observation buffer final allocation failed
530  bool _gps_buffer_fail{false};
531  bool _mag_buffer_fail{false};
532  bool _baro_buffer_fail{false};
533  bool _range_buffer_fail{false};
534  bool _airspeed_buffer_fail{false};
535  bool _flow_buffer_fail{false};
536  bool _ev_buffer_fail{false};
537  bool _drag_buffer_fail{false};
538  bool _auxvel_buffer_fail{false};
539 
540  uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds
541  uint64_t _time_last_gps{0}; // timestamp of last gps measurement in microseconds
542  uint64_t _time_last_mag{0}; // timestamp of last magnetometer measurement in microseconds
543  uint64_t _time_last_baro{0}; // timestamp of last barometer measurement in microseconds
544  uint64_t _time_last_range{0}; // timestamp of last range measurement in microseconds
545  uint64_t _time_last_airspeed{0}; // timestamp of last airspeed measurement in microseconds
546  uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
547  uint64_t _time_last_optflow{0};
548  uint64_t _time_last_gnd_effect_on{0}; //last time the baro ground effect compensation was turned on externally (uSec)
549  uint64_t _time_last_auxvel{0};
550 
551  fault_status_u _fault_status{};
552 
553  // allocate data buffers and initialize interface variables
554  bool initialise_interface(uint64_t timestamp);
555 
556  // free buffer memory
557  void unallocate_buffers();
558 
559  float _mag_declination_gps{0.0f}; // magnetic declination returned by the geo library using the last valid GPS position (rad)
560  float _mag_inclination_gps{0.0f}; // magnetic inclination returned by the geo library using the last valid GPS position (rad)
561  float _mag_strength_gps{0.0f}; // magnetic strength returned by the geo library using the last valid GPS position (T)
562 
563  // this is the current status of the filter control modes
564  filter_control_status_u _control_status{};
565 
566  // this is the previous status of the filter control modes - used to detect mode transitions
567  filter_control_status_u _control_status_prev{};
568 
569  // perform a vector cross product
570  Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2);
571 
572  // calculate the inverse rotation matrix from a quaternion rotation
573  Matrix3f quat_to_invrotmat(const Quatf &quat);
574 
575 };
Adapter / shim layer for system calls needed by ECL.
void get_hagl_innov(float *hagl_innov)
#define MASK_SAVE_GEO_DECL
set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library ...
Definition: common.h:185
static struct vehicle_status_s status
Definition: Commander.cpp:138
RingBuffer< outputSample > _output_buffer
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
static enum @74 state
RingBuffer< extVisionSample > _ext_vision_buffer
Definition of geo / math functions to perform geodesic calculations.
bool is_fixed_wing(const struct vehicle_status_s *current_status)
void get_terrain_vert_pos(float *ret)
void set_is_fixed_wing(bool is_fixed_wing)
SquareMatrix< float, 3 > Matrix3f
void set_fuse_beta_flag(bool fuse_beta)
void set_gnd_effect_flag(bool gnd_effect)
void print_status()
Definition: Commander.cpp:517
RingBuffer< outputVert > _output_vert_buffer
int reset(enum LPS22HB_BUS busid)
Reset the driver.
void get_velocity(float *vel)
constexpr T degrees(T radians)
Definition: Limits.hpp:91
Quaternion class.
Definition: Dcm.hpp:24
void get_control_mode(uint32_t *val)
RingBuffer< gpsSample > _gps_buffer
void init()
Activates/configures the hardware registers.
uint8_t * data
Definition: dataman.cpp:149
void get_vel_deriv_ned(float *vel_deriv)
void get_filter_fault_status(uint16_t *val)
RingBuffer< dragSample > _drag_buffer
RingBuffer< baroSample > _baro_buffer
parameters * getParamHandle()
float get_dt_imu_avg() const
static unsigned counter
Definition: safety.c:56
void get_hagl_innov_var(float *hagl_innov_var)
RingBuffer< magSample > _mag_buffer
imuSample get_imu_sample_delayed()
RingBuffer< auxVelSample > _auxvel_buffer
bool get_mag_decl_deg(float *val)
Vector3< float > Vector3f
Definition: Vector3.hpp:136
void set_rangefinder_limits(float min_distance, float max_distance)
baroSample get_baro_sample_delayed()
Quaternion< float > Quatf
Definition: Quaternion.hpp:544
RingBuffer< rangeSample > _range_buffer
static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C
Definition: geo.h:57
RingBuffer< imuSample > _imu_buffer
bool isVehicleAtRest() const
RingBuffer< airspeedSample > _airspeed_buffer
void get_pos_d_deriv(float *pos_d_deriv)
void set_in_air_status(bool in_air)
RingBuffer< flowSample > _flow_buffer
void copy_timestamp(uint64_t *time_us)
AlphaFilter< Vector3f > AlphaFilterVector3f
void set_air_density(float air_density)
const matrix::Quatf & get_quaternion() const
#define ISFINITE(x)
Definition: ecl.h:100
void get_position(float *pos)