Add vehicle_angular_acceleration topic and sensor

msg/CMakeLists.txt

@@ -130,6 +130,7 @@ set(msg_files
 	ulog_stream_ack.msg
 	vehicle_acceleration.msg
 	vehicle_air_data.msg
+	vehicle_angular_acceleration.msg
 	vehicle_angular_velocity.msg
 	vehicle_attitude.msg
 	vehicle_attitude_setpoint.msg

msg/vehicle_angular_acceleration.msg

@@ -0,0 +1,7 @@
+
+uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
+
+float32[3] xyz		# angular acceleration about X, Y, Z body axis in rad/s^2,
+			# computed by differentiating vehicle_angular_velocity and applying a low pass filter
+			# can be used to verify that the vehicle correctly tracks angular acceleration setpoints

src/modules/sensors/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_subdirectory(vehicle_acceleration)
 add_subdirectory(vehicle_angular_velocity)
+add_subdirectory(vehicle_angular_acceleration)
 
 px4_add_module(
 	MODULE modules__sensors
@@ -54,4 +55,5 @@ px4_add_module(
 		mathlib
 		vehicle_acceleration
 		vehicle_angular_velocity
+		vehicle_angular_acceleration
 	)

src/modules/sensors/parameters.cpp

@@ -155,6 +155,8 @@ void initialize_parameter_handles(ParameterHandles &parameter_handles)
 	parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN");
 	parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM");
 
+	parameter_handles.imu_dgyro_en = param_find("IMU_DGYRO_EN");
+
 	// These are parameters for which QGroundControl always expects to be returned in a list request.
 	// We do a param_find here to force them into the list.
 	(void)param_find("RC_CHAN_CNT");
@@ -385,6 +387,8 @@ int update_parameters(const ParameterHandles &parameter_handles, Parameters &par
 	param_get(parameter_handles.air_tube_length, &parameters.air_tube_length);
 	param_get(parameter_handles.air_tube_diameter_mm, &parameters.air_tube_diameter_mm);
 
+	param_get(parameter_handles.imu_dgyro_en, &parameters.imu_dgyro_en);
+
 	return ret;
 }
 

src/modules/sensors/parameters.h

@@ -143,6 +143,8 @@ struct Parameters {
 	int32_t air_cmodel;
 	float air_tube_length;
 	float air_tube_diameter_mm;
+
+	int32_t imu_dgyro_en;
 };
 
 struct ParameterHandles {
@@ -222,6 +224,7 @@ struct ParameterHandles {
 	param_t air_tube_length;
 	param_t air_tube_diameter_mm;
 
+	param_t imu_dgyro_en;
 };
 
 /**

src/modules/sensors/sensor_params.c

@@ -227,6 +227,31 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
 */
 PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
 
+/**
+* Enable computation of angular acceleration by gyro differentiation.
+*
+* @reboot_required true
+*
+* @boolean
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_DGYRO_EN, 1);
+
+/**
+* Cutoff frequency for angular acceleration
+*
+* The cutoff frequency for the 2nd order butterworth filter used on
+* the time derivative of the measured angular velocity.
+* Set to 0 to disable the filter.
+*
+* @min 0
+* @max 1000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 10.0f);
+
 /**
 * Gyro control data maximum publication rate
 *

src/modules/sensors/sensors.cpp

@@ -92,6 +92,7 @@
 
 #include "vehicle_acceleration/VehicleAcceleration.hpp"
 #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp"
+#include "vehicle_angular_acceleration/VehicleAngularAcceleration.hpp"
 
 using namespace DriverFramework;
 using namespace sensors;
@@ -170,8 +171,9 @@ private:
 	VotedSensorsUpdate _voted_sensors_update;
 
 
-	VehicleAcceleration	_vehicle_acceleration;
-	VehicleAngularVelocity	_vehicle_angular_velocity;
+	VehicleAcceleration		_vehicle_acceleration;
+	VehicleAngularVelocity		_vehicle_angular_velocity;
+	VehicleAngularAcceleration	_vehicle_angular_acceleration;
 
 
 	/**
@@ -215,18 +217,24 @@ Sensors::Sensors(bool hil_enabled) :
 	_voted_sensors_update(_parameters, hil_enabled)
 {
 	initialize_parameter_handles(_parameter_handles);
+	parameters_update();
 
 	_airspeed_validator.set_timeout(300000);
 	_airspeed_validator.set_equal_value_threshold(100);
 
 	_vehicle_acceleration.Start();
 	_vehicle_angular_velocity.Start();
+
+	if (_parameters.imu_dgyro_en) {
+		_vehicle_angular_acceleration.Start();
+	}
 }
 
 Sensors::~Sensors()
 {
 	_vehicle_acceleration.Stop();
 	_vehicle_angular_velocity.Stop();
+	_vehicle_angular_acceleration.Stop();
 }
 
 int
@@ -573,6 +581,7 @@ int Sensors::print_status()
 
 	_vehicle_acceleration.PrintStatus();
 	_vehicle_angular_velocity.PrintStatus();
+	_vehicle_angular_acceleration.PrintStatus();
 
 	return 0;
 }

src/modules/sensors/vehicle_angular_acceleration/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(vehicle_angular_acceleration
+	VehicleAngularAcceleration.cpp
+)
+target_link_libraries(vehicle_angular_acceleration PRIVATE px4_work_queue)

src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.cpp

@@ -0,0 +1,156 @@
+/****************************************************************************
+ *
+ *   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 "VehicleAngularAcceleration.hpp"
+
+#include <px4_log.h>
+
+using namespace matrix;
+using namespace time_literals;
+
+VehicleAngularAcceleration::VehicleAngularAcceleration() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
+	_cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: cycle time")),
+	_sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_acceleration: sensor latency"))
+{
+}
+
+VehicleAngularAcceleration::~VehicleAngularAcceleration()
+{
+	Stop();
+
+	perf_free(_cycle_perf);
+	perf_free(_sensor_latency_perf);
+}
+
+bool
+VehicleAngularAcceleration::Start()
+{
+	_angular_velocity_prev.zero();
+
+	// force initial updates
+	ParametersUpdate(true);
+
+	// Register callbacks
+	_vehicle_angular_velocity_sub.registerCallback();
+
+	return true;
+}
+
+void
+VehicleAngularAcceleration::Stop()
+{
+	Deinit();
+
+	// clear all registered callbacks
+	_vehicle_angular_velocity_sub.unregisterCallback();
+}
+
+void
+VehicleAngularAcceleration::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();
+
+		// Low pass filter
+		if (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) > 0.01f) {
+			_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
+			_lp_filter.reset(_angular_velocity_prev);
+		}
+	}
+}
+
+void
+VehicleAngularAcceleration::Run()
+{
+	perf_begin(_cycle_perf);
+
+	vehicle_angular_velocity_s v_angular_velocity;
+
+	if (_vehicle_angular_velocity_sub.update(&v_angular_velocity)) {
+		perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&v_angular_velocity.timestamp));
+
+		ParametersUpdate();
+
+		// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((v_angular_velocity.timestamp_sample - _timestamp_sample_prev) / 1e6f), 0.0002f,
+						 0.02f);
+		_timestamp_sample_prev = v_angular_velocity.timestamp_sample;
+
+		// Differentiate angular velocity
+		Vector3f angular_velocity(v_angular_velocity.xyz);
+		Vector3f angular_acceleration_raw = (angular_velocity - _angular_velocity_prev) / dt;
+		_angular_velocity_prev = angular_velocity;
+
+		// Apply low pass filter
+		Vector3f angular_acceleration(_lp_filter.apply(angular_acceleration_raw));
+
+		// Publish
+		vehicle_angular_acceleration_s v_angular_acceleration;
+		v_angular_acceleration.timestamp = hrt_absolute_time();
+		v_angular_acceleration.timestamp_sample = v_angular_velocity.timestamp_sample;
+		angular_acceleration.copyTo(v_angular_acceleration.xyz);
+
+		_vehicle_angular_acceleration_pub.publish(v_angular_acceleration);
+
+		// Calculate loop rate
+		_dt_accumulator += dt;
+		++_loop_counter;
+
+		if (_dt_accumulator > 1.f) {
+			const float loop_update_rate = (float)_loop_counter / _dt_accumulator;
+			_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
+			_dt_accumulator = 0;
+			_loop_counter = 0;
+
+			if (fabsf(_lp_filter.get_sample_freq() - _loop_update_rate_hz) > 1.0f) {
+				_lp_filter.set_cutoff_frequency(_loop_update_rate_hz, _param_imu_dgyro_cutoff.get());
+			}
+		}
+	}
+
+	perf_end(_cycle_perf);
+}
+
+void
+VehicleAngularAcceleration::PrintStatus()
+{
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_sensor_latency_perf);
+}

src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   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/mathlib/math/Limits.hpp>
+#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
+#include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/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/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_angular_acceleration.h>
+
+class VehicleAngularAcceleration : public ModuleParams, public px4::WorkItem
+{
+public:
+
+	VehicleAngularAcceleration();
+	virtual ~VehicleAngularAcceleration();
+
+	void	Run() override;
+
+	bool	Start();
+	void	Stop();
+
+	void	PrintStatus();
+
+private:
+
+	void	ParametersUpdate(bool force = false);
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::IMU_DGYRO_CUTOFF>) _param_imu_dgyro_cutoff
+	)
+
+	uORB::Publication<vehicle_angular_acceleration_s> _vehicle_angular_acceleration_pub{ORB_ID(vehicle_angular_acceleration)};
+
+	uORB::SubscriptionCallbackWorkItem 	_vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};	/**< angular velocity subscription */
+	uORB::Subscription			_params_sub{ORB_ID(parameter_update)};					/**< parameter updates subscription */
+
+	math::LowPassFilter2pVector3f _lp_filter{initial_update_rate_hz, 10.f};	/**< low-pass filter for angular acceleration */
+	static constexpr const float initial_update_rate_hz = 1000.f; 		/**< loop update rate used for initialization */
+	float _loop_update_rate_hz{initial_update_rate_hz};           		/**< current rate-controller loop update rate in [Hz] */
+
+	matrix::Vector3f			_angular_velocity_prev;
+
+	hrt_abstime _timestamp_sample_prev{0};
+	float _dt_accumulator{0.0f};
+	int _loop_counter{0};
+
+	perf_counter_t				_cycle_perf;
+	perf_counter_t				_sensor_latency_perf;
+};