sensors: create vehicle_angular_velocity module (#12596)

* split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl

src/modules/sensors/CMakeLists.txt

@@ -31,6 +31,8 @@
 #
 ############################################################################
 
+add_subdirectory(vehicle_angular_velocity)
+
 px4_add_module(
 	MODULE modules__sensors
 	MAIN sensors
@@ -51,4 +53,5 @@ px4_add_module(
 		git_ecl
 		ecl_validation
 		mathlib
+		sensors__vehicle_angular_velocity
 	)

src/modules/sensors/sensors.cpp

@@ -92,6 +92,8 @@
 #include "rc_update.h"
 #include "voted_sensors_update.h"
 
+#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
+
 using namespace DriverFramework;
 using namespace sensors;
 using namespace time_literals;
@@ -135,7 +137,7 @@ class Sensors : public ModuleBase<Sensors>, public ModuleParams
 {
 public:
 	Sensors(bool hil_enabled);
-	~Sensors() = default;
+	~Sensors() override;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -202,6 +204,9 @@ private:
 	VotedSensorsUpdate _voted_sensors_update;
 
 
+	VehicleAngularVelocity	_angular_velocity;
+
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -253,6 +258,13 @@ Sensors::Sensors(bool hil_enabled) :
 	}
 
 #endif /* BOARD_NUMBER_BRICKS > 0 */
+
+	_angular_velocity.Start();
+}
+
+Sensors::~Sensors()
+{
+	_angular_velocity.Stop();
 }
 
 int
@@ -724,6 +736,8 @@ int Sensors::print_status()
 	PX4_INFO("Airspeed status:");
 	_airspeed_validator.print();
 
+	_angular_velocity.PrintStatus();
+
 	return 0;
 }
 

