change gyro & accel dt from float to uint64. This has the benefit of

calculating the estimator timeslip correctly.

Signed-off-by: Nicolae Rosia <nicolae.rosia@gmail.com>

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

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

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;

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<float> 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);

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;

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