PX4 Firmware
PX4 Autopilot Software http://px4.io
estimator_interface.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013 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.cpp
36  * Definition of base class for attitude estimators
37  *
38  * @author Roman Bast <bapstroman@gmail.com>
39  * @author Paul Riseborough <p_riseborough@live.com.au>
40  * @author Siddharth B Purohit <siddharthbharatpurohit@gmail.com>
41  */
42 
43 #include "estimator_interface.h"
44 
45 #include <ecl.h>
46 #include <mathlib/mathlib.h>
47 
48 // Accumulate imu data and store to buffer at desired rate
50 {
51  if (!_initialised) {
52  init(imu_sample.time_us);
53  _initialised = true;
54  }
55 
56  const float dt = math::constrain((imu_sample.time_us - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f);
57 
58  _time_last_imu = imu_sample.time_us;
59 
60  if (_time_last_imu > 0) {
61  _dt_imu_avg = 0.8f * _dt_imu_avg + 0.2f * dt;
62  }
63 
64  // calculate a metric which indicates the amount of coning vibration
65  Vector3f temp = cross_product(imu_sample.delta_ang, _delta_ang_prev);
66  _vibe_metrics[0] = 0.99f * _vibe_metrics[0] + 0.01f * temp.norm();
67 
68  // calculate a metric which indicates the amount of high frequency gyro vibration
69  temp = imu_sample.delta_ang - _delta_ang_prev;
70  _delta_ang_prev = imu_sample.delta_ang;
71  _vibe_metrics[1] = 0.99f * _vibe_metrics[1] + 0.01f * temp.norm();
72 
73  // calculate a metric which indicates the amount of high frequency accelerometer vibration
74  temp = imu_sample.delta_vel - _delta_vel_prev;
75  _delta_vel_prev = imu_sample.delta_vel;
76  _vibe_metrics[2] = 0.99f * _vibe_metrics[2] + 0.01f * temp.norm();
77 
78  // detect if the vehicle is not moving when on ground
80  if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler)
81  || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler)
82  || ((imu_sample.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) {
83 
85  }
86 
87  _vehicle_at_rest = ((imu_sample.time_us - _time_last_move_detect_us) > (uint64_t)1E6);
88 
89  } else {
91  _vehicle_at_rest = false;
92  }
93 
94  // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available
95  if (collect_imu(imu_sample)) {
96 
97  // down-sample the drag specific force data by accumulating and calculating the mean when
98  // sufficient samples have been collected
100 
101  // Allocate the required buffer size if not previously done
102  // Do not retry if allocation has failed previously
105 
106  if (_drag_buffer_fail) {
107  ECL_ERR_TIMESTAMPED("EKF drag buffer allocation failed");
108  return;
109  }
110  }
111 
113  // note acceleration is accumulated as a delta velocity
114  _drag_down_sampled.accelXY(0) += imu_sample.delta_vel(0);
115  _drag_down_sampled.accelXY(1) += imu_sample.delta_vel(1);
116  _drag_down_sampled.time_us += imu_sample.time_us;
117  _drag_sample_time_dt += imu_sample.delta_vel_dt;
118 
119  // calculate the downsample ratio for drag specific force data
120  uint8_t min_sample_ratio = (uint8_t) ceilf((float)_imu_buffer_length / _obs_buffer_length);
121 
122  if (min_sample_ratio < 5) {
123  min_sample_ratio = 5;
124  }
125 
126  // calculate and store means from accumulated values
127  if (_drag_sample_count >= min_sample_ratio) {
128  // note conversion from accumulated delta velocity to acceleration
132 
133  // write to buffer
135 
136  // reset accumulators
137  _drag_sample_count = 0;
140  _drag_sample_time_dt = 0.0f;
141  }
142  }
143  }
144 }
145 
146 void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt,
147  float (&delta_ang)[3], float (&delta_vel)[3])
148 {
149  imuSample imu_sample_new;
150  imu_sample_new.delta_ang = Vector3f(delta_ang);
151  imu_sample_new.delta_vel = Vector3f(delta_vel);
152 
153  // convert time from us to secs
154  imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
155  imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
156  imu_sample_new.time_us = time_usec;
157 
158  setIMUData(imu_sample_new);
159 }
160 
161 void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
162 {
163  if (!_initialised || _mag_buffer_fail) {
164  return;
165  }
166 
167  // Allocate the required buffer size if not previously done
168  // Do not retry if allocation has failed previously
171 
172  if (_mag_buffer_fail) {
173  ECL_ERR_TIMESTAMPED("EKF mag buffer allocation failed");
174  return;
175  }
176  }
177 
178  // limit data rate to prevent data being lost
179  if ((time_usec - _time_last_mag) > _min_obs_interval_us) {
180 
181  magSample mag_sample_new;
182  mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
183 
184  mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
185  _time_last_mag = time_usec;
186 
187  mag_sample_new.mag = Vector3f(data);
188 
189  _mag_buffer.push(mag_sample_new);
190  }
191 }
192 
193 void EstimatorInterface::setGpsData(uint64_t time_usec, const gps_message &gps)
194 {
195  if (!_initialised || _gps_buffer_fail) {
196  return;
197  }
198 
199  // Allocate the required buffer size if not previously done
200  // Do not retry if allocation has failed previously
203 
204  if (_gps_buffer_fail) {
205  ECL_ERR_TIMESTAMPED("EKF GPS buffer allocation failed");
206  return;
207  }
208  }
209 
210  // limit data rate to prevent data being lost
212 
213  if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {
214  gpsSample gps_sample_new;
215  gps_sample_new.time_us = gps.time_usec - _params.gps_delay_ms * 1000;
216 
217  gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
218  _time_last_gps = time_usec;
219 
220  gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
221  gps_sample_new.vel = Vector3f(gps.vel_ned);
222 
224  gps_sample_new.sacc = gps.sacc;
225  gps_sample_new.hacc = gps.eph;
226  gps_sample_new.vacc = gps.epv;
227 
228  gps_sample_new.hgt = (float)gps.alt * 1e-3f;
229 
230  gps_sample_new.yaw = gps.yaw;
231  if (ISFINITE(gps.yaw_offset)) {
233  } else {
234  _gps_yaw_offset = 0.0f;
235  }
236 
237  // Only calculate the relative position if the WGS-84 location of the origin is set
238  if (collect_gps(gps)) {
239  float lpos_x = 0.0f;
240  float lpos_y = 0.0f;
241  map_projection_project(&_pos_ref, (gps.lat / 1.0e7), (gps.lon / 1.0e7), &lpos_x, &lpos_y);
242  gps_sample_new.pos(0) = lpos_x;
243  gps_sample_new.pos(1) = lpos_y;
244 
245  } else {
246  gps_sample_new.pos(0) = 0.0f;
247  gps_sample_new.pos(1) = 0.0f;
248  }
249 
250  _gps_buffer.push(gps_sample_new);
251  }
252 }
253 
254 void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
255 {
257  return;
258  }
259 
260  // Allocate the required buffer size if not previously done
261  // Do not retry if allocation has failed previously
264 
265  if (_baro_buffer_fail) {
266  ECL_ERR_TIMESTAMPED("EKF baro buffer allocation failed");
267  return;
268  }
269  }
270 
271  // limit data rate to prevent data being lost
272  if ((time_usec - _time_last_baro) > _min_obs_interval_us) {
273 
274  baroSample baro_sample_new;
275  baro_sample_new.hgt = data;
276  baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
277 
278  baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
279  _time_last_baro = time_usec;
280 
281  baro_sample_new.time_us = math::max(baro_sample_new.time_us, _imu_sample_delayed.time_us);
282 
283  _baro_buffer.push(baro_sample_new);
284  }
285 }
286 
287 void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas)
288 {
290  return;
291  }
292 
293  // Allocate the required buffer size if not previously done
294  // Do not retry if allocation has failed previously
297 
298  if (_airspeed_buffer_fail) {
299  ECL_ERR_TIMESTAMPED("EKF airspeed buffer allocation failed");
300  return;
301  }
302  }
303 
304  // limit data rate to prevent data being lost
305  if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) {
306  airspeedSample airspeed_sample_new;
307  airspeed_sample_new.true_airspeed = true_airspeed;
308  airspeed_sample_new.eas2tas = eas2tas;
309  airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
310  airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
311  _time_last_airspeed = time_usec;
312 
313  _airspeed_buffer.push(airspeed_sample_new);
314  }
315 }
316 
317 void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t quality)
318 {
320  return;
321  }
322 
323  // Allocate the required buffer size if not previously done
324  // Do not retry if allocation has failed previously
327 
328  if (_range_buffer_fail) {
329  ECL_ERR_TIMESTAMPED("EKF range finder buffer allocation failed");
330  return;
331  }
332  }
333 
334  // limit data rate to prevent data being lost
335  if ((time_usec - _time_last_range) > _min_obs_interval_us) {
336  rangeSample range_sample_new;
337  range_sample_new.rng = data;
338  range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
339  range_sample_new.quality = quality;
340  _time_last_range = time_usec;
341 
342  _range_buffer.push(range_sample_new);
343  }
344 }
345 
346 // set optical flow data
348 {
350  return;
351  }
352 
353  // Allocate the required buffer size if not previously done
354  // Do not retry if allocation has failed previously
357 
358  if (_flow_buffer_fail) {
359  ECL_ERR_TIMESTAMPED("EKF optical flow buffer allocation failed");
360  return;
361  }
362  }
363 
364  // limit data rate to prevent data being lost
365  if ((time_usec - _time_last_optflow) > _min_obs_interval_us) {
366  // check if enough integration time and fail if integration time is less than 50%
367  // of min arrival interval because too much data is being lost
368  float delta_time = 1e-6f * (float)flow->dt;
369  float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
370  bool delta_time_good = delta_time >= delta_time_min;
371 
372  if (!delta_time_good) {
373  // protect against overflow caused by division with very small delta_time
374  delta_time = delta_time_min;
375  }
376 
377 
378  // check magnitude is within sensor limits
379  // use this to prevent use of a saturated flow sensor when there are other aiding sources available
380  float flow_rate_magnitude;
381  bool flow_magnitude_good = true;
382  if (delta_time_good) {
383  flow_rate_magnitude = flow->flowdata.norm() / delta_time;
384  flow_magnitude_good = (flow_rate_magnitude <= _flow_max_rate);
385  }
386 
388 
389  // check quality metric
390  bool flow_quality_good = (flow->quality >= _params.flow_qual_min);
391 
392  // Check data validity and write to buffers
393  // Invalid flow data is allowed when on ground and is handled as a special case in controlOpticalFlowFusion()
394  bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow);
395  if (use_flow_data_to_navigate || (!_control_status.flags.in_air && relying_on_flow)) {
396  flowSample optflow_sample_new;
397  // calculate the system time-stamp for the trailing edge of the flow data integration period
398  optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000;
399 
400  // copy the quality metric returned by the PX4Flow sensor
401  optflow_sample_new.quality = flow->quality;
402 
403  // copy the optical and gyro measured delta angles
404  // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate
405  // is produced by a RH rotation of the image about the sensor axis.
406  optflow_sample_new.gyroXYZ = - flow->gyrodata;
407  optflow_sample_new.flowRadXY = -flow->flowdata;
408 
409  // convert integration interval to seconds
410  optflow_sample_new.dt = delta_time;
411  _time_last_optflow = time_usec;
412 
413  _flow_buffer.push(optflow_sample_new);
414  }
415  }
416 }
417 
418 // set attitude and position data derived from an external vision system
420 {
421  if (!_initialised || _ev_buffer_fail) {
422  return;
423  }
424 
425  // Allocate the required buffer size if not previously done
426  // Do not retry if allocation has failed previously
429 
430  if (_ev_buffer_fail) {
431  ECL_ERR_TIMESTAMPED("EKF external vision buffer allocation failed");
432  return;
433  }
434  }
435 
436  // limit data rate to prevent data being lost
437  if ((time_usec - _time_last_ext_vision) > _min_obs_interval_us) {
438  extVisionSample ev_sample_new;
439  // calculate the system time-stamp for the mid point of the integration period
440  ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000;
441 
442  // copy required data
443  ev_sample_new.angErr = evdata->angErr;
444  ev_sample_new.posErr = evdata->posErr;
445  ev_sample_new.velErr = evdata->velErr;
446  ev_sample_new.hgtErr = evdata->hgtErr;
447  ev_sample_new.quat = evdata->quat;
448  ev_sample_new.pos = evdata->pos;
449  ev_sample_new.vel = evdata->vel;
450 
451  // record time for comparison next measurement
452  _time_last_ext_vision = time_usec;
453 
454  _ext_vision_buffer.push(ev_sample_new);
455 
456  }
457 }
458 
459 void EstimatorInterface::setAuxVelData(uint64_t time_usec, float (&data)[2], float (&variance)[2])
460 {
462  return;
463  }
464 
465  // Allocate the required buffer size if not previously done
466  // Do not retry if allocation has failed previously
469 
470  if (_auxvel_buffer_fail) {
471  ECL_ERR_TIMESTAMPED("EKF aux vel buffer allocation failed");
472  return;
473  }
474  }
475 
476  // limit data rate to prevent data being lost
477  if ((time_usec - _time_last_auxvel) > _min_obs_interval_us) {
478 
479  auxVelSample auxvel_sample_new;
480  auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000;
481 
482  auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
483  _time_last_auxvel = time_usec;
484 
485  auxvel_sample_new.velNE = Vector2f(data);
486  auxvel_sample_new.velVarNE = Vector2f(variance);
487 
488  _auxvel_buffer.push(auxvel_sample_new);
489  }
490 }
491 
493 {
494  // find the maximum time delay the buffers are required to handle
495  uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms,
503 
504  // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
505  _imu_buffer_length = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1;
506 
507  // set the observation buffer length to handle the minimum time of arrival between observations in combination
508  // with the worst case delay from current time to ekf fusion time
509  // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
510  uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f));
511  _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1;
512 
513  // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)
515 
519 
520  ECL_ERR_TIMESTAMPED("EKF buffer allocation failed!");
522  return false;
523  }
524 
525  _dt_imu_avg = 0.0f;
526 
527  _imu_sample_delayed.delta_ang.setZero();
528  _imu_sample_delayed.delta_vel.setZero();
531  _imu_sample_delayed.time_us = timestamp;
532 
533  _initialised = false;
534 
535  _time_last_imu = 0;
536  _time_last_gps = 0;
537  _time_last_mag = 0;
538  _time_last_baro = 0;
539  _time_last_range = 0;
541  _time_last_optflow = 0;
542  _fault_status.value = 0;
544  return true;
545 }
546 
548 {
561 
562 }
563 
565 {
566  // return true if we are not doing unconstrained free inertial navigation
568 }
569 
571 {
572  ECL_INFO("local position valid: %s", local_position_is_valid() ? "yes" : "no");
573  ECL_INFO("global position valid: %s", global_position_is_valid() ? "yes" : "no");
574 
575  ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size());
576  ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size());
577  ECL_INFO("mag buffer: %d (%d Bytes)", _mag_buffer.get_length(), _mag_buffer.get_total_size());
578  ECL_INFO("baro buffer: %d (%d Bytes)", _baro_buffer.get_length(), _baro_buffer.get_total_size());
579  ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size());
580  ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size());
581  ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size());
582  ECL_INFO("ext vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size());
583  ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size());
584  ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
585  ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size());
586 }
void setIMUData(const imuSample &imu_sample)
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
Definition: Limits.hpp:66
void setMagData(uint64_t time_usec, float(&data)[3])
struct estimator::filter_control_status_u::@60 flags
float eph
GPS horizontal position accuracy in m.
Definition: common.h:65
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
Definition: common.h:107
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
Definition: common.h:63
float hgt
gps height measurement (m)
Definition: common.h:116
Adapter / shim layer for system calls needed by ECL.
float epv
GPS vertical position accuracy in m.
Definition: common.h:66
#define ECL_ERR_TIMESTAMPED
Definition: ecl.h:95
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
#define VDIST_SENSOR_GPS
Use GPS height.
Definition: common.h:179
RingBuffer< outputSample > _output_buffer
void setAuxVelData(uint64_t time_usec, float(&data)[2], float(&variance)[2])
uint64_t time_usec
Definition: common.h:58
RingBuffer< extVisionSample > _ext_vision_buffer
Vector2f pos
NE earth frame gps horizontal position measurement (m)
Definition: common.h:115
virtual bool collect_imu(const imuSample &imu)=0
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:158
float gps_delay_ms
GPS measurement delay relative to the IMU (mSec)
Definition: common.h:230
float min_delay_ms
Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) ...
Definition: common.h:227
float sacc
GPS speed accuracy in m/s.
Definition: common.h:67
Vector2f accelXY
measured specific force along the X and Y body axes (m/sec**2)
Definition: common.h:167
Vector3f gyrodata
Gyro rates about the XYZ body axes (rad/sec)
Definition: common.h:78
void unallocate()
Definition: RingBuffer.h:91
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:122
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
Definition: common.h:64
RingBuffer< outputVert > _output_vert_buffer
void setGpsData(uint64_t time_usec, const gps_message &gps)
float vacc
1-std vertical position error (m)
Definition: common.h:120
float flow_delay_ms
optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow...
Definition: common.h:232
Vector3f gyroXYZ
measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (...
Definition: common.h:150
float vel_ned[3]
GPS ground speed NED.
Definition: common.h:69
float _flow_max_rate
maximum angular flow rate that the optical flow sensor can measure (rad/s)
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
Definition: common.h:222
uint32_t dt
integration time of flow samples (sec)
Definition: common.h:79
int32_t vdist_sensor_type
selects the primary source for height data
Definition: common.h:223
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:160
float dt
amount of integration time (sec)
Definition: common.h:151
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:156
uint8_t quality
Quality of Flow data.
Definition: common.h:76
float rng
range (distance to ground) measurement (m)
Definition: common.h:136
Vector2f velVarNE
estimated error variance of the NE velocity (m/sec)**2
Definition: common.h:173
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:118
bool allocate(uint8_t size)
Definition: RingBuffer.h:64
float sacc
1-std speed error (m/sec)
Definition: common.h:121
RingBuffer< gpsSample > _gps_buffer
int8_t quality
Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
Definition: common.h:138
virtual bool collect_gps(const gps_message &gps)=0
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:88
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:144
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
Definition: common.h:465
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:163
float hgtErr
1-Sigma height accuracy (m)
Definition: common.h:87
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:174
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:159
bool vel_ned_valid
GPS ground speed is valid.
Definition: common.h:70
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
Definition: common.h:117
void setBaroData(uint64_t time_usec, float data)
float eas2tas
equivalent to true airspeed factor
Definition: common.h:143
float auxvel_delay_ms
auxiliary velocity measurement delay relative to the IMU (mSec)
Definition: common.h:235
uint8_t * data
Definition: dataman.cpp:149
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:111
float baro_delay_ms
barometer height measurement delay relative to the IMU (mSec)
Definition: common.h:229
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
Vector2< float > Vector2f
Definition: Vector2.hpp:69
RingBuffer< dragSample > _drag_buffer
uint8_t quality
quality indicator between 0 and 255
Definition: common.h:148
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
Definition: common.h:83
filter_control_status_u _control_status
virtual bool global_position_is_valid()=0
float mag_delay_ms
magnetometer measurement delay relative to the IMU (mSec)
Definition: common.h:228
RingBuffer< baroSample > _baro_buffer
int32_t lat
Latitude in 1E-7 degrees.
Definition: common.h:59
Vector2f flowRadXY
measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive ...
Definition: common.h:149
RingBuffer< magSample > _mag_buffer
fault_status_u _fault_status
float hgt
pressure altitude above sea level (m)
Definition: common.h:131
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas)
RingBuffer< auxVelSample > _auxvel_buffer
float airspeed_delay_ms
airspeed measurement delay relative to the IMU (mSec)
Definition: common.h:231
Vector2f velNE
measured NE velocity relative to the local origin (m/sec)
Definition: common.h:172
Vector2f flowdata
Optical flow rates about the X and Y body axes (rad/sec)
Definition: common.h:77
#define MASK_USE_GPS
set to true to use GPS data
Definition: common.h:189
constexpr _Tp min(_Tp a, _Tp b)
Definition: Limits.hpp:54
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:84
#define MASK_USE_DRAG
set to true to use the multi-rotor drag model to estimate wind
Definition: common.h:194
Vector3< float > Vector3f
Definition: Vector3.hpp:136
float range_delay_ms
range finder measurement delay relative to the IMU (mSec)
Definition: common.h:233
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
Definition: common.h:62
void setRangeData(uint64_t time_usec, float data, int8_t quality)
float true_airspeed
true airspeed measurement (m/sec)
Definition: common.h:142
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:132
virtual bool init(uint64_t timestamp)=0
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
Definition: geo.cpp:132
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:127
float angErr
1-Sigma angular error (rad)
Definition: common.h:89
float dt
Definition: px4io.c:73
uint32_t in_air
7 - true when the vehicle is airborne
Definition: common.h:448
Definition of base class for attitude estimators.
RingBuffer< rangeSample > _range_buffer
static constexpr unsigned FILTER_UPDATE_PERIOD_MS
RingBuffer< imuSample > _imu_buffer
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
Definition: common.h:61
#define ECL_INFO
Definition: ecl.h:90
int32_t sensor_interval_min_ms
minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation b...
Definition: common.h:224
Quatf quat
quaternion defining rotation from body to earth frame
Definition: common.h:85
uint32_t ev_pos
12 - true when local position data from external vision is being fused
Definition: common.h:453
RingBuffer< airspeedSample > _airspeed_buffer
float hacc
1-std horizontal position error (m)
Definition: common.h:119
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
Definition: common.h:157
float angErr
1-Sigma angular error (rad)
Definition: common.h:162
float is_moving_scaler
gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the...
Definition: common.h:360
int32_t flow_qual_min
minimum acceptable quality integer from the flow sensor
Definition: common.h:310
int get_total_size()
Definition: RingBuffer.h:161
uint32_t gps
2 - true if GPS measurements are being fused
Definition: common.h:443
RingBuffer< flowSample > _flow_buffer
float velErr
1-Sigma velocity accuracy (m/sec)
Definition: common.h:161
Vector3f mag
NED magnetometer body frame measurements (Gauss)
Definition: common.h:126
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
Definition: common.h:108
bool initialise_interface(uint64_t timestamp)
float delta_ang_dt
delta angle integration period (sec)
Definition: common.h:109
uint64_t time_us
timestamp of the integration period leading edge (uSec)
Definition: common.h:152
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:168
uint64_t time_us
timestamp of the measurement (uSec)
Definition: common.h:137
float ev_delay_ms
off-board vision measurement delay relative to the IMU (mSec)
Definition: common.h:234
float delta_vel_dt
delta velocity integration period (sec)
Definition: common.h:110
uint8_t get_length() const
Definition: RingBuffer.h:117
#define ISFINITE(x)
Definition: ecl.h:100
int32_t lon
Longitude in 1E-7 degrees.
Definition: common.h:60
float posErr
1-Sigma horizontal position accuracy (m)
Definition: common.h:86
void push(const data_type &sample)
Definition: RingBuffer.h:97
void setOpticalFlowData(uint64_t time_usec, flow_message *flow)