ekf2: move IMU down sampler to imu_down_sampler/

src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp

@@ -0,0 +1,69 @@
+#include "imu_down_sampler/imu_down_sampler.hpp"
+
+#include <lib/mathlib/mathlib.h>
+
+ImuDownSampler::ImuDownSampler(int32_t &target_dt_us) : _target_dt_us(target_dt_us)
+{
+	reset();
+}
+
+// integrate imu samples until target dt reached
+// assumes that dt of the gyroscope is close to the dt of the accelerometer
+// returns true if target dt is reached
+bool ImuDownSampler::update(const imuSample &imu_sample_new)
+{
+	_delta_ang_dt_avg = 0.9f * _delta_ang_dt_avg + 0.1f * imu_sample_new.delta_ang_dt;
+
+	// accumulate time deltas
+	_imu_down_sampled.time_us = imu_sample_new.time_us;
+	_imu_down_sampled.delta_ang_dt += imu_sample_new.delta_ang_dt;
+	_imu_down_sampled.delta_vel_dt += imu_sample_new.delta_vel_dt;
+	_imu_down_sampled.delta_vel_clipping[0] |= imu_sample_new.delta_vel_clipping[0];
+	_imu_down_sampled.delta_vel_clipping[1] |= imu_sample_new.delta_vel_clipping[1];
+	_imu_down_sampled.delta_vel_clipping[2] |= imu_sample_new.delta_vel_clipping[2];
+
+	// use a quaternion to accumulate delta angle data
+	// this quaternion represents the rotation from the start to end of the accumulation period
+	const Quatf delta_q(AxisAnglef(imu_sample_new.delta_ang));
+	_delta_angle_accumulated = _delta_angle_accumulated * delta_q;
+	_delta_angle_accumulated.normalize();
+
+	// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
+	const Dcmf delta_R(delta_q.inversed());
+	_imu_down_sampled.delta_vel = delta_R * _imu_down_sampled.delta_vel;
+
+	// accumulate the most recent delta velocity data at the updated rotation frame
+	// assume effective sample time is halfway between the previous and current rotation frame
+	_imu_down_sampled.delta_vel += (imu_sample_new.delta_vel + delta_R * imu_sample_new.delta_vel) * 0.5f;
+
+	_accumulated_samples++;
+
+
+	// required number of samples accumulated and the total time is at least half of the target
+	//  OR total time already exceeds the target
+	if ((_accumulated_samples >= _required_samples && _imu_down_sampled.delta_ang_dt > _min_dt_s)
+	    || (_imu_down_sampled.delta_ang_dt > _target_dt_s)) {
+
+		_imu_down_sampled.delta_ang = AxisAnglef(_delta_angle_accumulated);
+		return true;
+	}
+
+	return false;
+}
+
+void ImuDownSampler::reset()
+{
+	_imu_down_sampled = {};
+	_delta_angle_accumulated.setIdentity();
+	_accumulated_samples = 0;
+
+	// target dt in seconds safely constrained
+	float target_dt_s = math::constrain(_target_dt_us, (int32_t)1000, (int32_t)100000) * 1e-6f;
+
+	_required_samples = math::max((int)roundf(target_dt_s / _delta_ang_dt_avg), 1);
+
+	_target_dt_s = _required_samples * _delta_ang_dt_avg;
+
+	// minimum delta angle dt (in addition to number of samples)
+	_min_dt_s = math::max(_delta_ang_dt_avg * (_required_samples - 1.f), _delta_ang_dt_avg * 0.5f);
+}

src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019-2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Downsamples IMU data to a lower rate such that EKF predicition can happen less frequent
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#ifndef EKF_IMU_DOWN_SAMPLER_HPP
+#define EKF_IMU_DOWN_SAMPLER_HPP
+
+#include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
+
+#include "../common.h"
+
+using namespace estimator;
+
+class ImuDownSampler
+{
+public:
+	explicit ImuDownSampler(int32_t &target_dt_us);
+	~ImuDownSampler() = default;
+
+	bool update(const imuSample &imu_sample_new);
+
+	imuSample getDownSampledImuAndTriggerReset()
+	{
+		imuSample imu{_imu_down_sampled};
+		reset();
+		return imu;
+	}
+
+private:
+	void reset();
+
+	imuSample _imu_down_sampled{};
+	Quatf _delta_angle_accumulated{};
+
+	int _accumulated_samples{0};
+	int _required_samples{1};
+
+	int32_t &_target_dt_us;
+
+	float _target_dt_s{0.010f};
+	float _min_dt_s{0.005f};
+
+	float _delta_ang_dt_avg{0.005f};
+};
+#endif // !EKF_IMU_DOWN_SAMPLER_HPP