diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 2061f080c6..bc36fcb120 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -45,14 +45,8 @@ template class RingBuffer { public: - RingBuffer() - { - if (allocate(1)) { - // initialize with one empty sample - data_type d = {}; - push(d); - } - } + explicit RingBuffer(size_t size) { allocate(size); } + RingBuffer() { allocate(1); } ~RingBuffer() { delete[] _buffer; } // no copy, assignment, move, move assignment @@ -63,12 +57,20 @@ public: bool allocate(uint8_t size) { + if (valid() && (size == _size)) { + // no change + return true; + } + + if (size == 0) { + return false; + } if (_buffer != nullptr) { delete[] _buffer; } - _buffer = new data_type[size]; + _buffer = new data_type[size]{}; if (_buffer == nullptr) { return false; @@ -79,26 +81,15 @@ public: _head = 0; _tail = 0; - // set the time elements to zero so that bad data is not - // retrieved from the buffers - for (uint8_t index = 0; index < _size; index++) { - _buffer[index] = {}; - } - _first_write = true; return true; } - void unallocate() - { - delete[] _buffer; - _buffer = nullptr; - } + bool valid() const { return (_buffer != nullptr) && (_size > 0); } void push(const data_type &sample) { - uint8_t head_new = _head; if (!_first_write) { @@ -121,8 +112,8 @@ public: data_type &operator[](const uint8_t index) { return _buffer[index]; } - const data_type &get_newest() { return _buffer[_head]; } - const data_type &get_oldest() { return _buffer[_tail]; } + const data_type &get_newest() const { return _buffer[_head]; } + const data_type &get_oldest() const { return _buffer[_tail]; } uint8_t get_oldest_index() const { return _tail; } @@ -162,7 +153,7 @@ public: return false; } - int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; } + int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; } private: data_type *_buffer{nullptr}; diff --git a/EKF/common.h b/EKF/common.h index b396f332e0..45d9b485f2 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -55,10 +55,13 @@ using matrix::Vector2f; using matrix::Vector3f; using matrix::wrap_pi; -enum velocity_frame_t {LOCAL_FRAME_FRD, BODY_FRAME_FRD}; +enum class velocity_frame_t : uint8_t { + LOCAL_FRAME_FRD, + BODY_FRAME_FRD +}; struct gps_message { - uint64_t time_usec; + uint64_t time_usec{0}; int32_t lat; ///< Latitude in 1E-7 degrees int32_t lon; ///< Longitude in 1E-7 degrees int32_t alt; ///< Altitude in 1E-3 meters (millimeters) above MSL @@ -76,29 +79,30 @@ struct gps_message { }; struct outputSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Quatf quat_nominal; ///< nominal quaternion describing vehicle attitude Vector3f vel; ///< NED velocity estimate in earth frame (m/sec) Vector3f pos; ///< NED position estimate in earth frame (m/sec) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct outputVert { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) float vert_vel; ///< Vertical velocity calculated using alternative algorithm (m/sec) float vert_vel_integ; ///< Integral of vertical velocity (m) float dt; ///< delta time (sec) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct imuSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector3f delta_ang; ///< delta angle in body frame (integrated gyro measurements) (rad) Vector3f delta_vel; ///< delta velocity in body frame (integrated accelerometer measurements) (m/sec) float delta_ang_dt; ///< delta angle integration period (sec) float delta_vel_dt; ///< delta velocity integration period (sec) - uint64_t time_us; ///< timestamp of the measurement (uSec) bool delta_vel_clipping[3]{}; ///< true (per axis) if this sample contained any accelerometer clipping }; struct gpsSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector2f pos; ///< NE earth frame gps horizontal position measurement (m) float hgt; ///< gps height measurement (m) Vector3f vel; ///< NED earth frame gps velocity measurement (m/sec) @@ -106,59 +110,58 @@ struct gpsSample { float hacc; ///< 1-std horizontal position error (m) float vacc; ///< 1-std vertical position error (m) float sacc; ///< 1-std speed error (m/sec) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct magSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector3f mag; ///< NED magnetometer body frame measurements (Gauss) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct baroSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) float hgt; ///< pressure altitude above sea level (m) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct rangeSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) float rng; ///< range (distance to ground) measurement (m) - uint64_t time_us; ///< timestamp of the measurement (uSec) int8_t quality; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. }; struct airspeedSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) float true_airspeed; ///< true airspeed measurement (m/sec) float eas2tas; ///< equivalent to true airspeed factor - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct flowSample { - uint8_t quality; ///< quality indicator between 0 and 255 + uint64_t time_us{0}; ///< timestamp of the integration period leading edge (uSec) Vector2f flow_xy_rad; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive Vector3f gyro_xyz; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt; ///< amount of integration time (sec) - uint64_t time_us; ///< timestamp of the integration period leading edge (uSec) + uint8_t quality; ///< quality indicator between 0 and 255 }; struct extVisionSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis Vector3f vel; ///< FRD velocity in reference frame defined in vel_frame variable (m/sec) - Z must be aligned with down axis Quatf quat; ///< quaternion defining rotation from body to earth frame Vector3f posVar; ///< XYZ position variances (m**2) Matrix3f velCov; ///< XYZ velocity covariances ((m/sec)**2) float angVar; ///< angular heading variance (rad**2) - velocity_frame_t vel_frame = BODY_FRAME_FRD; - uint64_t time_us; ///< timestamp of the measurement (uSec) + velocity_frame_t vel_frame = velocity_frame_t::BODY_FRAME_FRD; }; struct dragSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector2f accelXY; ///< measured specific force along the X and Y body axes (m/sec**2) - uint64_t time_us; ///< timestamp of the measurement (uSec) }; struct auxVelSample { + uint64_t time_us{0}; ///< timestamp of the measurement (uSec) Vector3f vel; ///< measured NE velocity relative to the local origin (m/sec) Vector3f velVar; ///< estimated error variance of the NE velocity (m/sec)**2 - uint64_t time_us; ///< timestamp of the measurement (uSec) }; // Integer definitions for vdist_sensor_type @@ -218,15 +221,14 @@ struct parameters { 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) // measurement time delays - float min_delay_ms{0.0f}; ///< Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) float airspeed_delay_ms{100.0f}; ///< airspeed measurement delay relative to the IMU (mSec) float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) - float ev_delay_ms{100.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) - float auxvel_delay_ms{0.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec) + float ev_delay_ms{175.0f}; ///< off-board vision measurement delay relative to the IMU (mSec) + float auxvel_delay_ms{5.0f}; ///< auxiliary velocity measurement delay relative to the IMU (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 26510cf1f0..11c608290a 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1230,10 +1230,10 @@ Vector3f Ekf::getVisionVelocityInEkfFrame() const // rotate measurement into correct earth frame if required switch(_ev_sample_delayed.vel_frame) { - case BODY_FRAME_FRD: + case velocity_frame_t::BODY_FRAME_FRD: vel = _R_to_earth * (_ev_sample_delayed.vel - vel_offset_body); break; - case LOCAL_FRAME_FRD: + case velocity_frame_t::LOCAL_FRAME_FRD: const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; if (_params.fusion_mode & MASK_ROTATE_EV) { @@ -1253,11 +1253,11 @@ Vector3f Ekf::getVisionVelocityVarianceInEkfFrame() const // rotate measurement into correct earth frame if required switch(_ev_sample_delayed.vel_frame) { - case BODY_FRAME_FRD: + case velocity_frame_t::BODY_FRAME_FRD: ev_vel_cov = _R_to_earth * ev_vel_cov * _R_to_earth.transpose(); break; - case LOCAL_FRAME_FRD: + case velocity_frame_t::LOCAL_FRAME_FRD: if(_params.fusion_mode & MASK_ROTATE_EV) { ev_vel_cov = _R_ev_to_ekf * ev_vel_cov * _R_ev_to_ekf.transpose(); diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ea945c98e3..986bc71c52 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -512,34 +512,46 @@ void EstimatorInterface::setDragData(const imuSample &imu) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { // find the maximum time delay the buffers are required to handle - const uint16_t max_time_delay_ms = math::max(_params.mag_delay_ms, - math::max(_params.range_delay_ms, - math::max(_params.gps_delay_ms, - math::max(_params.flow_delay_ms, - math::max(_params.ev_delay_ms, - math::max(_params.auxvel_delay_ms, - math::max(_params.min_delay_ms, - math::max(_params.airspeed_delay_ms, - _params.baro_delay_ms)))))))); + float max_time_delay_ms = math::max(_params.baro_delay_ms, + math::max(_params.auxvel_delay_ms, + _params.airspeed_delay_ms)); + + // mag mode + if (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE) { + max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); + } + + // range aid or range height + if (_params.range_aid || (_params.vdist_sensor_type == VDIST_SENSOR_RANGE)) { + max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); + } + + if (_params.fusion_mode & MASK_USE_GPS) { + max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); + } + + if (_params.fusion_mode & MASK_USE_OF) { + max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); + } + + if (_params.fusion_mode & (MASK_USE_EVPOS | MASK_USE_EVYAW | MASK_USE_EVVEL)) { + 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 = (max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; + _imu_buffer_length = ceilf(max_time_delay_ms / FILTER_UPDATE_PERIOD_MS) + 1; // 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 uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((float)max_time_delay_ms * 0.5f)); - _obs_buffer_length = (ekf_delay_ms / _params.sensor_interval_min_ms) + 1; + const float ekf_delay_ms = max_time_delay_ms * 1.5f; + _obs_buffer_length = ceilf(ekf_delay_ms / _params.sensor_interval_min_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); - if (!(_imu_buffer.allocate(_imu_buffer_length) && - _output_buffer.allocate(_imu_buffer_length) && - _output_vert_buffer.allocate(_imu_buffer_length))) { - - printBufferAllocationFailed(""); - unallocate_buffers(); + if (!_imu_buffer.allocate(_imu_buffer_length) || !_output_buffer.allocate(_imu_buffer_length) || !_output_vert_buffer.allocate(_imu_buffer_length)) { + printBufferAllocationFailed("IMU and output"); return false; } @@ -553,22 +565,6 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) return true; } -void EstimatorInterface::unallocate_buffers() -{ - _imu_buffer.unallocate(); - _gps_buffer.unallocate(); - _mag_buffer.unallocate(); - _baro_buffer.unallocate(); - _range_buffer.unallocate(); - _airspeed_buffer.unallocate(); - _flow_buffer.unallocate(); - _ext_vision_buffer.unallocate(); - _output_buffer.unallocate(); - _output_vert_buffer.unallocate(); - _drag_buffer.unallocate(); - _auxvel_buffer.unallocate(); -} - bool EstimatorInterface::isOnlyActiveSourceOfHorizontalAiding(const bool aiding_flag) const { return aiding_flag && !isOtherSourceOfHorizontalAidingThan(aiding_flag); diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index d7b9b352bc..45465477a3 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -323,7 +323,10 @@ protected: bool _gps_drift_updated{false}; // true when _gps_drift_metrics has been updated and is ready for retrieval // data buffer instances - RingBuffer _imu_buffer; + RingBuffer _imu_buffer{12}; // buffer length 12 with default parameters + RingBuffer _output_buffer{12}; + RingBuffer _output_vert_buffer{12}; + RingBuffer _gps_buffer; RingBuffer _mag_buffer; RingBuffer _baro_buffer; @@ -331,8 +334,6 @@ protected: RingBuffer _airspeed_buffer; RingBuffer _flow_buffer; RingBuffer _ext_vision_buffer; - RingBuffer _output_buffer; - RingBuffer _output_vert_buffer; RingBuffer _drag_buffer; RingBuffer _auxvel_buffer; @@ -375,9 +376,6 @@ private: void printBufferAllocationFailed(const char *buffer_name); - // free buffer memory - void unallocate_buffers(); - ImuDownSampler _imu_down_sampler{FILTER_UPDATE_PERIOD_S}; unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) diff --git a/test/sensor_simulator/baro.cpp b/test/sensor_simulator/baro.cpp index a5cdde0b4e..75a25a7cd4 100644 --- a/test/sensor_simulator/baro.cpp +++ b/test/sensor_simulator/baro.cpp @@ -15,8 +15,7 @@ Baro::~Baro() void Baro::send(uint64_t time) { - const baroSample baro_sample {_baro_data, time}; - _ekf->setBaroData(baro_sample); + _ekf->setBaroData(baroSample{time, _baro_data}); } void Baro::setData(float baro) diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp index b40dad9773..f40b9117a6 100644 --- a/test/sensor_simulator/ekf_wrapper.cpp +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -17,9 +17,7 @@ void EkfWrapper::setBaroHeight() bool EkfWrapper::isIntendingBaroHeightFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.baro_hgt; + return _ekf->control_status_flags().baro_hgt; } void EkfWrapper::setGpsHeight() @@ -29,9 +27,7 @@ void EkfWrapper::setGpsHeight() bool EkfWrapper::isIntendingGpsHeightFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.gps_hgt; + return _ekf->control_status_flags().gps_hgt; } void EkfWrapper::setRangeHeight() @@ -41,9 +37,7 @@ void EkfWrapper::setRangeHeight() bool EkfWrapper::isIntendingRangeHeightFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.rng_hgt; + return _ekf->control_status_flags().rng_hgt; } void EkfWrapper::setVisionHeight() @@ -53,9 +47,7 @@ void EkfWrapper::setVisionHeight() bool EkfWrapper::isIntendingVisionHeightFusion() const { - filter_control_status_u control_status; - _ekf->get_control_mode(&control_status.value); - return control_status.flags.ev_hgt; + return _ekf->control_status_flags().ev_hgt; } void EkfWrapper::enableGpsFusion() diff --git a/test/sensor_simulator/mag.cpp b/test/sensor_simulator/mag.cpp index 3665feffc9..482af9c179 100644 --- a/test/sensor_simulator/mag.cpp +++ b/test/sensor_simulator/mag.cpp @@ -15,8 +15,7 @@ Mag::~Mag() void Mag::send(uint64_t time) { - const magSample mag_sample {_mag_data, time}; - _ekf->setMagData(mag_sample); + _ekf->setMagData(magSample{time, _mag_data}); } void Mag::setData(const Vector3f& mag) diff --git a/test/sensor_simulator/vio.cpp b/test/sensor_simulator/vio.cpp index 587ce51c2c..cf8fd52e34 100644 --- a/test/sensor_simulator/vio.cpp +++ b/test/sensor_simulator/vio.cpp @@ -61,15 +61,14 @@ void Vio::setOrientation(const Quatf& quat) void Vio::setVelocityFrameToBody() { - _vio_data.vel_frame = BODY_FRAME_FRD; + _vio_data.vel_frame = velocity_frame_t::BODY_FRAME_FRD; } void Vio::setVelocityFrameToLocal() { - _vio_data.vel_frame = LOCAL_FRAME_FRD; + _vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD; } - extVisionSample Vio::dataAtRest() { extVisionSample vio_data; @@ -79,7 +78,7 @@ extVisionSample Vio::dataAtRest() vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f}; vio_data.velCov = matrix::eye() * 0.1f; vio_data.angVar = 0.05f; - vio_data.vel_frame = LOCAL_FRAME_FRD; + vio_data.vel_frame = velocity_frame_t::LOCAL_FRAME_FRD; return vio_data; } diff --git a/test/test_EKF_measurementSampling.cpp b/test/test_EKF_measurementSampling.cpp index beb78c96a0..d053d0d17c 100644 --- a/test/test_EKF_measurementSampling.cpp +++ b/test/test_EKF_measurementSampling.cpp @@ -66,8 +66,7 @@ TEST_F(EkfMeasurementSamplingTest, baroDownSampling) imu_sample.time_us = time; _ekf->setIMUData(imu_sample); } - const baroSample baro_sample {baro_data, time}; - _ekf->setBaroData(baro_sample); + _ekf->setBaroData(baroSample{time, baro_data}); baro_data *= -1.0f; time += 1000000 / baro_rate_Hz; } diff --git a/test/test_EKF_ringbuffer.cpp b/test/test_EKF_ringbuffer.cpp index cb83a0f44b..fc26d9187b 100644 --- a/test/test_EKF_ringbuffer.cpp +++ b/test/test_EKF_ringbuffer.cpp @@ -62,7 +62,6 @@ class EkfRingBufferTest : public ::testing::Test { void TearDown() override { - _buffer->unallocate(); delete _buffer; } }; diff --git a/test/test_SensorRangeFinder.cpp b/test/test_SensorRangeFinder.cpp index 35fbe73a63..aaa8029bf6 100644 --- a/test/test_SensorRangeFinder.cpp +++ b/test/test_SensorRangeFinder.cpp @@ -59,7 +59,7 @@ public: protected: SensorRangeFinder _range_finder{}; - const rangeSample _good_sample{1.f, (uint64_t)2e6, 100}; // {range, time_us, quality} + const rangeSample _good_sample{(uint64_t)2e6, 1.f, 100}; // {time_us, range, quality} const float _min_range{0.5f}; const float _max_range{10.f};