sensor_combined: replace accel & gyro integral with value, use float …

…for dt

Reason: the value is easier to read & handle (for example plotting). In
most places the value is needed, not the integral.

Note that this breaks the replay format for sdlog2 replay

msg/ekf2_replay.msg

@@ -1,16 +1,16 @@
 uint64 time_ref 			# ekf2 reference time. This is a timestamp passed to the
 					# estimator which it uses a absolute reference.
-uint64 gyro_integral_dt			# gyro integration period in us
-uint64 accelerometer_integral_dt 	# accelerometer integration period in us
+float32 gyro_integral_dt		# gyro integration period in s
+float32 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
 uint64 flow_timestamp			# timestamp of optical flow measurement in us
 uint64 asp_timestamp			# timestamp of airspeed measurement in us
 uint64 ev_timestamp			# timestamp of external vision measurement in us
 
-float32[3] gyro_integral_rad		# integrated gyro vector in rad
-float32[3] accelerometer_integral_m_s  	# integrated accelerometer vector in m/s
+float32[3] gyro_rad			# gyro vector in rad
+float32[3] accelerometer_m_s2  	# accelerometer vector in m/s^2
 
 float32[3] magnetometer_ga 		# magnetometer measurement vector (body fixed NED) in ga
 float32 baro_alt_meter			# barometer altitude measurement in m

msg/sensor_combined.msg

@@ -5,15 +5,16 @@
 # change with board revisions and sensor updates.
 #
 
-int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid
+int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
+
 
 # gyro timstamp is equal to the timestamp of the message
-float32[3] gyro_integral_rad		# delta angle in the NED body frame in rad in the integration time frame
-uint32 gyro_integral_dt			# delta time for gyro integral in us
+float32[3] gyro_rad				# delta angle in the NED body frame in rad
+float32 gyro_integral_dt			# delta time for gyro integral in s
 
 int32 accelerometer_timestamp_relative	# timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
-float32[3] accelerometer_integral_m_s		# velocity in NED body frame, in m/s^2
-uint32 accelerometer_integral_dt		# delta time for accel integral in us
+float32[3] accelerometer_m_s2			# velocity in NED body frame, in m/s^2
+float32 accelerometer_integral_dt		# delta time for accel integral 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/drivers/frsky_telemetry/frsky_data.c

@@ -204,14 +204,9 @@ void frsky_update_topics()
 void frsky_send_frame1(int uart)
 {
 	/* send formatted frame */
-	float acceleration[3];
-	float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f;
-	acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
-	acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
-	acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
-	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f));
-	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
+	frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
 
 	frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
 			sensor_combined->baro_alt_meter);

src/examples/px4_simple_app/px4_simple_app.c

@@ -101,18 +101,15 @@ int px4_simple_app_main(int argc, char *argv[])
 				struct sensor_combined_s raw;
 				/* copy sensors raw data into local buffer */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
-				float sensor_accel[3];
-				float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
-				sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
-				sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
-				sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
 				PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
-					 (double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
+					 (double)raw.accelerometer_m_s2[0],
+					 (double)raw.accelerometer_m_s2[1],
+					 (double)raw.accelerometer_m_s2[2]);
 
 				/* set att and publish this information for other apps */
-				att.roll = sensor_accel[0];
-				att.pitch = sensor_accel[1];
-				att.yaw = sensor_accel[2];
+				att.roll = raw.accelerometer_m_s2[0];
+				att.pitch = raw.accelerometer_m_s2[1];
+				att.yaw = raw.accelerometer_m_s2[2];
 				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
 			}
 

src/lib/terrain_estimation/terrain_estimator.cpp

