sensors: create vehicle_acceleration module (#12597)

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 @@ private:
 
 	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 @@ private:
 
 	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 @@ private:
 	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 {};