sensors: create vehicle_accelerometer module

msg/CMakeLists.txt

@@ -122,6 +122,7 @@ set(msg_files
 	uavcan_parameter_value.msg
 	ulog_stream.msg
 	ulog_stream_ack.msg
+	vehicle_acceleration.msg
 	vehicle_air_data.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg

msg/sensor_bias.msg

@@ -5,20 +5,10 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-float32 accel_x		# Bias corrected acceleration in body X axis (in m/s^2)
-float32 accel_y		# Bias corrected acceleration in body Y axis (in m/s^2)
-float32 accel_z		# Bias corrected acceleration in body Z axis (in m/s^2)
-
 # In-run bias estimates (subtract from uncorrected data)
 
-float32 gyro_x_bias		# X gyroscope in-run bias in body frame (rad/s, x forward)
-float32 gyro_y_bias		# Y gyroscope in-run bias in body frame (rad/s, y right)
-float32 gyro_z_bias		# Z gyroscope in-run bias in body frame (rad/s, z down)
+float32[3] gyro_bias		# gyroscope in-run bias in body frame (rad/s)
 
-float32 accel_x_bias	# X accelerometer in-run bias in body frame (m/s^2, x forward)
-float32 accel_y_bias	# Y accelerometer in-run bias in body frame (m/s^2, y right)
-float32 accel_z_bias	# Z accelerometer in-run bias in body frame (m/s^2, z down)
+float32[3] accel_bias		# accelerometer in-run bias in body frame (m/s^2)
 
-float32 mag_x_bias		# X magnetometer in-run bias in body frame (Gauss, x forward)
-float32 mag_y_bias		# Y magnetometer in-run bias in body frame (Gauss, y right)
-float32 mag_z_bias		# Z magnetometer in-run bias in body frame (Gauss, z down)
+float32[3] mag_bias		# magnetometer in-run bias in body frame (Gauss)

msg/tools/uorb_rtps_message_ids.yaml

@@ -256,6 +256,8 @@ rtps:
     id: 111
   - msg: vehicle_angular_velocity
     id: 112
+  - msg: vehicle_acceleration
+    id: 113
   # multi topics
   - msg: actuator_controls_0
     id: 120

msg/vehicle_acceleration.msg

@@ -0,0 +1,6 @@
+
+uint64 timestamp		# time since system start (microseconds)
+
+uint64 timestamp_sample		# the timestamp of the raw data (microseconds)
+
+float32[3] xyz			# Bias corrected acceleration (including gravity) in body axis (in m/s^2)

src/modules/commander/Commander.cpp

@@ -3901,8 +3901,8 @@ void Commander::airspeed_use_check()
 	_airspeed_sub.update();
 	const airspeed_s &airspeed = _airspeed_sub.get();
 
-	_sensor_bias_sub.update();
-	const sensor_bias_s &sensors = _sensor_bias_sub.get();
+	vehicle_acceleration_s accel{};
+	_vehicle_acceleration_sub.copy(&accel);
 
 	bool is_fixed_wing = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
@@ -3988,7 +3988,7 @@ void Commander::airspeed_use_check()
 			float max_lift_ratio = fmaxf(airspeed.indicated_airspeed_m_s, 0.7f) / fmaxf(_airspeed_stall.get(), 1.0f);
 			max_lift_ratio *= max_lift_ratio;
 
-			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(sensors.accel_z) / CONSTANTS_ONE_G) / max_lift_ratio;
+			_load_factor_ratio = 0.95f * _load_factor_ratio + 0.05f * (fabsf(accel.xyz[2]) / CONSTANTS_ONE_G) / max_lift_ratio;
 			_load_factor_ratio = math::constrain(_load_factor_ratio, 0.25f, 2.0f);
 			load_factor_ratio_fail = (_load_factor_ratio > 1.1f);
 			status.load_factor_ratio = _load_factor_ratio;

src/modules/commander/Commander.hpp

@@ -60,8 +60,8 @@
 #include <uORB/topics/iridiumsbd_status.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/telemetry_status.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -250,11 +250,12 @@ class Commander : public ModuleBase<Commander>, public ModuleParams
 	bool _print_avoidance_msg_once{false};
 
 	// Subscriptions
+	uORB::Subscription					_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+
 	uORB::SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
 	uORB::SubscriptionData<estimator_status_s>		_estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::SubscriptionData<mission_result_s>		_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	uORB::SubscriptionData<sensor_bias_s>			_sensor_bias_sub{ORB_ID(sensor_bias)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
 

src/modules/ekf2/ekf2_main.cpp

@@ -1482,31 +1482,17 @@ void Ekf2::run()
 
 			{
 				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				sensor_bias_s bias;
+				sensor_bias_s bias{};
 
 				bias.timestamp = now;
 
 				// In-run bias estimates
-				float gyro_bias[3];
-				_ekf.get_gyro_bias(gyro_bias);
-				bias.gyro_x_bias = gyro_bias[0];
-				bias.gyro_y_bias = gyro_bias[1];
-				bias.gyro_z_bias = gyro_bias[2];
-
-				float accel_bias[3];
-				_ekf.get_accel_bias(accel_bias);
-				bias.accel_x_bias = accel_bias[0];
-				bias.accel_y_bias = accel_bias[1];
-				bias.accel_z_bias = accel_bias[2];
-
-				bias.mag_x_bias = _last_valid_mag_cal[0];
-				bias.mag_y_bias = _last_valid_mag_cal[1];
-				bias.mag_z_bias = _last_valid_mag_cal[2];
-
-				// TODO: remove from sensor_bias?
-				bias.accel_x = sensors.accelerometer_m_s2[0] - accel_bias[0];
-				bias.accel_y = sensors.accelerometer_m_s2[1] - accel_bias[1];
-				bias.accel_z = sensors.accelerometer_m_s2[2] - accel_bias[2];
+				_ekf.get_gyro_bias(bias.gyro_bias);
+				_ekf.get_accel_bias(bias.accel_bias);
+
+				bias.mag_bias[0] = _last_valid_mag_cal[0];
+				bias.mag_bias[1] = _last_valid_mag_cal[1];
+				bias.mag_bias[2] = _last_valid_mag_cal[2];
 
 				_sensor_bias_pub.publish(bias);
 			}

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -37,8 +37,6 @@ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]);
 
 FixedwingPositionControl::FixedwingPositionControl() :
 	ModuleParams(nullptr),
-	_sub_airspeed(ORB_ID(airspeed)),
-	_sub_sensors(ORB_ID(sensor_bias)),
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw_l1_control")),
 	_launchDetector(this),
 	_runway_takeoff(this)
