From 0a22a9c47cc7d1ae91c8bda69ff438a44a5a2bdc Mon Sep 17 00:00:00 2001 From: Nicolae Rosia Date: Wed, 12 Jul 2017 17:38:35 +0300 Subject: [PATCH] change gyro & accel dt from float to uint64. This has the benefit of calculating the estimator timeslip correctly. Signed-off-by: Nicolae Rosia --- msg/ekf2_replay.msg | 4 ++-- msg/sensor_combined.msg | 4 ++-- .../ekf_att_pos_estimator_main.cpp | 8 ++++---- src/modules/ekf2/ekf2_main.cpp | 20 ++++++++++--------- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- src/modules/sensors/voted_sensors_update.cpp | 8 ++++---- 6 files changed, 25 insertions(+), 23 deletions(-) diff --git a/msg/ekf2_replay.msg b/msg/ekf2_replay.msg index 0276ce3d43..77eec78d46 100644 --- a/msg/ekf2_replay.msg +++ b/msg/ekf2_replay.msg @@ -1,6 +1,6 @@ -float32 gyro_integral_dt # gyro integration period in s -float32 accelerometer_integral_dt # accelerometer integration period in s +uint64 gyro_integral_dt # gyro integration period in s +uint64 accelerometer_integral_dt # accelerometer integration period in s uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us uint64 baro_timestamp # timestamp of barometer measurement in us uint64 rng_timestamp # timestamp of range finder measurement in us diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 71c729867e..e5a4508941 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -10,11 +10,11 @@ int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relat # gyro timstamp is equal to the timestamp of the message float32[3] gyro_rad # average angular rate measured in the XYZ body frame in rad/s over the last gyro sampling period -float32 gyro_integral_dt # gyro measurement sampling period in s +uint64 gyro_integral_dt # gyro measurement sampling period in s int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp float32[3] accelerometer_m_s2 # average value acceleration measured in the XYZ body frame in m/s/s over the last accelerometer sampling period -float32 accelerometer_integral_dt # accelerometer measurement sampling period in s +uint64 accelerometer_integral_dt # accelerometer measurement sampling period in s int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index c027dbeb2a..c3e39b7254 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1359,7 +1359,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->angRate.y = _sensor_combined.gyro_rad[1]; _ekf->angRate.z = _sensor_combined.gyro_rad[2]; - float gyro_dt = _sensor_combined.gyro_integral_dt; + float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f; _ekf->dAngIMU = _ekf->angRate * gyro_dt; perf_count(_perf_gyro); @@ -1370,7 +1370,7 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; - float accel_dt = _sensor_combined.accelerometer_integral_dt; + float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f; _ekf->dVelIMU = _ekf->accel * accel_dt; _last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative; @@ -1394,8 +1394,8 @@ void AttitudePositionEstimatorEKF::pollData() // leave this in as long as larger improvements are still being made. #if 0 - float deltaTIntegral = _sensor_combined.gyro_integral_dt; - float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt; + float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1.e6f; + float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1.e6f; static unsigned dtoverflow5 = 0; static unsigned dtoverflow10 = 0; diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 3c2657ed4c..63d9b72003 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -582,14 +582,16 @@ void Ekf2::run() // push imu data into estimator float gyro_integral[3]; - gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt; - gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt; - gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt; + float gyro_dt = sensors.gyro_integral_dt / 1.e6f; + gyro_integral[0] = sensors.gyro_rad[0] * gyro_dt; + gyro_integral[1] = sensors.gyro_rad[1] * gyro_dt; + gyro_integral[2] = sensors.gyro_rad[2] * gyro_dt; float accel_integral[3]; - accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt; - accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt; - accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt; - _ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f, + float accel_dt = sensors.accelerometer_integral_dt / 1.e6f; + accel_integral[0] = sensors.accelerometer_m_s2[0] * accel_dt; + accel_integral[1] = sensors.accelerometer_m_s2[1] * accel_dt; + accel_integral[2] = sensors.accelerometer_m_s2[2] * accel_dt; + _ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt, gyro_integral, accel_integral); // read mag data @@ -780,7 +782,7 @@ void Ekf2::run() start_time_us = now; } else if (start_time_us > 0) { - integrated_time_us += (uint64_t)((double)sensors.gyro_integral_dt * 1.0e6); + integrated_time_us += sensors.gyro_integral_dt; } matrix::Quaternion q; @@ -1160,7 +1162,7 @@ void Ekf2::run() // calculate noise filtered velocity innovations which are used for pre-flight checking if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - float alpha = math::constrain(sensors.accelerometer_integral_dt * _innov_lpf_tau_inv, 0.0f, 1.0f); + float alpha = math::constrain(sensors.accelerometer_integral_dt / 1.e6f * _innov_lpf_tau_inv, 0.0f, 1.0f); float beta = 1.0f - alpha; _vel_innov_lpf_ned(0) = beta * _vel_innov_lpf_ned(0) + alpha * math::constrain(innovations.vel_pos_innov[0], -_vel_innov_spike_lim, _vel_innov_spike_lim); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index c054e1d0e0..306f46c936 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -518,8 +518,8 @@ struct log_EST5_s { #define LOG_RPL1_MSG 51 struct log_RPL1_s { uint64_t time_ref; - float gyro_integral_dt; - float accelerometer_integral_dt; + uint64_t gyro_integral_dt; + uint64_t accelerometer_integral_dt; uint64_t magnetometer_timestamp; uint64_t baro_timestamp; float gyro_x_rad; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 9dfe8bfde5..f33f5916f0 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -563,7 +563,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) accel_data = math::Vector<3>(accel_report.x_integral * dt_inv, accel_report.y_integral * dt_inv, accel_report.z_integral * dt_inv); - _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt / 1.e6f; + _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt; } else { // using the value instead of the integral (the integral is the prefered choice) @@ -579,7 +579,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) // approximate the delta time using the difference in accel data time stamps _last_sensor_data[uorb_index].accelerometer_integral_dt = - (accel_report.timestamp - _last_accel_timestamp[uorb_index]) / 1.e6f; + (accel_report.timestamp - _last_accel_timestamp[uorb_index]); } // handle temperature compensation @@ -670,7 +670,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) gyro_rate = math::Vector<3>(gyro_report.x_integral * dt_inv, gyro_report.y_integral * dt_inv, gyro_report.z_integral * dt_inv); - _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt / 1.e6f; + _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt; } else { //using the value instead of the integral (the integral is the prefered choice) @@ -686,7 +686,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) // approximate the delta time using the difference in gyro data time stamps _last_sensor_data[uorb_index].gyro_integral_dt = - (gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp) / 1.e6f; + (gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp); } // handle temperature compensation