diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 7fb5da77a9..48b9ab8b7e 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -39,6 +39,7 @@ px4_add_module( COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} -fno-associative-math + #-DDEBUG_BUILD INCLUDES EKF STACK_MAX diff --git a/src/modules/ekf2/EKF/RingBuffer.h b/src/modules/ekf2/EKF/RingBuffer.h index 512b3a6b6a..76811e3bea 100644 --- a/src/modules/ekf2/EKF/RingBuffer.h +++ b/src/modules/ekf2/EKF/RingBuffer.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2015 PX4 Development Team. All rights reserved. + * Copyright (C) 2015-2022 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -46,7 +46,7 @@ class RingBuffer { public: explicit RingBuffer(size_t size) { allocate(size); } - RingBuffer() { allocate(1); } + RingBuffer() = delete; ~RingBuffer() { delete[] _buffer; } // no copy, assignment, move, move assignment @@ -155,6 +155,19 @@ public: int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; } + int entries() const + { + int count = 0; + + for (uint8_t i = 0; i < _size; i++) { + if (_buffer[i].time_us != 0) { + count++; + } + } + + return count; + } + private: data_type *_buffer{nullptr}; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 7f1cbe977c..17410718b7 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -219,7 +219,8 @@ struct parameters { int32_t vdist_sensor_type{VDIST_SENSOR_BARO}; ///< selects the primary source for height data int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator - int32_t sensor_interval_min_ms{20}; ///< minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) + + int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) // measurement time delays float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ea57cb39f8..e3fe101ac5 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -85,47 +85,59 @@ void Ekf::controlFusionModes() } } - // check for intermittent data (before pop_first_older_than) - const baroSample &baro_init = _baro_buffer.get_newest(); - _baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); + if (_baro_buffer) { + // check for intermittent data (before pop_first_older_than) + const baroSample &baro_init = _baro_buffer->get_newest(); + _baro_hgt_faulty = !isRecent(baro_init.time_us, 2 * BARO_MAX_INTERVAL); - const gpsSample &gps_init = _gps_buffer.get_newest(); - _gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL); + const uint64_t baro_time_prev = _baro_sample_delayed.time_us; + _baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); - // check for arrival of new sensor data at the fusion time horizon - _time_prev_gps_us = _gps_sample_delayed.time_us; - _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); - _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); - - if (_mag_data_ready) { - _mag_lpf.update(_mag_sample_delayed.mag); - - // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. - // this is useful if there is a lot of interference on the sensor measurement. - if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) && (_NED_origin_initialised - || PX4_ISFINITE(_mag_declination_gps))) { - - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); - _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); - _control_status.flags.synthetic_mag_z = true; - - } else { - _control_status.flags.synthetic_mag_z = false; + // if we have a new baro sample save the delta time between this sample and the last sample which is + // used below for baro offset calculations + if (_baro_data_ready && baro_time_prev != 0) { + _delta_time_baro_us = _baro_sample_delayed.time_us - baro_time_prev; } } - _delta_time_baro_us = _baro_sample_delayed.time_us; - _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); - // if we have a new baro sample save the delta time between this sample and the last sample which is - // used below for baro offset calculations - if (_baro_data_ready) { - _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us; + if (_gps_buffer) { + const gpsSample &gps_init = _gps_buffer->get_newest(); + _gps_hgt_intermittent = !isRecent(gps_init.time_us, 2 * GPS_MAX_INTERVAL); + + // check for arrival of new sensor data at the fusion time horizon + _time_prev_gps_us = _gps_sample_delayed.time_us; + _gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); } - { + + if (_mag_buffer) { + _mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); + + if (_mag_data_ready) { + _mag_lpf.update(_mag_sample_delayed.mag); + + // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. + // this is useful if there is a lot of interference on the sensor measurement. + if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL) + && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) + ) { + + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); + _mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred); + _control_status.flags.synthetic_mag_z = true; + + } else { + _control_status.flags.synthetic_mag_z = false; + } + } + + + } + + if (_range_buffer) { // Get range data from buffer and check validity - const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); + bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress()); _range_sensor.setDataReadiness(is_rng_data_ready); // update range sensor angle parameters in case they have changed @@ -134,35 +146,42 @@ void Ekf::controlFusionModes() _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); _range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth); + + if (_range_sensor.isDataHealthy()) { + // correct the range data for position offset relative to the IMU + const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + } } - if (_range_sensor.isDataHealthy()) { - // correct the range data for position offset relative to the IMU - const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + if (_flow_buffer) { + // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. + // This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow, + // in this case we need to empty the buffer + if (!_flow_data_ready || !_control_status.flags.opt_flow) { + _flow_data_ready = _flow_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed); + } + + // check if we should fuse flow data for terrain estimation + if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) { + // TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion + // due to some checks failing + // only fuse flow for terrain if range data hasn't been fused for 5 seconds + _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6); + // only fuse flow for terrain if the main filter is not fusing flow and we are using gps + _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps); + } } - // We don't fuse flow data immediately because we have to wait for the mid integration point to fall behind the fusion time horizon. - // This means we stop looking for new data until the old data has been fused, unless we are not fusing optical flow, - // in this case we need to empty the buffer - if (!_flow_data_ready || !_control_status.flags.opt_flow) { - _flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed); + if (_ext_vision_buffer) { + _ev_data_ready = _ext_vision_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); } - // check if we should fuse flow data for terrain estimation - if (!_flow_for_terrain_data_ready && _flow_data_ready && _control_status.flags.in_air) { - // TODO: WARNING, _flow_data_ready can be modified in controlOpticalFlowFusion - // due to some checks failing - // only fuse flow for terrain if range data hasn't been fused for 5 seconds - _flow_for_terrain_data_ready = isTimedOut(_time_last_hagl_fuse, (uint64_t)5E6); - // only fuse flow for terrain if the main filter is not fusing flow and we are using gps - _flow_for_terrain_data_ready &= (!_control_status.flags.opt_flow && _control_status.flags.gps); + if (_airspeed_buffer) { + _tas_data_ready = _airspeed_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); } - _ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed); - _tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed); - // check for height sensor timeouts and reset and change sensor if necessary controlHeightSensorTimeouts(); @@ -632,8 +651,12 @@ void Ekf::controlHeightSensorTimeouts() if (_control_status.flags.baro_hgt) { // check if GPS height is available - const gpsSample &gps_init = _gps_buffer.get_newest(); - const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + bool gps_hgt_accurate = false; + + if (_gps_buffer) { + const gpsSample &gps_init = _gps_buffer->get_newest(); + gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + } // check for inertial sensing errors in the last BADACC_PROBATION seconds const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION); @@ -660,13 +683,21 @@ void Ekf::controlHeightSensorTimeouts() } else if (_control_status.flags.gps_hgt) { // check if GPS height is available - const gpsSample &gps_init = _gps_buffer.get_newest(); - const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + bool gps_hgt_accurate = false; + + if (_gps_buffer) { + const gpsSample &gps_init = _gps_buffer->get_newest(); + gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + } // check the baro height source for consistency and freshness - const baroSample &baro_init = _baro_buffer.get_newest(); - const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); - const bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate); + bool baro_data_consistent = false; + + if (_baro_buffer) { + const baroSample &baro_init = _baro_buffer->get_newest(); + const float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); + baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P(9, 9)) * sq(_params.baro_innov_gate); + } // if baro data is acceptable and GPS data is inaccurate, reset height to baro const bool reset_to_baro = !_baro_hgt_faulty && @@ -702,8 +733,12 @@ void Ekf::controlHeightSensorTimeouts() } else if (_control_status.flags.ev_hgt) { // check if vision data is available - const extVisionSample &ev_init = _ext_vision_buffer.get_newest(); - const bool ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL); + bool ev_data_available = false; + + if (_ext_vision_buffer) { + const extVisionSample &ev_init = _ext_vision_buffer->get_newest(); + ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL); + } if (ev_data_available) { request_height_reset = true; @@ -784,7 +819,7 @@ void Ekf::checkVerticalAccelerationHealth() const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling; if (bad_vert_accel) { - _time_bad_vert_accel = _time_last_imu; + _time_bad_vert_accel = _time_last_imu; } else { _time_good_vert_accel = _time_last_imu; @@ -807,7 +842,7 @@ void Ekf::controlHeightFusion() switch (_params.vdist_sensor_type) { default: - ECL_ERR("Invalid hgt mode: %d", _params.vdist_sensor_type); + ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type); // FALLTHROUGH case VDIST_SENSOR_BARO: @@ -825,6 +860,7 @@ void Ekf::controlHeightFusion() break; case VDIST_SENSOR_RANGE: + // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air @@ -845,6 +881,7 @@ void Ekf::controlHeightFusion() break; case VDIST_SENSOR_GPS: + // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts. // Do switching between GPS and rangefinder if using range finder as a height source when close @@ -870,6 +907,7 @@ void Ekf::controlHeightFusion() break; case VDIST_SENSOR_EV: + // don't start using EV data unless data is arriving frequently if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { startEvHgtFusion(); @@ -1022,10 +1060,9 @@ void Ekf::controlDragFusion() _control_status.flags.wind = true; resetWind(); - } else if (_drag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { + } else if (_drag_buffer && _drag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_drag_sample_delayed)) { fuseDrag(); } - } } @@ -1089,7 +1126,11 @@ void Ekf::controlFakePosFusion() void Ekf::controlAuxVelFusion() { - const bool data_ready = _auxvel_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed); + bool data_ready = false; + + if (_auxvel_buffer) { + data_ready = _auxvel_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_auxvel_sample_delayed); + } if (data_ready && isHorizontalAidingActive()) { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index c506b4f7cd..5feaf11946 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -143,7 +143,7 @@ bool Ekf::initialiseFilter() } // Sum the magnetometer measurements - if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { + if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_mag_sample_delayed.time_us != 0) { if (_mag_counter == 0) { _mag_lpf.reset(_mag_sample_delayed.mag); @@ -157,7 +157,7 @@ bool Ekf::initialiseFilter() } // accumulate enough height measurements to be confident in the quality of the data - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { + if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_baro_sample_delayed.time_us != 0) { if (_baro_counter == 0) { _baro_hgt_offset = _baro_sample_delayed.hgt; @@ -325,10 +325,13 @@ void Ekf::calculateOutputStates(const imuSample &imu) // Note fixed coefficients are used to save operations. The exact time constant is not important. _yaw_rate_lpf_ef = 0.95f * _yaw_rate_lpf_ef + 0.05f * spin_del_ang_D / imu.delta_ang_dt; + + _output_new.time_us = imu.time_us; + _output_vert_new.time_us = imu.time_us; + const Quatf dq(AxisAnglef{delta_angle}); // rotate the previous INS quaternion by the delta quaternions - _output_new.time_us = imu.time_us; _output_new.quat_nominal = _output_new.quat_nominal * dq; // the quaternions must always be normalised after modification diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 4c4f9d1ce2..27026f531f 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -546,8 +546,8 @@ private: terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground // height sensor status - bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use - bool _gps_hgt_intermittent{false}; ///< true if gps height into the buffer is intermittent + bool _baro_hgt_faulty{true}; ///< true if valid baro data is unavailable for use + bool _gps_hgt_intermittent{true}; ///< true if gps height into the buffer is intermittent // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ff2cb4f6ed..df74794247 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -255,9 +255,6 @@ void Ekf::resetVerticalPositionTo(const float &new_vert_pos) // Reset height state using the last height measurement void Ekf::resetHeight() { - // Get the most recent GPS data - const gpsSample &gps_newest = _gps_buffer.get_newest(); - // reset the vertical position if (_control_status.flags.rng_hgt) { float dist_bottom; @@ -277,15 +274,18 @@ void Ekf::resetHeight() P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - const baroSample &baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + if (_baro_buffer) { + const baroSample &baro_newest = _baro_buffer->get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } } else if (_control_status.flags.baro_hgt) { // initialize vertical position with newest baro measurement - const baroSample &baro_newest = _baro_buffer.get_newest(); - if (!_baro_hgt_faulty) { - resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset); + if (_baro_buffer) { + const baroSample &baro_newest = _baro_buffer->get_newest(); + resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset); + } // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); @@ -296,15 +296,18 @@ void Ekf::resetHeight() } else if (_control_status.flags.gps_hgt) { // initialize vertical position and velocity with newest gps measurement - if (!_gps_hgt_intermittent) { + if (!_gps_hgt_intermittent && _gps_buffer) { + const gpsSample &gps_newest = _gps_buffer->get_newest(); resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc)); // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup - const baroSample &baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + if (_baro_buffer) { + const baroSample &baro_newest = _baro_buffer->get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } } else { // TODO: reset to last known gps based estimate @@ -312,20 +315,23 @@ void Ekf::resetHeight() } else if (_control_status.flags.ev_hgt) { // initialize vertical position with newest measurement - const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); + if (_ext_vision_buffer) { + const extVisionSample &ev_newest = _ext_vision_buffer->get_newest(); - // use the most recent data if it's time offset from the fusion time horizon is smaller - if (ev_newest.time_us >= _ev_sample_delayed.time_us) { - resetVerticalPositionTo(ev_newest.pos(2)); + // use the most recent data if it's time offset from the fusion time horizon is smaller + if (ev_newest.time_us >= _ev_sample_delayed.time_us) { + resetVerticalPositionTo(ev_newest.pos(2)); - } else { - resetVerticalPositionTo(_ev_sample_delayed.pos(2)); + } else { + resetVerticalPositionTo(_ev_sample_delayed.pos(2)); + } } } // reset the vertical velocity state - if (_control_status.flags.gps && !_gps_hgt_intermittent) { + if (_control_status.flags.gps && !_gps_hgt_intermittent && _gps_buffer) { // If we are using GPS, then use it to reset the vertical velocity + const gpsSample &gps_newest = _gps_buffer->get_newest(); resetVerticalVelocityTo(gps_newest.vel(2)); // the state variance is the same as the observation @@ -400,7 +406,7 @@ bool Ekf::realignYawGPS() const bool badMagYaw = (badYawErr && badVelInnov); if (badMagYaw) { - _num_bad_flight_yaw_events ++; + _num_bad_flight_yaw_events++; } // correct yaw angle using GPS ground course if compass yaw bad or yaw is previously not aligned @@ -719,6 +725,7 @@ void Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons // reinitialize map projection to latitude, longitude, altitude, and reset position _pos_ref.initReference(latitude, longitude, _time_last_imu); + if (current_pos_available) { // reset horizontal position Vector2f position = _pos_ref.project(current_lat, current_lon); @@ -1337,7 +1344,7 @@ void Ekf::updateBaroHgtOffset() { // calculate a filtered offset between the baro origin and local NED origin if we are not // using the baro as a height reference - if (!_control_status.flags.baro_hgt && _baro_data_ready) { + if (!_control_status.flags.baro_hgt && _baro_data_ready && (_delta_time_baro_us != 0)) { const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset @@ -1361,7 +1368,7 @@ float Ekf::getGpsHeightVariance() void Ekf::updateBaroHgtBias() { // Baro bias estimation using GPS altitude - if (_baro_data_ready) { + if (_baro_data_ready && (_delta_time_baro_us != 0)) { const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); _baro_b_est.setMaxStateNoise(_params.baro_noise); _baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index b68c034988..ac3053dd12 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -44,6 +44,19 @@ #include +EstimatorInterface::~EstimatorInterface() +{ + delete _gps_buffer; + delete _mag_buffer; + delete _baro_buffer; + delete _range_buffer; + delete _airspeed_buffer; + delete _flow_buffer; + delete _ext_vision_buffer; + delete _drag_buffer; + delete _auxvel_buffer; +} + // Accumulate imu data and store to buffer at desired rate void EstimatorInterface::setIMUData(const imuSample &imu_sample) { @@ -104,70 +117,59 @@ bool EstimatorInterface::checkIfVehicleAtRest(float dt, const imuSample &imu) void EstimatorInterface::setMagData(const magSample &mag_sample) { - if (!_initialised || _mag_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_mag_buffer.get_length() < _obs_buffer_length) { - _mag_buffer_fail = !_mag_buffer.allocate(_obs_buffer_length); + if (_mag_buffer == nullptr) { + _mag_buffer = new RingBuffer(_obs_buffer_length); - if (_mag_buffer_fail) { + if (_mag_buffer == nullptr || !_mag_buffer->valid()) { + delete _mag_buffer; + _mag_buffer = nullptr; printBufferAllocationFailed("mag"); return; } } - // downsample to highest possible sensor rate - // by taking the average of incoming sample - _mag_sample_count++; - _mag_data_sum += mag_sample.mag; - _mag_timestamp_sum += mag_sample.time_us / 1000; // Dividing by 1000 to avoid overflow - // limit data rate to prevent data being lost if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) { _time_last_mag = mag_sample.time_us; magSample mag_sample_new; - // Use the time in the middle of the downsampling interval for the sample - mag_sample_new.time_us = 1000 * (_mag_timestamp_sum / _mag_sample_count); + mag_sample_new.time_us = mag_sample.time_us; mag_sample_new.time_us -= static_cast(_params.mag_delay_ms * 1000); mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - mag_sample_new.mag = _mag_data_sum / _mag_sample_count; + mag_sample_new.mag = mag_sample.mag; - _mag_buffer.push(mag_sample_new); - - _mag_sample_count = 0; - _mag_data_sum.setZero(); - _mag_timestamp_sum = 0; + _mag_buffer->push(mag_sample_new); + } else { + ECL_ERR("mag data too fast %" PRIu64, mag_sample.time_us - _time_last_mag); } } void EstimatorInterface::setGpsData(const gps_message &gps) { - if (!_initialised || _gps_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_gps_buffer.get_length() < _obs_buffer_length) { - _gps_buffer_fail = !_gps_buffer.allocate(_obs_buffer_length); + if (_gps_buffer == nullptr) { + _gps_buffer = new RingBuffer(_obs_buffer_length); - if (_gps_buffer_fail) { + if (_gps_buffer == nullptr || !_gps_buffer->valid()) { + delete _gps_buffer; + _gps_buffer = nullptr; printBufferAllocationFailed("GPS"); return; } } - // limit data rate to prevent data being lost - const bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS); - - // TODO: remove checks that are not timing related - if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) { + if ((gps.time_usec - _time_last_gps) > _min_obs_interval_us) { _time_last_gps = gps.time_usec; gpsSample gps_sample_new; @@ -202,67 +204,60 @@ void EstimatorInterface::setGpsData(const gps_message &gps) gps_sample_new.pos(1) = 0.0f; } - _gps_buffer.push(gps_sample_new); + _gps_buffer->push(gps_sample_new); + } else { + ECL_ERR("GPS data too fast %" PRIu64, gps.time_usec - _time_last_gps); } } void EstimatorInterface::setBaroData(const baroSample &baro_sample) { - if (!_initialised || _baro_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_baro_buffer.get_length() < _obs_buffer_length) { - _baro_buffer_fail = !_baro_buffer.allocate(_obs_buffer_length); + if (_baro_buffer == nullptr) { + _baro_buffer = new RingBuffer(_obs_buffer_length); - if (_baro_buffer_fail) { + if (_baro_buffer == nullptr || !_baro_buffer->valid()) { + delete _baro_buffer; + _baro_buffer = nullptr; printBufferAllocationFailed("baro"); return; } } - // downsample to highest possible sensor rate - // by baro data by taking the average of incoming sample - _baro_sample_count++; - _baro_alt_sum += baro_sample.hgt; - _baro_timestamp_sum += baro_sample.time_us / 1000; // Dividing by 1000 to avoid overflow - // limit data rate to prevent data being lost if ((baro_sample.time_us - _time_last_baro) > _min_obs_interval_us) { _time_last_baro = baro_sample.time_us; - const float baro_alt_avg = _baro_alt_sum / (float)_baro_sample_count; - baroSample baro_sample_new; - baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_alt_avg); + baro_sample_new.hgt = compensateBaroForDynamicPressure(baro_sample.hgt); - // Use the time in the middle of the downsampling interval for the sample - baro_sample_new.time_us = 1000 * (_baro_timestamp_sum / _baro_sample_count); + baro_sample_new.time_us = baro_sample.time_us; baro_sample_new.time_us -= static_cast(_params.baro_delay_ms * 1000); baro_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _baro_buffer.push(baro_sample_new); - - _baro_sample_count = 0; - _baro_alt_sum = 0.0f; - _baro_timestamp_sum = 0; + _baro_buffer->push(baro_sample_new); + } else { + ECL_ERR("baro data too fast %" PRIu64, baro_sample.time_us - _time_last_baro); } } void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) { - if (!_initialised || _airspeed_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_airspeed_buffer.get_length() < _obs_buffer_length) { - _airspeed_buffer_fail = !_airspeed_buffer.allocate(_obs_buffer_length); + if (_airspeed_buffer == nullptr) { + _airspeed_buffer = new RingBuffer(_obs_buffer_length); - if (_airspeed_buffer_fail) { + if (_airspeed_buffer == nullptr || !_airspeed_buffer->valid()) { + delete _airspeed_buffer; + _airspeed_buffer = nullptr; printBufferAllocationFailed("airspeed"); return; } @@ -277,22 +272,23 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) airspeed_sample_new.time_us -= static_cast(_params.airspeed_delay_ms * 1000); airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _airspeed_buffer.push(airspeed_sample_new); + _airspeed_buffer->push(airspeed_sample_new); } } void EstimatorInterface::setRangeData(const rangeSample &range_sample) { - if (!_initialised || _range_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_range_buffer.get_length() < _obs_buffer_length) { - _range_buffer_fail = !_range_buffer.allocate(_obs_buffer_length); + if (_range_buffer == nullptr) { + _range_buffer = new RingBuffer(_obs_buffer_length); - if (_range_buffer_fail) { + if (_range_buffer == nullptr || !_range_buffer->valid()) { + delete _range_buffer; + _range_buffer = nullptr; printBufferAllocationFailed("range"); return; } @@ -306,22 +302,23 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) range_sample_new.time_us -= static_cast(_params.range_delay_ms * 1000); range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _range_buffer.push(range_sample_new); + _range_buffer->push(range_sample_new); } } void EstimatorInterface::setOpticalFlowData(const flowSample &flow) { - if (!_initialised || _flow_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_flow_buffer.get_length() < _imu_buffer_length) { - _flow_buffer_fail = !_flow_buffer.allocate(_imu_buffer_length); + if (_flow_buffer == nullptr) { + _flow_buffer = new RingBuffer(_imu_buffer_length); - if (_flow_buffer_fail) { + if (_flow_buffer == nullptr || !_flow_buffer->valid()) { + delete _flow_buffer; + _flow_buffer = nullptr; printBufferAllocationFailed("flow"); return; } @@ -336,23 +333,24 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) optflow_sample_new.time_us -= static_cast(_params.flow_delay_ms * 1000); optflow_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _flow_buffer.push(optflow_sample_new); + _flow_buffer->push(optflow_sample_new); } } // set attitude and position data derived from an external vision system void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) { - if (!_initialised || _ev_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_ext_vision_buffer.get_length() < _obs_buffer_length) { - _ev_buffer_fail = !_ext_vision_buffer.allocate(_obs_buffer_length); + if (_ext_vision_buffer == nullptr) { + _ext_vision_buffer = new RingBuffer(_obs_buffer_length); - if (_ev_buffer_fail) { + if (_ext_vision_buffer == nullptr || !_ext_vision_buffer->valid()) { + delete _ext_vision_buffer; + _ext_vision_buffer = nullptr; printBufferAllocationFailed("vision"); return; } @@ -367,22 +365,23 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) ev_sample_new.time_us -= static_cast(_params.ev_delay_ms * 1000); ev_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _ext_vision_buffer.push(ev_sample_new); + _ext_vision_buffer->push(ev_sample_new); } } void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) { - if (!_initialised || _auxvel_buffer_fail) { + if (!_initialised) { return; } // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_auxvel_buffer.get_length() < _obs_buffer_length) { - _auxvel_buffer_fail = !_auxvel_buffer.allocate(_obs_buffer_length); + if (_auxvel_buffer == nullptr) { + _auxvel_buffer = new RingBuffer(_obs_buffer_length); - if (_auxvel_buffer_fail) { + if (_auxvel_buffer == nullptr || !_auxvel_buffer->valid()) { + delete _auxvel_buffer; + _auxvel_buffer = nullptr; printBufferAllocationFailed("aux vel"); return; } @@ -397,7 +396,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) auxvel_sample_new.time_us -= static_cast(_params.auxvel_delay_ms * 1000); auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _auxvel_buffer.push(auxvel_sample_new); + _auxvel_buffer->push(auxvel_sample_new); } } @@ -405,14 +404,15 @@ void EstimatorInterface::setDragData(const imuSample &imu) { // down-sample the drag specific force data by accumulating and calculating the mean when // sufficient samples have been collected - if ((_params.fusion_mode & MASK_USE_DRAG) && !_drag_buffer_fail) { + if ((_params.fusion_mode & MASK_USE_DRAG)) { // Allocate the required buffer size if not previously done - // Do not retry if allocation has failed previously - if (_drag_buffer.get_length() < _obs_buffer_length) { - _drag_buffer_fail = !_drag_buffer.allocate(_obs_buffer_length); + if (_drag_buffer == nullptr) { + _drag_buffer = new RingBuffer(_obs_buffer_length); - if (_drag_buffer_fail) { + if (_drag_buffer == nullptr || !_drag_buffer->valid()) { + delete _drag_buffer; + _drag_buffer = nullptr; printBufferAllocationFailed("drag"); return; } @@ -440,7 +440,7 @@ void EstimatorInterface::setDragData(const imuSample &imu) _drag_down_sampled.time_us /= _drag_sample_count; // write to buffer - _drag_buffer.push(_drag_down_sampled); + _drag_buffer->push(_drag_down_sampled); // reset accumulators _drag_sample_count = 0; @@ -454,9 +454,14 @@ void EstimatorInterface::setDragData(const imuSample &imu) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle - // it's reasonable to assume that barometer is always used, and its delay is low + // it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used - float max_time_delay_ms = math::max(_params.baro_delay_ms, _params.auxvel_delay_ms); + float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms); + + // using baro + if (_params.vdist_sensor_type == 0) { + max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); + } // using airspeed if (_params.arsp_thr > FLT_EPSILON) { @@ -485,18 +490,20 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); } - // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - _imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; + // calculate the IMU buffer length required to accommodate the maximum delay with some allowance for jitter + _imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS); // set the observation buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter const float ekf_delay_ms = max_time_delay_ms * 1.5f; - _obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_ms); + _obs_buffer_length = roundf(ekf_delay_ms / FILTER_UPDATE_PERIOD_MS); // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) _obs_buffer_length = math::min(_obs_buffer_length, _imu_buffer_length); + ECL_DEBUG("EKF max time delay %.1f ms, OBS length %d\n", (double)ekf_delay_ms, _obs_buffer_length); + if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) { @@ -550,15 +557,43 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) void EstimatorInterface::print_status() { - ECL_INFO("imu buffer: %d (%d Bytes)", _imu_buffer.get_length(), _imu_buffer.get_total_size()); - ECL_INFO("gps buffer: %d (%d Bytes)", _gps_buffer.get_length(), _gps_buffer.get_total_size()); - ECL_INFO("mag buffer: %d (%d Bytes)", _mag_buffer.get_length(), _mag_buffer.get_total_size()); - ECL_INFO("baro buffer: %d (%d Bytes)", _baro_buffer.get_length(), _baro_buffer.get_total_size()); - ECL_INFO("range buffer: %d (%d Bytes)", _range_buffer.get_length(), _range_buffer.get_total_size()); - ECL_INFO("airspeed buffer: %d (%d Bytes)", _airspeed_buffer.get_length(), _airspeed_buffer.get_total_size()); - ECL_INFO("flow buffer: %d (%d Bytes)", _flow_buffer.get_length(), _flow_buffer.get_total_size()); - ECL_INFO("vision buffer: %d (%d Bytes)", _ext_vision_buffer.get_length(), _ext_vision_buffer.get_total_size()); - ECL_INFO("output buffer: %d (%d Bytes)", _output_buffer.get_length(), _output_buffer.get_total_size()); - ECL_INFO("output vert buffer: %d (%d Bytes)", _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); - ECL_INFO("drag buffer: %d (%d Bytes)", _drag_buffer.get_length(), _drag_buffer.get_total_size()); + printf("IMU average dt: %.6f seconds\n", (double)_dt_imu_avg); + printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size()); + + printf("minimum observation interval %d us\n", _min_obs_interval_us); + + if (_gps_buffer) { + printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); + } + + if (_mag_buffer) { + printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); + } + + if (_baro_buffer) { + printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); + } + + if (_range_buffer) { + printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size()); + } + + if (_airspeed_buffer) { + printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size()); + } + + if (_flow_buffer) { + printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size()); + } + + if (_ext_vision_buffer) { + printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size()); + } + + if (_drag_buffer) { + printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size()); + } + + printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size()); + printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index bfd23da1ce..65f1430b46 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -46,11 +46,18 @@ # define ECL_INFO PX4_DEBUG # define ECL_WARN PX4_DEBUG # define ECL_ERR PX4_DEBUG +# define ECL_DEBUG PX4_DEBUG #else # define ECL_INFO(X, ...) printf(X "\n", ##__VA_ARGS__) # define ECL_WARN(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) # define ECL_ERR(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) +# if defined(DEBUG_BUILD) +# define ECL_DEBUG(X, ...) fprintf(stderr, X "\n", ##__VA_ARGS__) +# else +# define ECL_DEBUG(X, ...) +#endif + #endif #include "common.h" @@ -263,7 +270,7 @@ public: protected: EstimatorInterface() = default; - virtual ~EstimatorInterface() = default; + virtual ~EstimatorInterface(); virtual bool init(uint64_t timestamp) = 0; @@ -365,15 +372,15 @@ protected: RingBuffer _output_buffer{12}; RingBuffer _output_vert_buffer{12}; - RingBuffer _gps_buffer; - RingBuffer _mag_buffer; - RingBuffer _baro_buffer; - RingBuffer _range_buffer; - RingBuffer _airspeed_buffer; - RingBuffer _flow_buffer; - RingBuffer _ext_vision_buffer; - RingBuffer _drag_buffer; - RingBuffer _auxvel_buffer; + RingBuffer *_gps_buffer{nullptr}; + RingBuffer *_mag_buffer{nullptr}; + RingBuffer *_baro_buffer{nullptr}; + RingBuffer *_range_buffer{nullptr}; + RingBuffer *_airspeed_buffer{nullptr}; + RingBuffer *_flow_buffer{nullptr}; + RingBuffer *_ext_vision_buffer{nullptr}; + RingBuffer *_drag_buffer{nullptr}; + RingBuffer *_auxvel_buffer{nullptr}; // timestamps of latest in buffer saved measurement in microseconds uint64_t _time_last_imu{0}; @@ -429,30 +436,11 @@ private: // [1] high frequency vibration level in the IMU delta angle data (rad) // [2] high frequency vibration level in the IMU delta velocity data (m/s) - // Used to down sample barometer data - uint64_t _baro_timestamp_sum{0}; // summed timestamp to provide the timestamp of the averaged sample - float _baro_alt_sum{0.0f}; // summed pressure altitude readings (m) - uint8_t _baro_sample_count{0}; // number of barometric altitude measurements summed - // Used by the multi-rotor specific drag force fusion uint8_t _drag_sample_count{0}; // number of drag specific force samples assumulated at the filter prediction rate float _drag_sample_time_dt{0.0f}; // time integral across all samples used to form _drag_down_sampled (sec) - // Used to downsample magnetometer data - uint64_t _mag_timestamp_sum{0}; - Vector3f _mag_data_sum{}; - uint8_t _mag_sample_count{0}; - // observation buffer final allocation failed - bool _gps_buffer_fail{false}; - bool _mag_buffer_fail{false}; - bool _baro_buffer_fail{false}; - bool _range_buffer_fail{false}; - bool _airspeed_buffer_fail{false}; - bool _flow_buffer_fail{false}; - bool _ev_buffer_fail{false}; - bool _drag_buffer_fail{false}; - bool _auxvel_buffer_fail{false}; - + uint64_t _last_allocation_fail_print{0}; }; #endif // !EKF_ESTIMATOR_INTERFACE_H diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index ff8d442520..9d16b2aa6d 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -57,7 +57,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)), _wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)), _params(_ekf.getParamHandle()), - _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), _param_ekf2_mag_delay(_params->mag_delay_ms), _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_gps_delay(_params->gps_delay_ms), @@ -243,6 +242,10 @@ int EKF2::print_status() perf_print_counter(_msg_missed_odometry_perf); perf_print_counter(_msg_missed_optical_flow_perf); +#if defined(DEBUG_BUILD) + _ekf.print_status(); +#endif // DEBUG_BUILD + return 0; } @@ -272,6 +275,38 @@ void EKF2::Run() if (param_aspd_scale != PARAM_INVALID) { param_get(param_aspd_scale, &_airspeed_scale_factor); } + + // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output + if (_params->vdist_sensor_type == 0) { + float sens_baro_rate = 0.f; + + if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) { + if (sens_baro_rate > 0) { + float interval_ms = roundf(1000.f / sens_baro_rate); + + if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { + PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); + _params->sensor_interval_max_ms = interval_ms; + } + } + } + } + + // if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output + if (_params->mag_fusion_type != MAG_FUSE_TYPE_NONE) { + float sens_mag_rate = 0.f; + + if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) { + if (sens_mag_rate > 0) { + float interval_ms = roundf(1000.f / sens_mag_rate); + + if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { + PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); + _params->sensor_interval_max_ms = interval_ms; + } + } + } + } } if (!_callback_registered) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 55e9754c00..7e09533bd2 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -315,8 +315,6 @@ private: parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) DEFINE_PARAMETERS( - (ParamExtInt) - _param_ekf2_min_obs_dt, ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) (ParamExtFloat) _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 29f7a56ac8..ba474d1566 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -39,19 +39,6 @@ * */ -/** - * Minimum time of arrival delta between non-IMU observations before data is downsampled. - * - * Baro and Magnetometer data will be averaged before downsampling, other data will be point sampled resulting in loss of information. - * - * @group EKF2 - * @min 10 - * @max 50 - * @reboot_required true - * @unit ms - */ -PARAM_DEFINE_INT32(EKF2_MIN_OBS_DT, 20); - /** * Magnetometer measurement delay relative to IMU measurements * diff --git a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp index 0839a9ea18..44cf3a2e43 100644 --- a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp +++ b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp @@ -46,11 +46,11 @@ class EkfRingBufferTest : public ::testing::Test public: sample _x, _y, _z; - RingBuffer *_buffer; + RingBuffer *_buffer{nullptr}; void SetUp() override { - _buffer = new RingBuffer(); + _buffer = new RingBuffer(3); _x.time_us = 1000000; _x.data[0] = _x.data[1] = _x.data[2] = 1.5f;