@@ -346,9 +344,9 @@ FixedwingPositionControl::airspeed_poll()
 {
 	bool airspeed_valid = _airspeed_valid;
 
-	if (!_parameters.airspeed_disabled && _sub_airspeed.update()) {
+	if (!_parameters.airspeed_disabled && _airspeed_sub.update()) {
 
-		const airspeed_s &as = _sub_airspeed.get();
+		const airspeed_s &as = _airspeed_sub.get();
 
 		if (PX4_ISFINITE(as.indicated_airspeed_m_s)
 		    && PX4_ISFINITE(as.true_airspeed_m_s)
@@ -1292,7 +1290,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
 				}
 
 				/* Detect launch using body X (forward) acceleration */
-				_launchDetector.update(_sub_sensors.get().accel_x);
+				_launchDetector.update(_vehicle_acceleration_sub.get().xyz[0]);
 
 				/* update our copy of the launch detection state */
 				_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1750,7 +1748,6 @@ FixedwingPositionControl::run()
 			_alt_reset_counter = _global_pos.alt_reset_counter;
 			_pos_reset_counter = _global_pos.lat_lon_reset_counter;
 
-			_sub_sensors.update();
 			airspeed_poll();
 			_manual_control_sub.update(&_manual);
 			_pos_sp_triplet_sub.update(&_pos_sp_triplet);
@@ -1759,6 +1756,7 @@ FixedwingPositionControl::run()
 			vehicle_control_mode_poll();
 			_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 			vehicle_status_poll();
+			_vehicle_acceleration_sub.update();
 			_vehicle_rates_sub.update();
 
 			Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
@@ -1931,7 +1929,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 	float pitch_for_tecs = _pitch - _parameters.pitchsp_offset_rad;
 
 	/* filter speed and altitude for controller */
-	Vector3f accel_body(_sub_sensors.get().accel_x, _sub_sensors.get().accel_y, _sub_sensors.get().accel_z);
+	Vector3f accel_body(_vehicle_acceleration_sub.get().xyz);
 
 	// tailsitters use the multicopter frame as reference, in fixed wing
 	// we need to use the fixed wing frame

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -74,8 +74,8 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -184,8 +184,8 @@ class FixedwingPositionControl final : public ModuleBase<FixedwingPositionContro
 	vehicle_land_detected_s		_vehicle_land_detected {};	///< vehicle land detected */
 	vehicle_status_s		_vehicle_status {};		///< vehicle status */
 
-	SubscriptionData<airspeed_s> _sub_airspeed;
-	SubscriptionData<sensor_bias_s> _sub_sensors;
+	SubscriptionData<airspeed_s>			_airspeed_sub{ORB_ID(airspeed)};
+	SubscriptionData<vehicle_acceleration_s>	_vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 
 	perf_counter_t	_loop_perf;				///< loop performance counter */
 

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -45,6 +45,7 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <matrix/math.hpp>
 
 namespace land_detector
 {
@@ -59,7 +60,7 @@ FixedwingLandDetector::FixedwingLandDetector()
 void FixedwingLandDetector::_update_topics()
 {
 	_airspeed_sub.update(&_airspeed);
-	_sensor_bias_sub.update(&_sensor_bias);
+	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 }
 
@@ -110,8 +111,8 @@ bool FixedwingLandDetector::_get_landed_state()
 
 		// A leaking lowpass prevents biases from building up, but
 		// gives a mostly correct response for short impulses.
-		const float acc_hor = sqrtf(_sensor_bias.accel_x * _sensor_bias.accel_x +
-					    _sensor_bias.accel_y * _sensor_bias.accel_y);
+		const matrix::Vector3f accel{_vehicle_acceleration.xyz};
+		const float acc_hor = sqrtf(accel(0) * accel(0) + accel(1) * accel(1));
 
 		_accel_horz_lp = _accel_horz_lp * 0.8f + acc_hor * 0.18f;
 

src/modules/land_detector/FixedwingLandDetector.h

@@ -45,7 +45,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_local_position.h>
 
 #include "LandDetector.h"
@@ -75,11 +75,11 @@ class FixedwingLandDetector final : public LandDetector
 
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _param_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 
 	airspeed_s _airspeed{};
-	sensor_bias_s _sensor_bias{};
+	vehicle_acceleration_s _vehicle_acceleration{};
 	vehicle_local_position_s _vehicle_local_position{};
 
 	float _accel_horz_lp{0.0f};

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -63,6 +63,7 @@
 
 #include <cmath>
 #include <mathlib/mathlib.h>
+#include <matrix/math.hpp>
 
 #include "MulticopterLandDetector.h"
 
@@ -94,7 +95,7 @@ void MulticopterLandDetector::_update_topics()
 {
 	_actuator_controls_sub.update(&_actuator_controls);
 	_battery_sub.update(&_battery_status);
-	_sensor_bias_sub.update(&_sensor_bias);
+	_vehicle_acceleration_sub.update(&_vehicle_acceleration);
 	_vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity);
 	_vehicle_attitude_sub.update(&_vehicle_attitude);
 	_vehicle_control_mode_sub.update(&_control_mode);
@@ -126,17 +127,15 @@ bool MulticopterLandDetector::_get_freefall_state()
 		return false;
 	}
 
-	if (_sensor_bias.timestamp == 0) {
+	if (_vehicle_acceleration.timestamp == 0) {
 		// _sensors is not valid yet, we have to assume we're not falling.
 		return false;
 	}
 
-	float acc_norm = _sensor_bias.accel_x * _sensor_bias.accel_x
-			 + _sensor_bias.accel_y * _sensor_bias.accel_y
-			 + _sensor_bias.accel_z * _sensor_bias.accel_z;
-	acc_norm = sqrtf(acc_norm);	//norm of specific force. Should be close to 9.8 m/s^2 when landed.
+	// norm of specific force. Should be close to 9.8 m/s^2 when landed.
+	const matrix::Vector3f accel{_vehicle_acceleration.xyz};
 
-	return (acc_norm < _params.freefall_acc_threshold);	//true if we are currently falling
+	return (accel.norm() < _params.freefall_acc_threshold);	// true if we are currently falling
 }
 
 bool MulticopterLandDetector::_get_ground_contact_state()
@@ -216,7 +215,7 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 	// Next look if all rotation angles are not moving.
 	float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor;
 
-	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0])  > maxRotationScaled) ||
+	bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) ||
 			(fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) ||
 			(fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled);
 

src/modules/land_detector/MulticopterLandDetector.h

@@ -52,7 +52,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -126,7 +126,7 @@ class MulticopterLandDetector : public LandDetector
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
-	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
@@ -136,7 +136,7 @@ class MulticopterLandDetector : public LandDetector
 	actuator_controls_s               _actuator_controls {};
 	battery_status_s                  _battery_status {};
 	vehicle_control_mode_s            _control_mode {};
-	sensor_bias_s                     _sensor_bias {};
+	vehicle_acceleration_s           _vehicle_acceleration{};
 	vehicle_attitude_s                _vehicle_attitude {};
 	vehicle_angular_velocity_s        _vehicle_angular_velocity{};
 	vehicle_local_position_s          _vehicle_local_position {};

src/modules/landing_target_estimator/LandingTargetEstimator.cpp

@@ -80,14 +80,11 @@ void LandingTargetEstimator::update()
 			float dt = (hrt_absolute_time() - _last_predict) / SEC2USEC;
 
 			// predict target position with the help of accel data
-			matrix::Vector3f a;
+			matrix::Vector3f a{_vehicle_acceleration.xyz};
 
-			if (_vehicleAttitude_valid && _sensorBias_valid) {
+			if (_vehicleAttitude_valid && _vehicle_acceleration_valid) {
 				matrix::Quaternion<float> q_att(&_vehicleAttitude.q[0]);
 				_R_att = matrix::Dcm<float>(q_att);
-				a(0) = _sensorBias.accel_x;
-				a(1) = _sensorBias.accel_y;
-				a(2) = _sensorBias.accel_z;
 				a = _R_att * a;
 
 			} else {
@@ -244,7 +241,7 @@ void LandingTargetEstimator::_update_topics()
 {
 	_vehicleLocalPosition_valid = _vehicleLocalPositionSub.update(&_vehicleLocalPosition);
 	_vehicleAttitude_valid = _attitudeSub.update(&_vehicleAttitude);
-	_sensorBias_valid = _sensorBiasSub.update(&_sensorBias);
+	_vehicle_acceleration_valid = _vehicle_acceleration_sub.update(&_vehicle_acceleration);
 
 	_new_irlockReport = _irlockReportSub.update(&_irlockReport);
 }

src/modules/landing_target_estimator/LandingTargetEstimator.h

@@ -47,9 +47,9 @@
 #include <parameters/param.h>
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/irlock_report.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/landing_target_innovations.h>
@@ -130,18 +130,18 @@ class LandingTargetEstimator
 
 	uORB::Subscription _vehicleLocalPositionSub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _sensorBiasSub{ORB_ID(sensor_bias)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
 	uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)};
 
 	vehicle_local_position_s	_vehicleLocalPosition{};
-	vehicle_attitude_s	_vehicleAttitude{};
-	sensor_bias_s		_sensorBias{};
-	irlock_report_s		_irlockReport{};
+	vehicle_attitude_s		_vehicleAttitude{};
+	vehicle_acceleration_s		_vehicle_acceleration{};
+	irlock_report_s			_irlockReport{};
 
 	// keep track of which topics we have received
 	bool _vehicleLocalPosition_valid{false};
 	bool _vehicleAttitude_valid{false};
-	bool _sensorBias_valid{false};
+	bool _vehicle_acceleration_valid{false};
 	bool _new_irlockReport{false};
 	bool _estimator_initialized{false};
 	// keep track of whether last measurement was rejected