@@ -67,11 +67,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
 {
 	if (attitude->R_valid) {
 		matrix::Matrix<float, 3, 3> R_att(attitude->R);
-		matrix::Vector<float, 3> a;
-		float accel_dt = sensor->accelerometer_integral_dt / 1.e6f;
-		a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
-		a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
-		a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
+		matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
 		matrix::Vector<float, 3> u;
 		u = R_att * a;
 		_u_z = u(2) + 9.81f; // compensate for gravity

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -395,19 +395,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
 				}
 
-				float gyro_rad_s[3];
-				float gyro_dt = raw.gyro_integral_dt / 1.e6f;
-				gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
-				gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
-				gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
-
 				if (!initialized) {
 					// XXX disabling init for now
 					initialized = true;
 
-					// gyro_offsets[0] += gyro_rad_s[0];
-					// gyro_offsets[1] += gyro_rad_s[1];
-					// gyro_offsets[2] += gyro_rad_s[2];
+					// gyro_offsets[0] += raw.gyro_rad[0];
+					// gyro_offsets[1] += raw.gyro_rad[1];
+					// gyro_offsets[2] += raw.gyro_rad[2];
 					// offset_count++;
 
 					// if (hrt_absolute_time() - start_time > 3000000LL) {
@@ -433,9 +427,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-					z_k[0] =  gyro_rad_s[0] - gyro_offsets[0];
-					z_k[1] =  gyro_rad_s[1] - gyro_offsets[1];
-					z_k[2] =  gyro_rad_s[2] - gyro_offsets[2];
+					z_k[0] =  raw.gyro_rad[0] - gyro_offsets[0];
+					z_k[1] =  raw.gyro_rad[1] - gyro_offsets[1];
+					z_k[2] =  raw.gyro_rad[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
@@ -476,15 +470,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						last_vel_t = 0;
 					}
 
-					matrix::Vector3f raw_accel;
-					float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
-					raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
-					raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
-					raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
-
-					z_k[3] = raw_accel(0) - acc(0);
-					z_k[4] = raw_accel(1) - acc(1);
-					z_k[5] = raw_accel(2) - acc(2);
+					z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
+					z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
+					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
@@ -628,9 +616,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					att.g_comp[0] = raw_accel(0) - acc(0);
-					att.g_comp[1] = raw_accel(1) - acc(1);
-					att.g_comp[2] = raw_accel(2) - acc(2);
+					att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
+					att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
+					att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -328,17 +328,15 @@ void AttitudeEstimatorQ::task_main()
 			// Feed validator with recent sensor data
 
 			if (sensors.timestamp > 0) {
-				float gyro_dt = sensors.gyro_integral_dt / 1e6;
-				_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
-				_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
-				_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
+				_gyro(0) = sensors.gyro_rad[0];
+				_gyro(1) = sensors.gyro_rad[1];
+				_gyro(2) = sensors.gyro_rad[2];
 			}
 
 			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
-				float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
-				_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
-				_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
-				_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
+				_accel(0) = sensors.accelerometer_m_s2[0];
+				_accel(1) = sensors.accelerometer_m_s2[1];
+				_accel(2) = sensors.accelerometer_m_s2[2];
 
 				if (_accel.length() < 0.01f) {
 					warnx("WARNING: degenerate accel!");

src/modules/commander/calibration_routines.cpp

@@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
 
 			for (unsigned i = 0; i < ndim; i++) {
 
-				float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
+				float di = sensor.accelerometer_m_s2[i];
 
 				float d = di - accel_ema[i];
 				accel_ema[i] += d * w;

src/modules/ekf2/ekf2_main.cpp

@@ -505,8 +505,16 @@ void Ekf2::task_main()
 		}
 
 		// push imu data into estimator
-		_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
-				sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
+		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 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,
+				gyro_integral, accel_integral);
 
 		// read mag data
 		if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
@@ -622,15 +630,14 @@ void Ekf2::task_main()
 			control_state_s ctrl_state = {};
 			float gyro_bias[3] = {};
 			_ekf.get_gyro_bias(gyro_bias);
-			float gyro_rad_s[3];
-			float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
-			gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
-			gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
-			gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
 			ctrl_state.timestamp = hrt_absolute_time();
-			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
-			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
-			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
+			float gyro_rad[3];
+			gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
+			gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
+			gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
+			ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
+			ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
+			ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
 
 			// Velocity in body frame
 			float velocity[3];
@@ -657,11 +664,7 @@ void Ekf2::task_main()
 			ctrl_state.q[3] = q(3);
 
 			// Acceleration data
-			matrix::Vector<float, 3> acceleration;
-			float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
-			acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
-			acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
-			acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
+			matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
 
 			float accel_bias[3];
 			_ekf.get_accel_bias(accel_bias);
@@ -722,9 +725,9 @@ void Ekf2::task_main()
 			att.q[3] = q(3);
 			att.q_valid = true;
 
-			att.rollspeed = gyro_rad_s[0];
-			att.pitchspeed = gyro_rad_s[1];
-			att.yawspeed = gyro_rad_s[2];
+			att.rollspeed = gyro_rad[0];
+			att.pitchspeed = gyro_rad[1];
+			att.yawspeed = gyro_rad[2];
 
 			// publish vehicle attitude data
 			if (_att_pub == nullptr) {
@@ -917,9 +920,8 @@ void Ekf2::task_main()
 			replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
 			replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
 			replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
-			memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
-			memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
-			       sizeof(replay.accelerometer_integral_m_s));
+			memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
+			memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
 			memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
 			replay.baro_alt_meter = sensors.baro_alt_meter;
 

src/modules/ekf2_replay/ekf2_replay_main.cpp

@@ -378,15 +378,15 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
 		parseMessage(data, dest_ptr, type);
 		_sensors.timestamp = replay_part1.time_ref;
 		_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
-		_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
+		_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
 		_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
 		_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
-		_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
-		_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
-		_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
-		_sensors.accelerometer_integral_m_s[0] = replay_part1.accelerometer_integral_x_m_s;
-		_sensors.accelerometer_integral_m_s[1] = replay_part1.accelerometer_integral_y_m_s;
-		_sensors.accelerometer_integral_m_s[2] = replay_part1.accelerometer_integral_z_m_s;
+		_sensors.gyro_rad[0] = replay_part1.gyro_x_rad;
+		_sensors.gyro_rad[1] = replay_part1.gyro_y_rad;
+		_sensors.gyro_rad[2] = replay_part1.gyro_z_rad;
+		_sensors.accelerometer_m_s2[0] = replay_part1.accelerometer_x_m_s2;
+		_sensors.accelerometer_m_s2[1] = replay_part1.accelerometer_y_m_s2;
+		_sensors.accelerometer_m_s2[2] = replay_part1.accelerometer_z_m_s2;
 		_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
 		_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
 		_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -1357,23 +1357,23 @@ void AttitudePositionEstimatorEKF::pollData()
 	/* fill in last data set */
 	_ekf->dtIMU = deltaT;
 
-	_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
-	_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
-	_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
+	_ekf->angRate.x = _sensor_combined.gyro_rad[0];
+	_ekf->angRate.y = _sensor_combined.gyro_rad[1];
+	_ekf->angRate.z = _sensor_combined.gyro_rad[2];
 
-	float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
-	_ekf->angRate = _ekf->dAngIMU / gyro_dt;
+	float gyro_dt = _sensor_combined.gyro_integral_dt;
+	_ekf->dAngIMU = _ekf->angRate * gyro_dt;
 
 	perf_count(_perf_gyro);
 
 	if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
 
-		_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
-		_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
-		_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
+		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
+		_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 / 1.e6f;
-		_ekf->accel = _ekf->dVelIMU / accel_dt;
+		float accel_dt = _sensor_combined.accelerometer_integral_dt;
+		_ekf->dVelIMU = _ekf->accel * accel_dt;
 
 		_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
 	}
@@ -1396,8 +1396,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 / 1e6f;
-	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
+	float deltaTIntegral = _sensor_combined.gyro_integral_dt;
+	float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
@@ -1413,10 +1413,10 @@ void AttitudePositionEstimatorEKF::pollData()
 			 (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
 		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
-			 (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[0]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[1]),
-			 (double)(_sensor_combined.accelerometer_integral_m_s[2]));
+			 (double)(_sensor_combined.gyro_rad[0]), (double)(_sensor_combined.gyro_rad[2]),
+			 (double)(_sensor_combined.accelerometer_m_s2[0]),
+			 (double)(_sensor_combined.accelerometer_m_s2[1]),
+			 (double)(_sensor_combined.accelerometer_m_s2[2]));
 
 		PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
 			 (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1230,11 +1230,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body;
-	float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
-	accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
-	accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
-	accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
+	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
 	math::Vector<3> accel_earth = _R_nb * accel_body;
 
 	/* tell TECS to update its state, but let it know when it cannot actually control the plane */

src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp

@@ -844,11 +844,7 @@ void BlockLocalPositionEstimator::predict()
 
 	if (integrate && _sub_att.get().R_valid) {
 		Matrix3f R_att(_sub_att.get().R);
-		Vector3f a;
-		float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f;
-		a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
-		a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
-		a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
+		Vector3f a(_sub_sensor.get().accelerometer_m_s2);
 		_u = R_att * a;
 		_u(U_az) += 9.81f; // add g