src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(sensors__vehicle_angular_velocity
+	VehicleAngularVelocity.cpp
+)
+target_link_libraries(sensors__vehicle_angular_velocity PRIVATE px4_work_queue)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "VehicleAngularVelocity.hpp"
+
+#include <px4_log.h>
+
+using namespace time_literals;
+using namespace matrix;
+
+VehicleAngularVelocity::VehicleAngularVelocity() :
+	ModuleParams(nullptr),
+	WorkItem(px4::wq_configurations::rate_ctrl),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")),
+	_interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")),
+	_sensor_gyro_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor gyro latency"))
+{
+}
+
+VehicleAngularVelocity::~VehicleAngularVelocity()
+{
+	Stop();
+
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
+	perf_free(_sensor_gyro_latency_perf);
+}
+
+bool
+VehicleAngularVelocity::Start()
+{
+	// initialize thermal corrections as we might not immediately get a topic update (only non-zero values)
+	_scale = Vector3f{1.0f, 1.0f, 1.0f};
+	_offset.zero();
+	_bias.zero();
+
+	// force initial updates
+	ParametersUpdate(true);
+	SensorBiasUpdate(true);
+
+	_sensor_correction_sub.register_callback();
+
+	return SensorCorrectionsUpdate(true);
+}
+
+void
+VehicleAngularVelocity::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	for (auto sub : _sensor_gyro_sub) {
+		sub.unregister_callback();
+	}
+
+	_sensor_correction_sub.unregister_callback();
+}
+
+void
+VehicleAngularVelocity::SensorBiasUpdate(bool force)
+{
+	if (_sensor_bias_sub.updated() || force) {
+		sensor_bias_s bias;
+
+		if (_sensor_bias_sub.copy(&bias)) {
+			// TODO: should be checking device ID
+			_bias(0) = bias.gyro_x_bias;
+			_bias(1) = bias.gyro_y_bias;
+			_bias(2) = bias.gyro_z_bias;
+		}
+	}
+}
+
+bool
+VehicleAngularVelocity::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected gyro has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		sensor_correction_s corrections{};
+		_sensor_correction_sub.copy(&corrections);
+
+		// TODO: should be checking device ID
+		if (_selected_gyro == 0) {
+			_offset = Vector3f{corrections.gyro_offset_0};
+			_scale = Vector3f{corrections.gyro_scale_0};
+
+		} else if (_selected_gyro == 1) {
+			_offset = Vector3f{corrections.gyro_offset_1};
+			_scale = Vector3f{corrections.gyro_scale_1};
+
+		} else if (_selected_gyro == 2) {
+			_offset = Vector3f{corrections.gyro_offset_2};
+			_scale = Vector3f{corrections.gyro_scale_2};
+
+		} else {
+			_offset = Vector3f{0.0f, 0.0f, 0.0f};
+			_scale = Vector3f{1.0f, 1.0f, 1.0f};
+		}
+
+		// update the latest gyro selection
+		if (_selected_gyro != corrections.selected_gyro_instance) {
+			if (corrections.selected_gyro_instance < MAX_GYRO_COUNT) {
+				// clear all registered callbacks
+				for (auto sub : _sensor_gyro_sub) {
+					sub.unregister_callback();
+				}
+
+				const int gyro_new = corrections.selected_gyro_instance;
+
+				if (_sensor_gyro_sub[gyro_new].register_callback()) {
+					PX4_DEBUG("selected gyro changed %d -> %d", _selected_gyro, gyro_new);
+					_selected_gyro = gyro_new;
+
+					return true;
+				}
+			}
+		}
+	}
+
+	return false;
+}
+
+void
+VehicleAngularVelocity::ParametersUpdate(bool force)
+{
+	// Check if parameters have changed
+	if (_params_sub.updated() || force) {
+		// clear update
+		parameter_update_s param_update;
+		_params_sub.copy(&param_update);
+
+		updateParams();
+
+		// get transformation matrix from sensor/board to body frame
+		const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get());
+
+		// fine tune the rotation
+		const Dcmf board_rotation_offset(Eulerf(
+				math::radians(_param_sens_board_x_off.get()),
+				math::radians(_param_sens_board_y_off.get()),
+				math::radians(_param_sens_board_z_off.get())));
+
+		_board_rotation = board_rotation_offset * board_rotation;
+	}
+}
+
+void
+VehicleAngularVelocity::Run()
+{
+	perf_begin(_cycle_perf);
+	perf_count(_interval_perf);
+
+	// update corrections first to set _selected_gyro
+	SensorCorrectionsUpdate();
+
+	sensor_gyro_s sensor_gyro;
+
+	if (_sensor_gyro_sub[_selected_gyro].update(&sensor_gyro)) {
+		perf_set_elapsed(_sensor_gyro_latency_perf, hrt_elapsed_time(&sensor_gyro.timestamp));
+
+		ParametersUpdate();
+		SensorBiasUpdate();
+
+		// get the raw gyro data and correct for thermal errors
+		const Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z};
+
+		// apply offsets and scale
+		Vector3f rates{(gyro - _offset).emult(_scale)};
+
+		// rotate corrected measurements from sensor to body frame
+		rates = _board_rotation * rates;
+
+		// correct for in-run bias errors
+		rates -= _bias;
+
+		vehicle_angular_velocity_s angular_velocity;
+		angular_velocity.timestamp_sample = sensor_gyro.timestamp;
+		rates.copyTo(angular_velocity.xyz);
+		angular_velocity.timestamp = hrt_absolute_time();
+
+		_vehicle_angular_velocity_pub.publish(angular_velocity);
+	}
+
+	perf_end(_cycle_perf);
+}
+
+void
+VehicleAngularVelocity::PrintStatus()
+{
+	PX4_INFO("selected gyro: %d", _selected_gyro);
+
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
+	perf_print_counter(_sensor_gyro_latency_perf);
+}

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <lib/perf/perf_counter.h>
+#include <px4_config.h>
+#include <px4_getopt.h>
+#include <px4_log.h>
+#include <px4_module_params.h>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/mathlib/math/Limits.hpp>
+#include <lib/mathlib/math/Functions.hpp>
+#include <lib/conversion/rotation.h>
+#include <px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_bias.h>
+#include <uORB/topics/sensor_correction.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+
+#define MAX_GYRO_COUNT 3
+
+class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem
+{
+public:
+
+	VehicleAngularVelocity();
+	virtual ~VehicleAngularVelocity();
+
+	void	Run() override;
+
+	bool	Start();
+	void	Stop();
+
+	void	PrintStatus();
+
+private:
+
+	void	ParametersUpdate(bool force = false);
+	void	SensorBiasUpdate(bool force = false);
+	bool	SensorCorrectionsUpdate(bool force = false);
+
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
+
+		(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
+		(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
+		(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
+	)
+
+	uORB::Publication<vehicle_angular_velocity_s>	_vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
+
+	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};		/**< parameter updates subscription */
+	uORB::Subscription			_sensor_bias_sub{ORB_ID(sensor_bias)};		/**< sensor in-run bias correction subscription */
+
+	uORB::SubscriptionCallbackWorkItem	_sensor_correction_sub{this, ORB_ID(sensor_correction)};	/**< sensor thermal correction subscription */
+	uORB::SubscriptionCallbackWorkItem	_sensor_gyro_sub[MAX_GYRO_COUNT] {				/**< gyro data subscription */
+		{this, ORB_ID(sensor_gyro), 0},
+		{this, ORB_ID(sensor_gyro), 1},
+		{this, ORB_ID(sensor_gyro), 2}
+	};
+
+	matrix::Dcmf				_board_rotation;				/**< rotation matrix for the orientation that the board is mounted */
+
+	matrix::Vector3f			_offset;
+	matrix::Vector3f			_scale;
+	matrix::Vector3f			_bias;
+
+	perf_counter_t				_cycle_perf;
+	perf_counter_t				_interval_perf;
+	perf_counter_t				_sensor_gyro_latency_perf;
+
+	int					_selected_gyro{-1};
+
+};