forked from Archive/PX4-Autopilot
df_mpu9250_wrapper: use FIFO time offsets
- Make use of the FIFO time offsets provided by the MPU9250 driver. It allows to use proper dt for the integration. - Got rid of the unnecessary perf_counters for now. - Properly use the changed integrator lib. - Provide integral_dt for ekf2.
This commit is contained in:
Julian Oes
Lorenz Meier
parent
d824ca3a14
commit
75444ae084
|
|
@ -51,9 +51,9 @@
|
|||
#include <px4_getopt.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/device/integrator.h>
|
||||
|
|
@ -63,9 +63,8 @@
|
|||
#include <mpu9250/MPU9250.hpp>
|
||||
#include <DevMgr.hpp>
|
||||
|
||||
|
||||
// publish frequency of 250 Hz
|
||||
#define MPU9250_PUBLISH_INTERVAL_US 4000
|
||||
// We don't want to auto publish, therefore set this to 0.
|
||||
#define MPU9250_NEVER_AUTOPUBLISH_US 0
|
||||
|
||||
|
||||
extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
|
||||
|
|
@ -131,9 +130,8 @@ private:
|
|||
Integrator _accel_int;
|
||||
Integrator _gyro_int;
|
||||
|
||||
perf_counter_t _accel_sample_perf;
|
||||
perf_counter_t _gyro_sample_perf;
|
||||
|
||||
unsigned _publish_count;
|
||||
hrt_abstime _first_sample_time;
|
||||
};
|
||||
|
||||
DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
|
||||
|
|
@ -145,11 +143,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
|
|||
_gyro_calibration{},
|
||||
_accel_orb_class_instance(-1),
|
||||
_gyro_orb_class_instance(-1),
|
||||
_accel_int(MPU9250_PUBLISH_INTERVAL_US, false),
|
||||
_gyro_int(MPU9250_PUBLISH_INTERVAL_US, true),
|
||||
_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
|
||||
_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
|
||||
_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
|
||||
_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
|
||||
/*_rotation(rotation)*/
|
||||
_publish_count(0),
|
||||
_first_sample_time(0)
|
||||
{
|
||||
// Set sane default calibration values
|
||||
_accel_calibration.x_scale = 1.0f;
|
||||
|
|
@ -169,8 +167,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
|
|||
|
||||
DfMpu9250Wrapper::~DfMpu9250Wrapper()
|
||||
{
|
||||
perf_free(_accel_sample_perf);
|
||||
perf_free(_gyro_sample_perf);
|
||||
}
|
||||
|
||||
int DfMpu9250Wrapper::start()
|
||||
|
|
@ -390,8 +386,6 @@ void DfMpu9250Wrapper::_update_accel_calibration()
|
|||
|
||||
int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
||||
{
|
||||
bool should_notify = false;
|
||||
|
||||
/* Check if calibration values are still up-to-date. */
|
||||
bool updated;
|
||||
orb_check(_param_update_sub, &updated);
|
||||
|
|
@ -404,16 +398,17 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
|||
_update_gyro_calibration();
|
||||
}
|
||||
|
||||
/* Publish accel first. */
|
||||
perf_begin(_accel_sample_perf);
|
||||
|
||||
accel_report accel_report = {};
|
||||
accel_report.timestamp = hrt_absolute_time();
|
||||
gyro_report gyro_report = {};
|
||||
|
||||
// Only take a timestamp once for a series of consecutive samples.
|
||||
// 0 is the reset value.
|
||||
if (_first_sample_time == 0) {
|
||||
_first_sample_time = hrt_absolute_time();
|
||||
}
|
||||
|
||||
accel_report.timestamp = gyro_report.timestamp = _first_sample_time + data.time_offset_us;
|
||||
|
||||
// TODO: remove these (or get the values)
|
||||
accel_report.x_raw = NAN;
|
||||
accel_report.y_raw = NAN;
|
||||
accel_report.z_raw = NAN;
|
||||
accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale;
|
||||
accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale;
|
||||
accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale;
|
||||
|
|
@ -421,47 +416,13 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
|||
math::Vector<3> accel_val(accel_report.x,
|
||||
accel_report.y,
|
||||
accel_report.z);
|
||||
math::Vector<3> accel_val_integrated;
|
||||
math::Vector<3> accel_val_integrated_unused;
|
||||
|
||||
const bool should_publish_accel = _accel_int.put(accel_report.timestamp,
|
||||
accel_val,
|
||||
accel_val_integrated,
|
||||
accel_report.integral_dt);
|
||||
_accel_int.put(accel_report.timestamp,
|
||||
accel_val,
|
||||
accel_val_integrated_unused,
|
||||
accel_report.integral_dt);
|
||||
|
||||
accel_report.x_integral = accel_val_integrated(0);
|
||||
accel_report.y_integral = accel_val_integrated(1);
|
||||
accel_report.z_integral = accel_val_integrated(2);
|
||||
|
||||
|
||||
// TODO: get these right
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
|
||||
accel_report.device_id = m_id.dev_id;
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked) && should_publish_accel) {
|
||||
|
||||
if (_accel_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
|
||||
should_notify = true;
|
||||
}
|
||||
|
||||
perf_end(_accel_sample_perf);
|
||||
|
||||
|
||||
/* Then publish gyro. */
|
||||
perf_begin(_gyro_sample_perf);
|
||||
|
||||
gyro_report gyro_report = {};
|
||||
gyro_report.timestamp = hrt_absolute_time();
|
||||
|
||||
// TODO: remove these (or get the values)
|
||||
gyro_report.x_raw = NAN;
|
||||
gyro_report.y_raw = NAN;
|
||||
gyro_report.z_raw = NAN;
|
||||
gyro_report.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
|
||||
gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
|
||||
gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
|
||||
|
|
@ -469,38 +430,72 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
|||
math::Vector<3> gyro_val(gyro_report.x,
|
||||
gyro_report.y,
|
||||
gyro_report.z);
|
||||
math::Vector<3> gyro_val_integrated(gyro_report.x,
|
||||
gyro_report.y,
|
||||
gyro_report.z);
|
||||
math::Vector<3> gyro_val_integrated_unused;
|
||||
|
||||
const bool should_publish_gyro = _gyro_int.put(gyro_report.timestamp,
|
||||
gyro_val,
|
||||
gyro_val_integrated,
|
||||
gyro_report.integral_dt);
|
||||
_gyro_int.put(gyro_report.timestamp,
|
||||
gyro_val,
|
||||
gyro_val_integrated_unused,
|
||||
gyro_report.integral_dt);
|
||||
|
||||
/* If the time offset is 0, we are receiving the latest sample and can publish,
|
||||
* at least every 4th time because the driver is running at 1kHz but we should
|
||||
* only publish at 250 Hz. */
|
||||
if (data.time_offset_us != 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* reset the first sample time to the reset value */
|
||||
_first_sample_time = 0;
|
||||
|
||||
// Bail out if it's not the 4th time yet.
|
||||
++_publish_count;
|
||||
if (_publish_count < 4) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
_publish_count = 0;
|
||||
|
||||
// TODO: get these right
|
||||
gyro_report.scaling = -1.0f;
|
||||
gyro_report.range_rad_s = -1.0f;
|
||||
gyro_report.device_id = m_id.dev_id;
|
||||
|
||||
accel_report.scaling = -1.0f;
|
||||
accel_report.range_m_s2 = -1.0f;
|
||||
accel_report.device_id = m_id.dev_id;
|
||||
|
||||
// TODO: remove these (or get the values)
|
||||
gyro_report.x_raw = NAN;
|
||||
gyro_report.y_raw = NAN;
|
||||
gyro_report.z_raw = NAN;
|
||||
|
||||
accel_report.x_raw = NAN;
|
||||
accel_report.y_raw = NAN;
|
||||
accel_report.z_raw = NAN;
|
||||
|
||||
// Read and reset.
|
||||
math::Vector<3> gyro_val_integrated = _gyro_int.get(true, gyro_report.integral_dt);
|
||||
math::Vector<3> accel_val_integrated = _accel_int.get(true, accel_report.integral_dt);
|
||||
|
||||
gyro_report.x_integral = gyro_val_integrated(0);
|
||||
gyro_report.y_integral = gyro_val_integrated(1);
|
||||
gyro_report.z_integral = gyro_val_integrated(2);
|
||||
|
||||
// TODO: get these right
|
||||
gyro_report.scaling = -1.0f;
|
||||
gyro_report.range_rad_s = -1.0f;
|
||||
|
||||
gyro_report.device_id = m_id.dev_id;
|
||||
accel_report.x_integral = accel_val_integrated(0);
|
||||
accel_report.y_integral = accel_val_integrated(1);
|
||||
accel_report.z_integral = accel_val_integrated(2);
|
||||
|
||||
// TODO: when is this ever blocked?
|
||||
if (!(m_pub_blocked) && should_publish_gyro) {
|
||||
if (!(m_pub_blocked)) {
|
||||
|
||||
if (_gyro_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
|
||||
}
|
||||
|
||||
should_notify = true;
|
||||
}
|
||||
if (_accel_topic != nullptr) {
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
}
|
||||
|
||||
perf_end(_gyro_sample_perf);
|
||||
|
||||
if (should_notify) {
|
||||
/* Notify anyone waiting for data. */
|
||||
DevMgr::updateNotify(*this);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue