diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index b2968034b9..9ddf27ee47 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -51,9 +51,9 @@ #include #include -#include #include +#include #include #include #include @@ -63,9 +63,8 @@ #include #include - -// 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); }