From 3e4137ffe8e32ca4ba9d5520b3ac342334e6ca60 Mon Sep 17 00:00:00 2001 From: Julien Lecoeur Date: Thu, 17 Oct 2019 12:23:23 +0200 Subject: [PATCH] Add vehicle_angular_acceleration topic and sensor --- msg/CMakeLists.txt | 1 + msg/vehicle_angular_acceleration.msg | 7 + src/modules/sensors/CMakeLists.txt | 2 + src/modules/sensors/parameters.cpp | 4 + src/modules/sensors/parameters.h | 3 + src/modules/sensors/sensor_params.c | 25 +++ src/modules/sensors/sensors.cpp | 13 +- .../CMakeLists.txt | 37 +++++ .../VehicleAngularAcceleration.cpp | 156 ++++++++++++++++++ .../VehicleAngularAcceleration.hpp | 91 ++++++++++ 10 files changed, 337 insertions(+), 2 deletions(-) create mode 100644 msg/vehicle_angular_acceleration.msg create mode 100644 src/modules/sensors/vehicle_angular_acceleration/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.cpp create mode 100644 src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 43cff31fc5..60b240d127 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/vehicle_angular_acceleration.msg b/msg/vehicle_angular_acceleration.msg new file mode 100644 index 0000000000..ea183ed5f0 --- /dev/null +++ b/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 diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 0a5b6620f1..0678fa0da2 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/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 ) diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index c6783717ad..52c3e00e31 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -155,6 +155,8 @@ void initialize_parameter_handles(ParameterHandles ¶meter_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 ¶meter_handles, Parameters &par param_get(parameter_handles.air_tube_length, ¶meters.air_tube_length); param_get(parameter_handles.air_tube_diameter_mm, ¶meters.air_tube_diameter_mm); + param_get(parameter_handles.imu_dgyro_en, ¶meters.imu_dgyro_en); + return ret; } diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index 6c7cd44aaf..8420e49bd6 100644 --- a/src/modules/sensors/parameters.h +++ b/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; }; /** diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dbfbcd03da..f4a3cbf770 100644 --- a/src/modules/sensors/sensor_params.c +++ b/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 * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fccd8359eb..78cd377017 100644 --- a/src/modules/sensors/sensors.cpp +++ b/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; } diff --git a/src/modules/sensors/vehicle_angular_acceleration/CMakeLists.txt b/src/modules/sensors/vehicle_angular_acceleration/CMakeLists.txt new file mode 100644 index 0000000000..37466ab2d9 --- /dev/null +++ b/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) diff --git a/src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.cpp b/src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.cpp new file mode 100644 index 0000000000..9d4e36f2c3 --- /dev/null +++ b/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 + +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(¶m_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); +} diff --git a/src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.hpp b/src/modules/sensors/vehicle_angular_acceleration/VehicleAngularAcceleration.hpp new file mode 100644 index 0000000000..80f86344df --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +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) _param_imu_dgyro_cutoff + ) + + uORB::Publication _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; +};