51 #include <mathlib/mathlib.h> 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;
72 virtual void get_vel_pos_innov(
float vel_pos_innov[6]) = 0;
75 virtual void get_aux_vel_innov(
float aux_vel_innov[2]) = 0;
78 virtual void get_mag_innov(
float mag_innov[3]) = 0;
81 virtual void get_airspeed_innov(
float *airspeed_innov) = 0;
84 virtual void get_beta_innov(
float *beta_innov) = 0;
87 virtual void get_heading_innov(
float *heading_innov) = 0;
91 virtual void get_vel_pos_innov_var(
float vel_pos_innov_var[6]) = 0;
94 virtual void get_mag_innov_var(
float mag_innov_var[3]) = 0;
97 virtual void get_airspeed_innov_var(
float *get_airspeed_innov_var) = 0;
100 virtual void get_beta_innov_var(
float *get_beta_innov_var) = 0;
103 virtual void get_heading_innov_var(
float *heading_innov_var) = 0;
105 virtual void get_state_delayed(
float *
state) = 0;
107 virtual void get_wind_velocity(
float *wind) = 0;
109 virtual void get_wind_velocity_var(
float *wind_var) = 0;
111 virtual void get_true_airspeed(
float *tas) = 0;
114 virtual void get_vel_var(
Vector3f &vel_var) = 0;
117 virtual void get_pos_var(
Vector3f &pos_var) = 0;
120 virtual void get_flow_innov_var(
float flow_innov_var[2]) = 0;
123 virtual void get_flow_innov(
float flow_innov[2]) = 0;
126 virtual void get_drag_innov_var(
float drag_innov_var[2]) = 0;
129 virtual void get_drag_innov(
float drag_innov[2]) = 0;
131 virtual void getHaglInnovVar(
float *hagl_innov_var) = 0;
132 virtual void getHaglInnov(
float *hagl_innov) = 0;
140 virtual void get_output_tracking_error(
float error[3]) = 0;
148 virtual void get_imu_vibe_metrics(
float vibe[3]) = 0;
158 virtual bool get_gps_drift_metrics(
float drift[3],
bool *blocked) = 0;
165 virtual void get_ekf_gpos_accuracy(
float *ekf_eph,
float *ekf_epv) = 0;
168 virtual void get_ekf_lpos_accuracy(
float *ekf_eph,
float *ekf_epv) = 0;
171 virtual void get_ekf_vel_accuracy(
float *ekf_evh,
float *ekf_evv) = 0;
174 virtual void get_ekf_ctrl_limits(
float *vxy_max,
float *vz_max,
float *hagl_min,
float *hagl_max) = 0;
180 virtual bool collect_imu(
const imuSample &imu) = 0;
183 void setIMUData(
const imuSample &imu_sample);
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]);
189 void setMagData(uint64_t time_usec,
float (&
data)[3]);
192 void setGpsData(uint64_t time_usec,
const gps_message &gps);
195 void setBaroData(uint64_t time_usec,
float data);
198 void setAirspeedData(uint64_t time_usec,
float true_airspeed,
float eas2tas);
201 void setRangeData(uint64_t time_usec,
float data, int8_t quality);
205 void setOpticalFlowData(uint64_t time_usec,
flow_message *flow);
211 void setAuxVelData(uint64_t time_usec,
float (&data)[2],
float (&variance)[2]);
225 virtual bool reset_imu_bias() = 0;
240 void set_fuse_beta_flag(
bool fuse_beta) {_control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air);}
247 _control_status.flags.gnd_effect = gnd_effect;
248 _time_last_gnd_effect_on = _time_last_imu;
257 _rng_valid_min_val = min_distance;
258 _rng_valid_max_val = max_distance;
264 _flow_max_rate = max_flow_rate;
265 _flow_min_distance = min_distance;
266 _flow_max_distance = max_distance;
270 virtual bool global_position_is_valid() = 0;
275 virtual bool isTerrainEstimateValid()
const = 0;
280 virtual void getTerrainVertPos(
float *ret) = 0;
285 bool local_position_is_valid();
290 virtual void get_ev2ekf_quaternion(
float *quat) = 0;
295 Vector3f vel_earth = _output_new.vel - _vel_imu_rel_body_ned;
297 for (
unsigned i = 0; i < 3; i++) {
298 vel[i] = vel_earth(i);
305 for (
unsigned i = 0; i < 3; i++) {
306 vel_deriv[i] = _vel_deriv_ned(i);
313 float var = _output_vert_new.vel_d - _vel_imu_rel_body_ned(2);
321 Vector3f pos_offset_earth = _R_to_earth_now * _params.imu_pos_body;
324 for (
unsigned i = 0; i < 3; i++) {
325 pos[i] = _output_new.pos(i) - pos_offset_earth(i);
330 *time_us = _time_last_imu;
339 if (_NED_origin_initialised && (_params.mag_declination_source &
MASK_SAVE_GEO_DECL)) {
348 virtual void get_accel_bias(
float bias[3]) = 0;
349 virtual void get_gyro_bias(
float bias[3]) = 0;
354 *val = _control_status.value;
360 *val = _fault_status.value;
366 virtual void get_gps_check_status(uint16_t *val) = 0;
369 virtual void get_posD_reset(
float *delta, uint8_t *
counter) = 0;
372 virtual void get_velD_reset(
float *delta, uint8_t *counter) = 0;
375 virtual void get_posNE_reset(
float delta[2], uint8_t *counter) = 0;
378 virtual void get_velNE_reset(
float delta[2], uint8_t *counter) = 0;
381 virtual void get_quat_reset(
float delta_quat[4], uint8_t *counter) = 0;
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;
391 virtual void get_ekf_soln_status(uint16_t *status) = 0;
399 return _imu_sample_delayed;
405 return _baro_sample_delayed;
410 static constexpr
unsigned FILTER_UPDATE_PERIOD_MS{8};
411 static constexpr
float FILTER_UPDATE_PERIOD_S{FILTER_UPDATE_PERIOD_MS * 0.001f};
424 uint8_t _obs_buffer_length{0};
432 uint8_t _imu_buffer_length{0};
434 unsigned _min_obs_interval_us{0};
436 float _dt_imu_avg{0.0f};
453 uint8_t _drag_sample_count{0};
454 float _drag_sample_time_dt{0.0f};
458 float _rng_valid_min_val{0.0f};
459 float _rng_valid_max_val{0.0f};
460 float _flow_max_rate{0.0f};
461 float _flow_min_distance{0.0f};
462 float _flow_max_distance{0.0f};
474 bool _imu_updated{
false};
475 bool _initialised{
false};
477 bool _NED_origin_initialised{
false};
478 bool _gps_speed_valid{
false};
479 float _gps_origin_eph{0.0f};
480 float _gps_origin_epv{0.0f};
483 float _gps_alt_prev{0.0f};
484 float _gps_yaw_offset{0.0f};
487 float _yaw_test_ratio{0.0f};
488 float _mag_test_ratio[3] {};
489 float _vel_pos_test_ratio[6] {};
490 float _tas_test_ratio{0.0f};
491 float _terr_test_ratio{0.0f};
492 float _beta_test_ratio{0.0f};
493 float _drag_test_ratio[2] {};
496 bool _is_dead_reckoning{
false};
497 bool _deadreckon_time_exceeded{
true};
498 bool _is_wind_dead_reckoning{
false};
503 float _vibe_metrics[3] {};
507 float _gps_drift_metrics[3] {};
511 bool _vehicle_at_rest{
false};
512 uint64_t _time_last_move_detect_us{0};
513 bool _gps_drift_updated{
false};
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};
540 uint64_t _time_last_imu{0};
541 uint64_t _time_last_gps{0};
542 uint64_t _time_last_mag{0};
543 uint64_t _time_last_baro{0};
544 uint64_t _time_last_range{0};
545 uint64_t _time_last_airspeed{0};
546 uint64_t _time_last_ext_vision{0};
547 uint64_t _time_last_optflow{0};
548 uint64_t _time_last_gnd_effect_on{0};
549 uint64_t _time_last_auxvel{0};
554 bool initialise_interface(uint64_t timestamp);
557 void unallocate_buffers();
559 float _mag_declination_gps{0.0f};
560 float _mag_inclination_gps{0.0f};
561 float _mag_strength_gps{0.0f};
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 ...
static struct vehicle_status_s status
RingBuffer< outputSample > _output_buffer
void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
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)
RingBuffer< outputVert > _output_vert_buffer
int reset(enum LPS22HB_BUS busid)
Reset the driver.
void get_velocity(float *vel)
Vector3f _vel_imu_rel_body_ned
constexpr T degrees(T radians)
void get_control_mode(uint32_t *val)
RingBuffer< gpsSample > _gps_buffer
void init()
Activates/configures the hardware registers.
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
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
void set_rangefinder_limits(float min_distance, float max_distance)
baroSample get_baro_sample_delayed()
Quaternion< float > Quatf
RingBuffer< rangeSample > _range_buffer
static constexpr float CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C
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)
bool inertial_dead_reckoning()
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
void get_position(float *pos)