diff --git a/src/modules/sensors/sensors/CMakeLists.txt b/src/modules/sensors/sensors/CMakeLists.txt deleted file mode 100644 index 1f84dec4db72..000000000000 --- a/src/modules/sensors/sensors/CMakeLists.txt +++ /dev/null @@ -1,110 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015-2022 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. -# -############################################################################ - -add_subdirectory(data_validator) - -include_directories(${CMAKE_CURRENT_SOURCE_DIR}) - -if(CONFIG_SENSORS_VEHICLE_ACCELERATION) - add_subdirectory(vehicle_acceleration) -endif() - -add_subdirectory(vehicle_imu) - -if(CONFIG_SENSORS_VEHICLE_AIR_DATA) - add_subdirectory(vehicle_air_data) -endif() - -if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - add_subdirectory(vehicle_angular_velocity) -endif() - -if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - add_subdirectory(vehicle_gps_position) -endif() - -if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - add_subdirectory(vehicle_magnetometer) -endif() - -if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - add_subdirectory(vehicle_optical_flow) -endif() - -px4_add_module( - MODULE modules__sensors - MAIN sensors - INCLUDES - ${CMAKE_CURRENT_SOURCE_DIR} - SRCS - sensors.cpp - voted_sensors_update.cpp - voted_sensors_update.h - Integrator.hpp - MODULE_CONFIG - module.yaml - DEPENDS - conversion - data_validator - mathlib - sensor_calibration - vehicle_imu - ) - -if(CONFIG_SENSORS_VEHICLE_ACCELERATION) - target_link_libraries(modules__sensors PRIVATE vehicle_acceleration) -endif() - -if(CONFIG_SENSORS_VEHICLE_AIR_DATA) - target_link_libraries(modules__sensors PRIVATE vehicle_air_data) -endif() - -if(CONFIG_SENSORS_VEHICLE_AIRSPEED) - target_link_libraries(modules__sensors PRIVATE airspeed) -endif() - -if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - target_link_libraries(modules__sensors PRIVATE vehicle_angular_velocity) -endif() - -if(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - target_link_libraries(modules__sensors PRIVATE vehicle_gps_position) -endif() - -if(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - target_link_libraries(modules__sensors PRIVATE vehicle_magnetometer) -endif() - -if(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - target_link_libraries(modules__sensors PRIVATE vehicle_optical_flow) -endif() diff --git a/src/modules/sensors/sensors/Integrator.hpp b/src/modules/sensors/sensors/Integrator.hpp deleted file mode 100644 index b6c26f331e08..000000000000 --- a/src/modules/sensors/sensors/Integrator.hpp +++ /dev/null @@ -1,223 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2022 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. - * - ****************************************************************************/ - -/** - * @file Integrator.hpp - * - * A resettable integrator - * - * @author Lorenz Meier - * @author Julian Oes - */ - -#pragma once - -#include -#include - -namespace sensors -{ - -class Integrator -{ -public: - Integrator() = default; - ~Integrator() = default; - - static constexpr float DT_MIN{1e-6f}; // 1 microsecond - static constexpr float DT_MAX{static_cast(UINT16_MAX) * 1e-6f}; - - /** - * Put an item into the integral. - * - * @param timestamp Timestamp of the current value. - * @param val Item to put. - * @return true if data was accepted and integrated. - */ - inline void put(const matrix::Vector3f &val, const float dt) - { - if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { - _alpha += integrate(val, dt); - - } else { - reset(); - _last_val = val; - } - } - - /** - * Set reset interval during runtime. This won't reset the integrator. - * - * @param reset_interval New reset time interval for the integrator in microseconds. - */ - void set_reset_interval(uint32_t reset_interval_us) { _reset_interval_min = reset_interval_us * 1e-6f; } - - /** - * Set required samples for reset. This won't reset the integrator. - * - * @param reset_samples New reset time interval for the integrator. - */ - void set_reset_samples(uint8_t reset_samples) { _reset_samples_min = reset_samples; } - uint8_t get_reset_samples() const { return _reset_samples_min; } - - /** - * Is the Integrator ready to reset? - * - * @return true if integrator has sufficient data (minimum interval & samples satisfied) to reset. - */ - inline bool integral_ready() const { return (_integrated_samples >= _reset_samples_min) || (_integral_dt >= _reset_interval_min); } - - float integral_dt() const { return _integral_dt; } - - void reset() - { - _alpha.zero(); - _integral_dt = 0; - _integrated_samples = 0; - } - - /* Reset integrator and return current integral & integration time - * - * @param integral_dt Get the dt in us of the current integration. - * @return true if integral valid - */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) - { - if (integral_ready()) { - integral = _alpha; - integral_dt = roundf(_integral_dt * 1e6f); // seconds to microseconds - - reset(); - - return true; - } - - return false; - } - -protected: - - inline matrix::Vector3f integrate(const matrix::Vector3f &val, const float dt) - { - // Use trapezoidal integration to calculate the delta integral - _integrated_samples++; - _integral_dt += dt; - const matrix::Vector3f delta_alpha{(val + _last_val) *dt * 0.5f}; - _last_val = val; - - return delta_alpha; - } - - matrix::Vector3f _alpha{0.f, 0.f, 0.f}; /**< integrated value before coning corrections are applied */ - matrix::Vector3f _last_val{0.f, 0.f, 0.f}; /**< previous input */ - float _integral_dt{0}; - - float _reset_interval_min{0.001f}; /**< the interval after which the content will be published and the integrator reset */ - uint8_t _reset_samples_min{1}; - - uint8_t _integrated_samples{0}; -}; - -class IntegratorConing : public Integrator -{ -public: - IntegratorConing() = default; - ~IntegratorConing() = default; - - /** - * Put an item into the integral. - * - * @param timestamp Timestamp of the current value. - * @param val Item to put. - * @return true if data was accepted and integrated. - */ - inline void put(const matrix::Vector3f &val, const float dt) - { - if ((dt > DT_MIN) && (_integral_dt + dt < DT_MAX)) { - // Use trapezoidal integration to calculate the delta integral - const matrix::Vector3f delta_alpha{integrate(val, dt)}; - - // Calculate coning corrections - // Coning compensation derived by Paul Riseborough and Jonathan Challinger, - // following: - // Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms - // Sourced: https://arc.aiaa.org/doi/pdf/10.2514/2.4228 - // Simulated: https://github.com/priseborough/InertialNav/blob/master/models/imu_error_modelling.m - _beta += ((_last_alpha + _last_delta_alpha * (1.f / 6.f)) % delta_alpha) * 0.5f; - _last_delta_alpha = delta_alpha; - _last_alpha = _alpha; - - // accumulate delta integrals - _alpha += delta_alpha; - - } else { - reset(); - _last_val = val; - } - } - - void reset() - { - Integrator::reset(); - _beta.zero(); - _last_alpha.zero(); - } - - const matrix::Vector3f &accumulated_coning_corrections() const { return _beta; } - - /* Reset integrator and return current integral & integration time - * - * @param integral_dt Get the dt in us of the current integration. - * @return true if integral valid - */ - bool reset(matrix::Vector3f &integral, uint16_t &integral_dt) - { - if (Integrator::reset(integral, integral_dt)) { - // apply coning corrections - integral += _beta; - _beta.zero(); - _last_alpha.zero(); - return true; - } - - return false; - } - -private: - matrix::Vector3f _beta{0.f, 0.f, 0.f}; /**< accumulated coning corrections */ - matrix::Vector3f _last_delta_alpha{0.f, 0.f, 0.f}; /**< integral from previous previous sampling interval */ - matrix::Vector3f _last_alpha{0.f, 0.f, 0.f}; /**< previous value of _alpha */ - -}; - -}; // namespace sensors diff --git a/src/modules/sensors/sensors/Kconfig b/src/modules/sensors/sensors/Kconfig deleted file mode 100644 index 9319e8063355..000000000000 --- a/src/modules/sensors/sensors/Kconfig +++ /dev/null @@ -1,43 +0,0 @@ -menuconfig MODULES_SENSORS - bool "sensors" - default n - ---help--- - Enable support for sensors - -menuconfig USER_SENSORS - bool "sensors running as userspace module" - default n - depends on BOARD_PROTECTED && MODULES_SENSORS - ---help--- - Put sensors in userspace memory - -if MODULES_SENSORS - config SENSORS_VEHICLE_AIRSPEED - bool "Include vehicle airspeed" - default y - - config SENSORS_VEHICLE_AIR_DATA - bool "Include vehicle air data" - default y - - config SENSORS_VEHICLE_ANGULAR_VELOCITY - bool "Include vehicle angular velocity" - default y - - config SENSORS_VEHICLE_ACCELERATION - bool "Include vehicle acceleration" - default y - - config SENSORS_VEHICLE_GPS_POSITION - bool "Include vehicle gps position" - default y - - config SENSORS_VEHICLE_MAGNETOMETER - bool "Include vehicle magnetometer" - default y - - config SENSORS_VEHICLE_OPTICAL_FLOW - bool "Include vehicle optical flow" - default y - -endif #MODULES_SENSORS diff --git a/src/modules/sensors/sensors/data_validator/CMakeLists.txt b/src/modules/sensors/sensors/data_validator/CMakeLists.txt deleted file mode 100644 index e60b18d4b308..000000000000 --- a/src/modules/sensors/sensors/data_validator/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018-2020 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(data_validator - DataValidator.cpp - DataValidator.hpp - DataValidatorGroup.cpp - DataValidatorGroup.hpp -) diff --git a/src/modules/sensors/sensors/data_validator/DataValidator.cpp b/src/modules/sensors/sensors/data_validator/DataValidator.cpp deleted file mode 100644 index 75b2f88e88cc..000000000000 --- a/src/modules/sensors/sensors/data_validator/DataValidator.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2020 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. - * - ****************************************************************************/ - -/** - * @file DataValidator.cpp - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "DataValidator.hpp" - -#include -#include - -void DataValidator::put(uint64_t timestamp, float val, uint32_t error_count_in, uint8_t priority_in) -{ - float data[dimensions] = {val}; // sets the first value and all others to 0 - put(timestamp, data, error_count_in, priority_in); -} - -void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint32_t error_count_in, uint8_t priority_in) -{ - _event_count++; - - if (error_count_in > _error_count) { - _error_density += (error_count_in - _error_count); - - } else if (_error_density > 0) { - _error_density--; - } - - _error_count = error_count_in; - _priority = priority_in; - - for (unsigned i = 0; i < dimensions; i++) { - if (PX4_ISFINITE(val[i])) { - if (_time_last == 0) { - _mean[i] = 0; - _lp[i] = val[i]; - _M2[i] = 0; - - } else { - float lp_val = val[i] - _lp[i]; - - float delta_val = lp_val - _mean[i]; - _mean[i] += delta_val / _event_count; - _M2[i] += delta_val * (lp_val - _mean[i]); - _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); - - if (fabsf(_value[i] - val[i]) < 0.000001f) { - _value_equal_count++; - - } else { - _value_equal_count = 0; - } - } - - // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.99f + 0.01f * val[i]; - - _value[i] = val[i]; - } - } - - _time_last = timestamp; -} - -float DataValidator::confidence(uint64_t timestamp) -{ - - float ret = 1.0f; - - /* check if we have any data */ - if (_time_last == 0) { - _error_mask |= ERROR_FLAG_NO_DATA; - ret = 0.0f; - - } else if (timestamp > _time_last + _timeout_interval) { - /* timed out - that's it */ - _error_mask |= ERROR_FLAG_TIMEOUT; - ret = 0.0f; - - } else if (_value_equal_count > _value_equal_count_threshold) { - /* we got the exact same sensor value N times in a row */ - _error_mask |= ERROR_FLAG_STALE_DATA; - ret = 0.0f; - - } else if (_error_count > NORETURN_ERRCOUNT) { - /* check error count limit */ - _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; - ret = 0.0f; - - } else if (_error_density > ERROR_DENSITY_WINDOW) { - /* cap error density counter at window size */ - _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; - _error_density = ERROR_DENSITY_WINDOW; - } - - /* no critical errors */ - if (ret > 0.0f) { - /* return local error density for last N measurements */ - ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); - - if (ret > 0.0f) { - _error_mask = ERROR_FLAG_NO_ERROR; - } - } - - return ret; -} - -void DataValidator::print() -{ - if (_time_last == 0) { - PX4_INFO_RAW("\tno data\n"); - return; - } - - for (unsigned i = 0; i < dimensions; i++) { - PX4_INFO_RAW("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f\n", (double)_value[i], - (double)_lp[i], (double)_mean[i], (double)_rms[i], (double)confidence(hrt_absolute_time())); - } -} diff --git a/src/modules/sensors/sensors/data_validator/DataValidator.hpp b/src/modules/sensors/sensors/data_validator/DataValidator.hpp deleted file mode 100644 index 9c4bb105b14d..000000000000 --- a/src/modules/sensors/sensors/data_validator/DataValidator.hpp +++ /dev/null @@ -1,200 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2020 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. - * - ****************************************************************************/ - -/** - * @file DataValidator.hpp - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include -#include - -class DataValidator -{ -public: - static const unsigned dimensions = 3; - - DataValidator() = default; - ~DataValidator() = default; - - /** - * Put an item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, float val, uint32_t error_count, uint8_t priority); - - /** - * Put a 3D item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, const float val[dimensions], uint32_t error_count, uint8_t priority); - - /** - * Get the next sibling in the group - * - * @return the next sibling - */ - DataValidator *sibling() { return _sibling; } - - /** - * Set the sibling to the next node in the group - * - */ - void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; } - - /** - * Get the confidence of this validator - * @return the confidence between 0 and 1 - */ - float confidence(uint64_t timestamp); - - /** - * Get the error count of this validator - * @return the error count - */ - uint32_t error_count() const { return _error_count; } - - /** - * Get the values of this validator - * @return the stored value - */ - float *value() { return _value; } - - /** - * Get the used status of this validator - * @return true if this validator ever saw data - */ - bool used() const { return (_time_last > 0); } - - /** - * Get the priority of this validator - * @return the stored priority - */ - uint8_t priority() const { return _priority; } - - /** - * Get the error state of this validator - * @return the bitmask with the error status - */ - uint32_t state() const { return _error_mask; } - - /** - * Reset the error state of this validator - */ - void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } - - /** - * Get the RMS values of this validator - * @return the stored RMS - */ - float *rms() { return _rms; } - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } - - /** - * Set the equal count threshold - * - * @param threshold The number of equal values before considering the sensor stale - */ - void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; } - - /** - * Get the timeout value - * - * @return The timeout interval in microseconds - */ - uint32_t get_timeout() const { return _timeout_interval; } - - /** - * Data validator error states - */ - static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); - static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); - static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); - static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); - -private: - uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */ - - uint32_t _timeout_interval{40000}; /**< interval in which the datastream times out in us */ - - uint64_t _time_last{0}; /**< last timestamp */ - uint64_t _event_count{0}; /**< total data counter */ - uint32_t _error_count{0}; /**< error count */ - - int _error_density{0}; /**< ratio between successful reads and errors */ - - uint8_t _priority{0}; /**< sensor nominal priority */ - - float _mean[dimensions] {}; /**< mean of value */ - float _lp[dimensions] {}; /**< low pass value */ - float _M2[dimensions] {}; /**< RMS component value */ - float _rms[dimensions] {}; /**< root mean square error */ - float _value[dimensions] {}; /**< last value */ - - unsigned _value_equal_count{0}; /**< equal values in a row */ - unsigned _value_equal_count_threshold{ - VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */ - - DataValidator *_sibling{nullptr}; /**< sibling in the group */ - - static const constexpr unsigned NORETURN_ERRCOUNT = - 10000; /**< if the error count reaches this value, return sensor as invalid */ - static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ - static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = - 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ - - /* we don't want this class to be copied */ - DataValidator(const DataValidator &) = delete; - DataValidator operator=(const DataValidator &) = delete; -}; diff --git a/src/modules/sensors/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/sensors/data_validator/DataValidatorGroup.cpp deleted file mode 100644 index a7ded78d26cb..000000000000 --- a/src/modules/sensors/sensors/data_validator/DataValidatorGroup.cpp +++ /dev/null @@ -1,343 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2020 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. - * - ****************************************************************************/ - -/** - * @file DataValidatorGroup.cpp - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "DataValidatorGroup.hpp" - -#include - -#include - -DataValidatorGroup::DataValidatorGroup(unsigned siblings) -{ - - DataValidator *next = nullptr; - DataValidator *prev = nullptr; - - for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(); - - if (i == 0) { - _first = next; - - } else { - prev->setSibling(next); - } - - prev = next; - } - - _last = next; - - if (_first) { - _timeout_interval_us = _first->get_timeout(); - } -} - -DataValidatorGroup::~DataValidatorGroup() -{ - while (_first) { - DataValidator *next = _first->sibling(); - delete (_first); - _first = next; - } -} - -DataValidator *DataValidatorGroup::add_new_validator() -{ - - DataValidator *validator = new DataValidator(); - - if (!validator) { - return nullptr; - } - - _last->setSibling(validator); - _last = validator; - _last->set_timeout(_timeout_interval_us); - return _last; -} - -void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) -{ - - DataValidator *next = _first; - - while (next != nullptr) { - next->set_timeout(timeout_interval_us); - next = next->sibling(); - } - - _timeout_interval_us = timeout_interval_us; -} - -void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) -{ - - DataValidator *next = _first; - - while (next != nullptr) { - next->set_equal_value_threshold(threshold); - next = next->sibling(); - } -} - -void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint32_t error_count, - uint8_t priority) -{ - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - next->put(timestamp, val, error_count, priority); - break; - } - - next = next->sibling(); - i++; - } -} - -float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) -{ - - DataValidator *next = _first; - - // XXX This should eventually also include voting - int pre_check_best = _curr_best; - float pre_check_confidence = 1.0f; - int pre_check_prio = -1; - float max_confidence = -1.0f; - int max_priority = -1000; - int max_index = -1; - DataValidator *best = nullptr; - - int i = 0; - - // First find the current selected sensor - while (next != nullptr) { - if (i == pre_check_best) { - const int prio = next->priority(); - const float confidence = next->confidence(timestamp); - - pre_check_prio = prio; - pre_check_confidence = confidence; - - max_index = i; - max_confidence = confidence; - max_priority = prio; - best = next; - break; - } - - next = next->sibling(); - i++; - } - - i = 0; - next = _first; - - while (next != nullptr) { - float confidence = next->confidence(timestamp); - - /* - * Switch if: - * 1) the confidence is higher and priority is equal or higher - * 2) the confidence is less than 1% different and the priority is higher - */ - if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || - (confidence > max_confidence && (next->priority() >= max_priority)) || - (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) && - (confidence > 0.0f)) { - max_index = i; - max_confidence = confidence; - max_priority = next->priority(); - best = next; - } - - next = next->sibling(); - i++; - } - - /* the current best sensor is not matching the previous best sensor, - * or the only sensor went bad */ - if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) { - bool true_failsafe = true; - - /* check whether the switch was a failsafe or preferring a higher priority sensor */ - if (pre_check_prio != -1 && pre_check_prio < max_priority && - fabsf(pre_check_confidence - max_confidence) < 0.1f) { - /* this is not a failover */ - true_failsafe = false; - - /* reset error flags, this is likely a hotplug sensor coming online late */ - if (best != nullptr) { - best->reset_state(); - } - } - - /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ - if (_curr_best < 0) { - _prev_best = max_index; - - } else { - /* we were initialized before, this is a real failsafe */ - _prev_best = pre_check_best; - - if (true_failsafe) { - _toggle_count++; - - /* if this is the first time, log when we failed */ - if (_first_failover_time == 0) { - _first_failover_time = timestamp; - } - } - } - - /* for all cases we want to keep a record of the best index */ - _curr_best = max_index; - } - - *index = max_index; - return (best) ? best->value() : nullptr; -} - -void DataValidatorGroup::print() -{ - PX4_INFO_RAW("validator: best: %d, prev best: %d, failsafe: %s (%u events)\n", _curr_best, _prev_best, - (_toggle_count > 0) ? "YES" : "NO", _toggle_count); - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used()) { - uint32_t flags = next->state(); - - PX4_INFO_RAW("sensor #%u, prio: %d, state:%s%s%s%s%s%s\n", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); - - next->print(); - } - - next = next->sibling(); - i++; - } -} - -int DataValidatorGroup::failover_index() -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && - (i == (unsigned)_prev_best)) { - return i; - } - - next = next->sibling(); - i++; - } - - return -1; -} - -uint32_t DataValidatorGroup::failover_state() -{ - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && - (i == (unsigned)_prev_best)) { - return next->state(); - } - - next = next->sibling(); - i++; - } - - return DataValidator::ERROR_FLAG_NO_ERROR; -} - -uint32_t DataValidatorGroup::get_sensor_state(unsigned index) -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - return next->state(); - } - - next = next->sibling(); - i++; - } - - // sensor index not found - return UINT32_MAX; -} - -uint8_t DataValidatorGroup::get_sensor_priority(unsigned index) -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - return next->priority(); - } - - next = next->sibling(); - i++; - } - - // sensor index not found - return 0; -} diff --git a/src/modules/sensors/sensors/data_validator/DataValidatorGroup.hpp b/src/modules/sensors/sensors/data_validator/DataValidatorGroup.hpp deleted file mode 100644 index 5ba6ea1cae3d..000000000000 --- a/src/modules/sensors/sensors/data_validator/DataValidatorGroup.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-2020 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. - * - ****************************************************************************/ - -/** - * @file DataValidatorGroup.hpp - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include "DataValidator.hpp" - -class DataValidatorGroup -{ -public: - /** - * @param siblings initial number of DataValidator's. Must be > 0. - */ - DataValidatorGroup(unsigned siblings); - ~DataValidatorGroup(); - - /** - * Create a new Validator (with index equal to the number of currently existing validators) - * @return the newly created DataValidator or nullptr on error - */ - DataValidator *add_new_validator(); - - /** - * Put an item into the validator group. - * - * @param index Sensor index - * @param timestamp The timestamp of the measurement - * @param val The 3D vector - * @param error_count The current error count of the sensor - * @param priority The priority of the sensor - */ - void put(unsigned index, uint64_t timestamp, const float val[3], uint32_t error_count, uint8_t priority); - - /** - * Get the best data triplet of the group - * - * @return pointer to the array of best values - */ - float *get_best(uint64_t timestamp, int *index); - - /** - * Get the number of failover events - * - * @return the number of failovers - */ - unsigned failover_count() const { return _toggle_count; } - - /** - * Get the index of the failed sensor in the group - * - * @return index of the failed sensor - */ - int failover_index(); - - /** - * Get the error state of the failed sensor in the group - * - * @return bitmask with erro states of the failed sensor - */ - uint32_t failover_state(); - - /** - * Get the error state of the sensor with the specified index - * - * @return bitmask with error states of the sensor - */ - uint32_t get_sensor_state(unsigned index); - - /** - * Get the priority of the sensor with the specified index - * - * @return priority - */ - uint8_t get_sensor_priority(unsigned index); - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value for the whole group - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint32_t timeout_interval_us); - - /** - * Set the equal count threshold for the whole group - * - * @param threshold The number of equal values before considering the sensor stale - */ - void set_equal_value_threshold(uint32_t threshold); - -private: - DataValidator *_first{nullptr}; /**< first node in the group */ - DataValidator *_last{nullptr}; /**< last node in the group */ - - uint32_t _timeout_interval_us{0}; /**< currently set timeout */ - - int _curr_best{-1}; /**< currently best index */ - int _prev_best{-1}; /**< the previous best index */ - - uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */ - - unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */ - - static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; - - /* we don't want this class to be copied */ - DataValidatorGroup(const DataValidatorGroup &); - DataValidatorGroup operator=(const DataValidatorGroup &); -}; diff --git a/src/modules/sensors/sensors/data_validator/tests/CMakeLists.txt b/src/modules/sensors/sensors/data_validator/tests/CMakeLists.txt deleted file mode 100644 index 641b178b6360..000000000000 --- a/src/modules/sensors/sensors/data_validator/tests/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -add_executable(ecl_tests_data_validator test_data_validator.cpp tests_common.cpp) -target_link_libraries(ecl_tests_data_validator ecl_validation) - -add_test(NAME ecl_tests_data_validator - COMMAND ecl_tests_data_validator - ) - -add_executable(ecl_tests_data_validator_group test_data_validator_group.cpp tests_common.cpp) -target_link_libraries(ecl_tests_data_validator_group ecl_validation) - -add_test(NAME ecl_tests_data_validator_group - COMMAND ecl_tests_data_validator_group - ) diff --git a/src/modules/sensors/sensors/data_validator/tests/test_data_validator.cpp b/src/modules/sensors/sensors/data_validator/tests/test_data_validator.cpp deleted file mode 100644 index 56d1238b9218..000000000000 --- a/src/modules/sensors/sensors/data_validator/tests/test_data_validator.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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. - * - ****************************************************************************/ -/** - * @file test_data_validator.cpp - * Testing the DataValidator class - * - * @author Todd Stellanova - */ - -#include -#include -#include -#include -#include -#include -#include - - -void test_init() -{ - printf("\n--- test_init ---\n"); - - uint64_t fake_timestamp = 666; - const uint32_t timeout_usec = 2000;//from original private value - - DataValidator *validator = new DataValidator; - // initially there should be no siblings - assert(nullptr == validator->sibling()); - // initially we should have zero confidence - assert(0.0f == validator->confidence(fake_timestamp)); - // initially the error count should be zero - assert(0 == validator->error_count()); - // initially unused - assert(!validator->used()); - // initially no priority - assert(0 == validator->priority()); - validator->set_timeout(timeout_usec); - assert(validator->get_timeout() == timeout_usec); - - - DataValidator *sibling_validator = new DataValidator; - validator->setSibling(sibling_validator); - assert(sibling_validator == validator->sibling()); - - //verify that with no data, confidence is zero and error mask is set - assert(0.0f == validator->confidence(fake_timestamp + 1)); - uint32_t state = validator->state(); - assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state)); - - //verify that calling print doesn't crash tests - validator->print(); - - delete validator; //force delete -} - -void test_put() -{ - printf("\n--- test_put ---\n"); - - uint64_t timestamp = 500; - const uint32_t timeout_usec = 2000;//derived from class-private value - float val = 3.14159f; - //derived from class-private value: this is min change needed to avoid stale detection - const float sufficient_incr_value = (1.1f * 1E-6f); - - DataValidator *validator = new DataValidator; - fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); - - assert(validator->used()); - //verify that the last value we inserted is the current validator value - float last_val = val - sufficient_incr_value; - assert(validator->value()[0] == last_val); - - // we've just provided a bunch of valid data: should be fully confident - float conf = validator->confidence(timestamp); - - if (1.0f != conf) { - printf("conf: %f\n", (double)conf); - dump_validator_state(validator); - } - - assert(1.0f == conf); - // should be no errors - assert(0 == validator->state()); - - //now check confidence much beyond the timeout window-- should timeout - conf = validator->confidence(timestamp + (1.1 * timeout_usec)); - - if (0.0f != conf) { - printf("conf: %f\n", (double)conf); - dump_validator_state(validator); - } - - assert(0.0f == conf); - assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); - - delete validator; //force delete -} - -/** - * Verify that the DataValidator detects sensor data that does not vary sufficiently - */ -void test_stale_detector() -{ - printf("\n--- test_stale_detector ---\n"); - - uint64_t timestamp = 500; - float val = 3.14159f; - //derived from class-private value, this is insufficient to avoid stale detection: - const float insufficient_incr_value = (0.99 * 1E-6f); - - DataValidator *validator = new DataValidator; - fill_validator_with_samples(validator, insufficient_incr_value, &val, ×tamp); - - // data is stale: should have no confidence - assert(0.0f == validator->confidence(timestamp)); - - // should be a stale error - uint32_t state = validator->state(); - - if (DataValidator::ERROR_FLAG_STALE_DATA != state) { - dump_validator_state(validator); - } - - assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state)); - - delete validator; //force delete -} - -/** - * Verify the RMS error calculated by the DataValidator for a series of samples - */ -void test_rms_calculation() -{ - printf("\n--- test_rms_calculation ---\n"); - const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT - const float mean_value = 3.14159f; - const uint32_t sample_count = 1000; - float expected_rms_err = 0.0f; - uint64_t timestamp = 500; - - DataValidator *validator = new DataValidator; - validator->set_equal_value_threshold(equal_value_count); - - insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, ×tamp); - float *rms = validator->rms(); - assert(nullptr != rms); - float calc_rms_err = rms[0]; - float diff = fabsf(calc_rms_err - expected_rms_err); - float diff_frac = (diff / expected_rms_err); - printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err, - (double)diff, (double)diff_frac); - assert(diff_frac < 0.03f); - - delete validator; //force delete -} - -/** - * Verify error tracking performed by DataValidator::put - */ -void test_error_tracking() -{ - printf("\n--- test_error_tracking ---\n"); - uint64_t timestamp = 500; - uint64_t timestamp_incr = 5; - const uint32_t timeout_usec = 2000;//from original private value - float val = 3.14159f; - uint32_t error_count = 0; - int expected_error_density = 0; - uint8_t priority = 50; - //from private value: this is min change needed to avoid stale detection - const float sufficient_incr_value = (1.1f * 1E-6f); - //default is private VALUE_EQUAL_COUNT_DEFAULT - const int equal_value_count = 50000; - //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT - const int total_iterations = 1000; - - DataValidator *validator = new DataValidator; - validator->set_timeout(timeout_usec); - validator->set_equal_value_threshold(equal_value_count); - - //put a bunch of values that are all different - for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - - //up to a 50% random error rate appears to pass the error density filter - if ((((float)rand() / (float)RAND_MAX)) < 0.500f) { - error_count += 1; - expected_error_density += 1; - - } else if (expected_error_density > 0) { - expected_error_density -= 1; - } - - validator->put(timestamp, val, error_count, priority); - } - - assert(validator->used()); - //at this point, error_count should be less than NORETURN_ERRCOUNT - assert(validator->error_count() == error_count); - - // we've just provided a bunch of valid data with some errors: - // confidence should be reduced by the number of errors - float conf = validator->confidence(timestamp); - printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf); - assert(1.0f != conf); //we should not be fully confident - assert(0.0f != conf); //neither should we be completely unconfident - // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT - assert(0 == validator->state()); - - // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW) - // ERROR_DENSITY_WINDOW is currently private, but == 100.0f - float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f); - double diff = fabs(reduced_conf - conf); - - if (reduced_conf != conf) { - printf("conf: %f reduced_conf: %f diff: %f\n", - (double)conf, (double)reduced_conf, diff); - dump_validator_state(validator); - } - - assert(diff < 1E-6f); - - //Now, insert a series of errors and ensure we trip the error detector - for (int i = 0; i < 250; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - //100% error rate - error_count += 1; - expected_error_density += 1; - validator->put(timestamp, val, error_count, priority); - } - - conf = validator->confidence(timestamp); - assert(0.0f == conf); // should we be completely unconfident - // we should have triggered the high error density detector - assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state())); - - - validator->reset_state(); - - //Now insert so many errors that we exceed private NORETURN_ERRCOUNT - for (int i = 0; i < 10000; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - //100% error rate - error_count += 1; - expected_error_density += 1; - validator->put(timestamp, val, error_count, priority); - } - - conf = validator->confidence(timestamp); - assert(0.0f == conf); // should we be completely unconfident - // we should have triggered the high error count detector - assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state())); - - delete validator; //force delete - -} - -int main(int argc, char *argv[]) -{ - (void)argc; // unused - (void)argv; // unused - - srand(666); - test_init(); - test_put(); - test_stale_detector(); - test_rms_calculation(); - test_error_tracking(); - - return 0; //passed -} diff --git a/src/modules/sensors/sensors/data_validator/tests/test_data_validator_group.cpp b/src/modules/sensors/sensors/data_validator/tests/test_data_validator_group.cpp deleted file mode 100644 index 2af5c2acd4e2..000000000000 --- a/src/modules/sensors/sensors/data_validator/tests/test_data_validator_group.cpp +++ /dev/null @@ -1,385 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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. - * - ****************************************************************************/ -/** - * @file test_data_validator_group.cpp - * Testing the DataValidatorGroup class - * - * @author Todd Stellanova - */ - -#include -#include -#include -#include -#include -#include -#include -#include - - -const uint32_t base_timeout_usec = 2000;//from original private value -const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT -const uint64_t base_timestamp = 666; -const unsigned base_num_siblings = 4; - - -/** - * Initialize a DataValidatorGroup with some common settings; - * @param sibling_count (out) the number of siblings initialized - */ -DataValidatorGroup *setup_base_group(unsigned *sibling_count) -{ - unsigned num_siblings = base_num_siblings; - - DataValidatorGroup *group = new DataValidatorGroup(num_siblings); - assert(nullptr != group); - //verify that calling print doesn't crash the tests - group->print(); - printf("\n"); - - //should be no failovers yet - assert(0 == group->failover_count()); - assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); - assert(-1 == group->failover_index()); - - //this sets the timeout on all current members of the group, as well as members added later - group->set_timeout(base_timeout_usec); - //the following sets the threshold on all CURRENT members of the group, but not any added later - //TODO This is likely a bug in DataValidatorGroup - group->set_equal_value_threshold(equal_value_count); - - //return values - *sibling_count = num_siblings; - - return group; -} - -/** - * Fill one DataValidator with samples, by index. - * - * @param group - * @param val1_idx Index of the validator to fill with samples - * @param num_samples - */ -void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples) -{ - uint64_t timestamp = base_timestamp; - uint32_t error_count = 0; - float last_best_val = 0.0f; - - for (uint32_t i = 0; i < num_samples; i++) { - float val = ((float) rand() / (float) RAND_MAX); - float data[DataValidator::dimensions] = {val}; - group->put(val1_idx, timestamp, data, error_count, 100); - last_best_val = val; - } - - int best_idx = 0; - float *best_data = group->get_best(timestamp, &best_idx); - assert(last_best_val == best_data[0]); - assert(best_idx == val1_idx); -} - - - -/** - * Fill two validators in the group with samples, by index. - * Both validators will be filled with the same data, but - * the priority of the first validator will be higher than the second. - * - * @param group - * @param val1_idx index of the first validator to fill - * @param val2_idx index of the second validator to fill - * @param num_samples - */ -void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples) -{ - uint64_t timestamp = base_timestamp; - uint32_t error_count = 0; - float last_best_val = 0.0f; - - for (uint32_t i = 0; i < num_samples; i++) { - float val = ((float) rand() / (float) RAND_MAX); - float data[DataValidator::dimensions] = {val}; - //two sensors with identical values, but different priorities - group->put(val1_idx, timestamp, data, error_count, 100); - group->put(val2_idx, timestamp, data, error_count, 10); - last_best_val = val; - } - - int best_idx = 0; - float *best_data = group->get_best(timestamp, &best_idx); - assert(last_best_val == best_data[0]); - assert(best_idx == val1_idx); - -} - -/** - * Dynamically add a validator to the group after construction - * @param group - * @return - */ -DataValidator *add_validator_to_group(DataValidatorGroup *group) -{ - DataValidator *validator = group->add_new_validator(); - //verify the previously set timeout applies to the new group member - assert(validator->get_timeout() == base_timeout_usec); - //for testing purposes, ensure this newly added member is consistent with the rest of the group - //TODO this is likely a bug in DataValidatorGroup - validator->set_equal_value_threshold(equal_value_count); - - return validator; -} - -/** - * Create a DataValidatorGroup and tack on two additional DataValidators - * - * @param validator1_handle (out) first DataValidator added to the group - * @param validator2_handle (out) second DataValidator added to the group - * @param sibling_count (in/out) in: number of initial siblings to create, out: total - * @return - */ -DataValidatorGroup *setup_group_with_two_validator_handles( - DataValidator **validator1_handle, - DataValidator **validator2_handle, - unsigned *sibling_count) -{ - DataValidatorGroup *group = setup_base_group(sibling_count); - - //now we add validators - *validator1_handle = add_validator_to_group(group); - *validator2_handle = add_validator_to_group(group); - *sibling_count += 2; - - return group; -} - - -void test_init() -{ - unsigned num_siblings = 0; - - DataValidatorGroup *group = setup_base_group(&num_siblings); - - //should not yet be any best value - int best_index = -1; - assert(nullptr == group->get_best(base_timestamp, &best_index)); - - delete group; //force cleanup -} - - -/** - * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority - */ -void test_put() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - printf("num_siblings: %d \n", num_siblings); - unsigned val1_idx = num_siblings - 2; - unsigned val2_idx = num_siblings - 1; - - fill_two_with_valid_data(group, val1_idx, val2_idx, 500); - int best_idx = -1; - float *best_data = group->get_best(timestamp, &best_idx); - assert(nullptr != best_data); - float best_val = best_data[0]; - - float *cur_val1 = validator1->value(); - assert(nullptr != cur_val1); - //printf("cur_val1 %p \n", cur_val1); - assert(best_val == cur_val1[0]); - - float *cur_val2 = validator2->value(); - assert(nullptr != cur_val2); - //printf("cur_val12 %p \n", cur_val2); - assert(best_val == cur_val2[0]); - - delete group; //force cleanup -} - - -/** - * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best". - */ -void test_priority_switch() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - //printf("num_siblings: %d \n",num_siblings); - int val1_idx = (int)num_siblings - 2; - int val2_idx = (int)num_siblings - 1; - uint32_t error_count = 0; - - fill_two_with_valid_data(group, val1_idx, val2_idx, 100); - - int best_idx = -1; - float *best_data = nullptr; - //now, switch the priorities, which switches "best" but doesn't trigger a failover - float new_best_val = 3.14159f; - float data[DataValidator::dimensions] = {new_best_val}; - //a single sample insertion should be sufficient to trigger a priority switch - group->put(val1_idx, timestamp, data, error_count, 1); - group->put(val2_idx, timestamp, data, error_count, 100); - best_data = group->get_best(timestamp, &best_idx); - assert(new_best_val == best_data[0]); - //the new best sensor should now be the sensor with the higher priority - assert(best_idx == val2_idx); - //should not have detected a real failover - assert(0 == group->failover_count()); - - delete group; //cleanup -} - -/** - * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors - */ -void test_simple_failover() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - //printf("num_siblings: %d \n",num_siblings); - int val1_idx = (int)num_siblings - 2; - int val2_idx = (int)num_siblings - 1; - - - fill_two_with_valid_data(group, val1_idx, val2_idx, 100); - - int best_idx = -1; - float *best_data = nullptr; - - //trigger a real failover - float new_best_val = 3.14159f; - float data[DataValidator::dimensions] = {new_best_val}; - //trigger a bunch of errors on the previous best sensor - unsigned val1_err_count = 0; - - for (int i = 0; i < 25; i++) { - group->put(val1_idx, timestamp, data, ++val1_err_count, 100); - group->put(val2_idx, timestamp, data, 0, 10); - } - - assert(validator1->error_count() == val1_err_count); - - //since validator1 is experiencing errors, we should see a failover to validator2 - best_data = group->get_best(timestamp + 1, &best_idx); - assert(nullptr != best_data); - assert(new_best_val == best_data[0]); - assert(best_idx == val2_idx); - //should have detected a real failover - printf("failover_count: %d \n", group->failover_count()); - assert(1 == group->failover_count()); - - //even though validator1 has encountered a bunch of errors, it hasn't failed - assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state()); - - // although we failed over from one sensor to another, this is not the same thing tracked by failover_index - int fail_idx = group->failover_index(); - assert(-1 == fail_idx);//no failed sensor - - //since no sensor has actually hard-failed, the group failover state is NO_ERROR - assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); - - - delete group; //cleanup -} - -/** - * Force once sensor to fail and ensure that we detect it - */ -void test_sensor_failure() -{ - unsigned num_siblings = 0; - uint64_t timestamp = base_timestamp; - const float sufficient_incr_value = (1.1f * 1E-6f); - const uint32_t timeout_usec = 2000;//derived from class-private value - - float val = 3.14159f; - - DataValidatorGroup *group = setup_base_group(&num_siblings); - - //now we add validators - DataValidator *validator = add_validator_to_group(group); - assert(nullptr != validator); - num_siblings++; - int val_idx = num_siblings - 1; - - fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); - //the best should now be the one validator we've filled with samples - - int best_idx = -1; - float *best_data = group->get_best(timestamp, &best_idx); - assert(nullptr != best_data); - //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx); - assert(best_idx == val_idx); - - //now force a timeout failure in the one validator, by checking confidence long past timeout - validator->confidence(timestamp + (1.1 * timeout_usec)); - assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); - - //now that the one sensor has failed, the group should detect this as well - int fail_idx = group->failover_index(); - assert(val_idx == fail_idx); - - delete group; -} - -int main(int argc, char *argv[]) -{ - (void)argc; // unused - (void)argv; // unused - - test_init(); - test_put(); - test_simple_failover(); - test_priority_switch(); - test_sensor_failure(); - - return 0; //passed -} diff --git a/src/modules/sensors/sensors/data_validator/tests/tests_common.cpp b/src/modules/sensors/sensors/data_validator/tests/tests_common.cpp deleted file mode 100644 index 899f7871f332..000000000000 --- a/src/modules/sensors/sensors/data_validator/tests/tests_common.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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 -#include "tests_common.h" - - -void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, - uint64_t *timestamp_io) -{ - uint64_t timestamp = *timestamp_io; - uint64_t timestamp_incr = 5; - const uint32_t error_count = 0; - const uint8_t priority = 50; - const float swing = 1E-2f; - double sum_dev_squares = 0.0f; - - //insert a series of values that swing around the mean - for (uint32_t i = 0; i < count; i++) { - float iter_swing = (0 == (i % 2)) ? swing : -swing; - float iter_val = mean + iter_swing; - float iter_dev = iter_val - mean; - sum_dev_squares += (iter_dev * iter_dev); - timestamp += timestamp_incr; - validator->put(timestamp, iter_val, error_count, priority); - } - - double rms = sqrt(sum_dev_squares / (double)count); - //note: this should be approximately equal to "swing" - *rms_err = (float)rms; - *timestamp_io = timestamp; -} - -void dump_validator_state(DataValidator *validator) -{ - uint32_t state = validator->state(); - printf("state: 0x%x no_data: %d stale: %d timeout:%d\n", - validator->state(), - DataValidator::ERROR_FLAG_NO_DATA & state, - DataValidator::ERROR_FLAG_STALE_DATA & state, - DataValidator::ERROR_FLAG_TIMEOUT & state - ); - validator->print(); - printf("\n"); -} - -void fill_validator_with_samples(DataValidator *validator, - const float incr_value, - float *value_io, - uint64_t *timestamp_io) -{ - uint64_t timestamp = *timestamp_io; - const uint64_t timestamp_incr = 5; //usec - const uint32_t timeout_usec = 2000;//derived from class-private value - - float val = *value_io; - const uint32_t error_count = 0; - const uint8_t priority = 50; //"medium" priority - const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT - - validator->set_equal_value_threshold(equal_value_count); - validator->set_timeout(timeout_usec); - - //put a bunch of values that are all different - for (int i = 0; i < equal_value_count; i++, val += incr_value) { - timestamp += timestamp_incr; - validator->put(timestamp, val, error_count, priority); - } - - *timestamp_io = timestamp; - *value_io = val; - -} diff --git a/src/modules/sensors/sensors/data_validator/tests/tests_common.h b/src/modules/sensors/sensors/data_validator/tests/tests_common.h deleted file mode 100644 index ae1351dfe11e..000000000000 --- a/src/modules/sensors/sensors/data_validator/tests/tests_common.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder 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. - * - ****************************************************************************/ - -#ifndef ECL_TESTS_COMMON_H -#define ECL_TESTS_COMMON_H - -#include - -/** - * Insert a series of samples around a mean value - * @param validator The validator under test - * @param mean The mean value - * @param count The number of samples to insert in the validator - * @param rms_err (out) calculated rms error of the inserted samples - * @param timestamp_io (in/out) in: start timestamp, out: last timestamp - */ -void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, - uint64_t *timestamp_io); - -/** - * Print out the state of a DataValidator - * @param validator - */ -void dump_validator_state(DataValidator *validator); - -/** - * Insert a time series of samples into the validator - * @param validator - * @param incr_value The amount to increment the value by on each iteration - * @param value_io (in/out) in: initial value, out: final value - * @param timestamp_io (in/out) in: initial timestamp, out: final timestamp - */ -void fill_validator_with_samples(DataValidator *validator, - const float incr_value, - float *value_io, - uint64_t *timestamp_io); - -#endif //ECL_TESTS_COMMON_H diff --git a/src/modules/sensors/sensors/module.yaml b/src/modules/sensors/sensors/module.yaml deleted file mode 100644 index 5fa440d5ff24..000000000000 --- a/src/modules/sensors/sensors/module.yaml +++ /dev/null @@ -1,611 +0,0 @@ -__max_num_sensor_instances: &max_num_sensor_instances 4 - -module_name: sensors - -parameters: - - group: Sensor Calibration - definitions: - - # Accelerometer calibration - CAL_ACC${i}_ID: - description: - short: Accelerometer ${i} calibration device ID - long: Device ID of the accelerometer this calibration applies to. - category: System - type: int32 - default: 0 - decimal: 3 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_PRIO: - description: - short: Accelerometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - decimal: 3 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_ROT: - description: - short: Accelerometer ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - min: -1 - max: 40 - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_XOFF: - description: - short: Accelerometer ${i} X-axis offset - category: System - type: float - default: 0.0 - unit: m/s^2 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_YOFF: - description: - short: Accelerometer ${i} Y-axis offset - category: System - type: float - default: 0.0 - unit: m/s^2 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_ZOFF: - description: - short: Accelerometer ${i} Z-axis offset - category: System - type: float - default: 0.0 - unit: m/s^2 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_XSCALE: - description: - short: Accelerometer ${i} X-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_YSCALE: - description: - short: Accelerometer ${i} Y-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_ACC${i}_ZSCALE: - description: - short: Accelerometer ${i} Z-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - # Barometer calibration - CAL_BARO${i}_ID: - description: - short: Barometer ${i} calibration device ID - long: Device ID of the barometer this calibration applies to. - category: System - type: int32 - default: 0 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_BARO${i}_PRIO: - description: - short: Barometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_BARO${i}_OFF: - description: - short: Barometer ${i} offset - category: System - type: float - default: 0.0 - decimal: 3 - #unit: Pa - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - # Gyroscope calibration - CAL_GYRO${i}_ID: - description: - short: Gyroscope ${i} calibration device ID - long: Device ID of the gyroscope this calibration applies to. - category: System - type: int32 - default: 0 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_PRIO: - description: - short: Gyroscope ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_ROT: - description: - short: Gyroscope ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - min: -1 - max: 40 - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_XOFF: - description: - short: Gyroscope ${i} X-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: rad/s - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_YOFF: - description: - short: Gyroscope ${i} Y-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: rad/s - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_GYRO${i}_ZOFF: - description: - short: Gyroscope ${i} Z-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: rad/s - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - # Magnetometer calibration - CAL_MAG${i}_ID: - description: - short: Magnetometer ${i} calibration device ID - long: Device ID of the magnetometer this calibration applies to. - category: System - type: int32 - default: 0 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_PRIO: - description: - short: Magnetometer ${i} priority - category: System - type: enum - values: - -1: Uninitialized - 0: Disabled - 1: Min - 25: Low - 50: Medium (Default) - 75: High - 100: Max - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ROT: - description: - short: Magnetometer ${i} rotation relative to airframe - long: | - An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. - Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW. - category: System - type: enum - values: - -1: Internal - 0: No rotation - 1: Yaw 45° - 2: Yaw 90° - 3: Yaw 135° - 4: Yaw 180° - 5: Yaw 225° - 6: Yaw 270° - 7: Yaw 315° - 8: Roll 180° - 9: Roll 180°, Yaw 45° - 10: Roll 180°, Yaw 90° - 11: Roll 180°, Yaw 135° - 12: Pitch 180° - 13: Roll 180°, Yaw 225° - 14: Roll 180°, Yaw 270° - 15: Roll 180°, Yaw 315° - 16: Roll 90° - 17: Roll 90°, Yaw 45° - 18: Roll 90°, Yaw 90° - 19: Roll 90°, Yaw 135° - 20: Roll 270° - 21: Roll 270°, Yaw 45° - 22: Roll 270°, Yaw 90° - 23: Roll 270°, Yaw 135° - 24: Pitch 90° - 25: Pitch 270° - 26: Pitch 180°, Yaw 90° - 27: Pitch 180°, Yaw 270° - 28: Roll 90°, Pitch 90° - 29: Roll 180°, Pitch 90° - 30: Roll 270°, Pitch 90° - 31: Roll 90°, Pitch 180° - 32: Roll 270°, Pitch 180° - 33: Roll 90°, Pitch 270° - 34: Roll 180°, Pitch 270° - 35: Roll 270°, Pitch 270° - 36: Roll 90°, Pitch 180°, Yaw 90° - 37: Roll 90°, Yaw 270° - 38: Roll 90°, Pitch 68°, Yaw 293° - 39: Pitch 315° - 40: Roll 90°, Pitch 315° - 100: Custom Euler Angle - min: -1 - max: 100 - default: -1 - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ROLL: - description: - short: Magnetometer ${i} Custom Euler Roll Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_PITCH: - description: - short: Magnetometer ${i} Custom Euler Pitch Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YAW: - description: - short: Magnetometer ${i} Custom Euler Yaw Angle - long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" - category: System - type: float - default: 0.0 - min: -180 - max: 180 - unit: deg - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_XOFF: - description: - short: Magnetometer ${i} X-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: gauss - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YOFF: - description: - short: Magnetometer ${i} Y-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: gauss - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ZOFF: - description: - short: Magnetometer ${i} Z-axis offset - category: System - type: float - default: 0.0 - decimal: 3 - unit: gauss - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_XSCALE: - description: - short: Magnetometer ${i} X-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YSCALE: - description: - short: Magnetometer ${i} Y-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ZSCALE: - description: - short: Magnetometer ${i} Z-axis scaling factor - category: System - type: float - default: 1.0 - decimal: 3 - min: 0.1 - max: 3.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_XODIAG: - description: - short: Magnetometer ${i} X-axis off diagonal scale factor - category: System - type: float - default: 0.0 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YODIAG: - description: - short: Magnetometer ${i} Y-axis off diagonal scale factor - category: System - type: float - default: 0.0 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ZODIAG: - description: - short: Magnetometer ${i} Z-axis off diagonal scale factor - category: System - type: float - default: 0.0 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_XCOMP: - description: - short: Magnetometer ${i} X Axis throttle compensation - long: | - Coefficient describing linear relationship between - X component of magnetometer in body frame axis - and either current or throttle depending on value of CAL_MAG_COMP_TYP. - Unit for throttle-based compensation is [G] and - for current-based compensation [G/kA] - category: System - type: float - default: 0.0 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_YCOMP: - description: - short: Magnetometer ${i} Y Axis throttle compensation - long: | - Coefficient describing linear relationship between - Y component of magnetometer in body frame axis - and either current or throttle depending on value of CAL_MAG_COMP_TYP. - Unit for throttle-based compensation is [G] and - for current-based compensation [G/kA] - category: System - type: float - default: 0.0 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 - - CAL_MAG${i}_ZCOMP: - description: - short: Magnetometer ${i} Z Axis throttle compensation - long: | - Coefficient describing linear relationship between - Z component of magnetometer in body frame axis - and either current or throttle depending on value of CAL_MAG_COMP_TYP. - Unit for throttle-based compensation is [G] and - for current-based compensation [G/kA] - category: System - type: float - default: 0.0 - decimal: 3 - volatile: true - num_instances: *max_num_sensor_instances - instance_start: 0 diff --git a/src/modules/sensors/sensors/sensor_params.c b/src/modules/sensors/sensors/sensor_params.c deleted file mode 100644 index 3941212eb1fa..000000000000 --- a/src/modules/sensors/sensors/sensor_params.c +++ /dev/null @@ -1,547 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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. - * - ****************************************************************************/ - -/** - * Airspeed sensor compensation model for the SDP3x - * - * Model with Pitot - * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. - * Model without Pitot (1.5 mm tubes) - * CAL_AIR_TUBED_MM: Not used, 1.5 mm tubes assumed. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor. - * Tube Pressure Drop - * CAL_AIR_TUBED_MM: Diameter in mm of the pitot and tubes, must have the same diameter. - * CAL_AIR_TUBELEN: Length of the tubes connecting the pitot to the sensor and the static + dynamic port length of the pitot. - * - * @value 0 Model with Pitot - * @value 1 Model without Pitot (1.5 mm tubes) - * @value 2 Tube Pressure Drop - * - * @group Sensors - */ -PARAM_DEFINE_INT32(CAL_AIR_CMODEL, 0); - -/** - * Airspeed sensor tube length. - * - * See the CAL_AIR_CMODEL explanation on how this parameter should be set. - * - * @min 0.01 - * @max 2.00 - * @unit m - * - * @group Sensors - */ -PARAM_DEFINE_FLOAT(CAL_AIR_TUBELEN, 0.2f); - -/** - * Airspeed sensor tube diameter. Only used for the Tube Pressure Drop Compensation. - * - * @min 1.5 - * @max 100 - * @unit mm - * - * @group Sensors - */ -PARAM_DEFINE_FLOAT(CAL_AIR_TUBED_MM, 1.5f); - -/** - * Differential pressure sensor offset - * - * The offset (zero-reading) in Pascal - * - * @category system - * @group Sensor Calibration - * @volatile - */ -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); - -/** - * Differential pressure sensor analog scaling - * - * Pick the appropriate scaling from the datasheet. - * this number defines the (linear) conversion from voltage - * to Pascal (pa). For the MPXV7002DP this is 1000. - * - * NOTE: If the sensor always registers zero, try switching - * the static and dynamic tubes. - * - * @group Sensor Calibration - */ -PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); - -/** - * Board rotation - * - * This parameter defines the rotation of the FMU board relative to the platform. - * - * @value 0 No rotation - * @value 1 Yaw 45° - * @value 2 Yaw 90° - * @value 3 Yaw 135° - * @value 4 Yaw 180° - * @value 5 Yaw 225° - * @value 6 Yaw 270° - * @value 7 Yaw 315° - * @value 8 Roll 180° - * @value 9 Roll 180°, Yaw 45° - * @value 10 Roll 180°, Yaw 90° - * @value 11 Roll 180°, Yaw 135° - * @value 12 Pitch 180° - * @value 13 Roll 180°, Yaw 225° - * @value 14 Roll 180°, Yaw 270° - * @value 15 Roll 180°, Yaw 315° - * @value 16 Roll 90° - * @value 17 Roll 90°, Yaw 45° - * @value 18 Roll 90°, Yaw 90° - * @value 19 Roll 90°, Yaw 135° - * @value 20 Roll 270° - * @value 21 Roll 270°, Yaw 45° - * @value 22 Roll 270°, Yaw 90° - * @value 23 Roll 270°, Yaw 135° - * @value 24 Pitch 90° - * @value 25 Pitch 270° - * @value 26 Pitch 180°, Yaw 90° - * @value 27 Pitch 180°, Yaw 270° - * @value 28 Roll 90°, Pitch 90° - * @value 29 Roll 180°, Pitch 90° - * @value 30 Roll 270°, Pitch 90° - * @value 31 Roll 90°, Pitch 180° - * @value 32 Roll 270°, Pitch 180° - * @value 33 Roll 90°, Pitch 270° - * @value 34 Roll 180°, Pitch 270° - * @value 35 Roll 270°, Pitch 270° - * @value 36 Roll 90°, Pitch 180°, Yaw 90° - * @value 37 Roll 90°, Yaw 270° - * @value 38 Roll 90°, Pitch 68°, Yaw 293° - * @value 39 Pitch 315° - * @value 40 Roll 90°, Pitch 315° - * - * @min -1 - * @max 40 - * @reboot_required true - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); - -/** - * Board rotation Y (Pitch) offset - * - * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user - * to fine tune the board offset in the event of misalignment. - * - * @unit deg - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); - -/** - * Board rotation X (Roll) offset - * - * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user - * to fine tune the board offset in the event of misalignment. - * - * @unit deg - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); - -/** - * Board rotation Z (YAW) offset - * - * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user - * to fine tune the board offset in the event of misalignment. - * - * @unit deg - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); - -/** - * Thermal control of sensor temperature - * - * @value -1 Thermal control unavailable - * @value 0 Thermal control off - * @value 1 Thermal control enabled - * @category system - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1); - -/** - * External I2C probe. - * - * Probe for optional external I2C devices. - * - * @boolean - * @category system - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_EXT_I2C_PRB, 1); - -/** - * Sensors hub IMU mode - * - * @value 0 Disabled - * @value 1 Publish primary IMU selection - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_IMU_MODE, 1); - -/** - * Enable internal barometers - * - * For systems with an external barometer, this should be set to false to make sure that the external is used. - * - * @boolean - * @reboot_required true - * @category system - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_INT_BARO_EN, 1); - -/** - * ************************************************************************************************** -*/ -/** - * FAILURE - ACCEL - FAULT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_ACCEL_FAULT, 0); - -/** - * FAILURE - ACCEL - NOISE - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_ACCEL_NOISE, 0); - -/** - * FAILURE - ACCEL - BIAS - SHIFT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_ACCEL_SHIF, 0); - -/** - * FAILURE - ACCEL - BIAS - SCALE - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_ACCEL_SCAL, 0); - -/** - * FAILURE - ACCEL - DRIFT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_ACCEL_DRIFT, 0); - -/** - * FAILURE - ACCEL - ZERO - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_ACCEL_ZERO, 0); - - -/** - * *********************************************** -*/ - -/** - * FAILURE - GYRO - FAULT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_GYRO_FAULT, 0); - -/** - * FAILURE - GYRO - NOISE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GYRO_NOISE, 0); - -/** - * FAILURE - GYRO - BIAS - SHIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GYRO_SHIF, 0); - -/** - * FAILURE - GYRO - BIAS - SCALE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GYRO_SCAL, 0); - -/** - * FAILURE - GYRO - DRIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GYRO_DRIFT, 0); - -/** - * FAILURE - GYRO - ZERO - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_GYRO_ZERO, 0); - - -/** - * *********************************************** -*/ - -/** - * FAILURE - MAG - FAULT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_MAG_FAULT, 0); - -/** - * FAILURE - MAG - NOISE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_MAG_NOISE, 0); - -/** - * FAILURE - MAG - BIAS - SHIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_MAG_SHIF, 0); - -/** - * FAILURE - MAG - BIAS - SCALE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_MAG_SCAL, 0); - -/** - * FAILURE - MAG - DRIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_MAG_DRIFT, 0); - -/** - * *********************************************** -*/ - -/** - * FAILURE - BARO - FAULT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_BARO_FAULT, 0); - -/** - * FAILURE - BARO - NOISE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BARO_NOISE, 0); - -/** - * FAILURE - BARO - BIAS - SHIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BARO_SHIF, 0); - -/** - * FAILURE - BARO - BIAS - SCALE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BARO_SCAL, 0); - -/** - * FAILURE - BARO - DRIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_BARO_DRIFT, 0); - - -/** - * *********************************************** -*/ - -/** - * FAILURE - GPS - FAULT - * - * @value 0 Functional - * @value 1 Failure - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_GPS_FAULT, 0); - -/** - * FAILURE - GPS - NOISE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GPS_NOISE, 0); - -/** - * FAILURE - GPS - BIAS - SHIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GPS_SHIF, 0); - -/** - * FAILURE - GPS - BIAS - SCALE - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GPS_SCAL, 0); - -/** - * FAILURE - GPS - DRIFT - * - * - * @category system - * @reboot_required false - * @group Sensors - */ -PARAM_DEFINE_FLOAT(SENS_GPS_DRIFT, 0); diff --git a/src/modules/sensors/sensors/sensor_params_flow.c b/src/modules/sensors/sensors/sensor_params_flow.c deleted file mode 100644 index 174cf5ba944c..000000000000 --- a/src/modules/sensors/sensors/sensor_params_flow.c +++ /dev/null @@ -1,113 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 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. - * - ****************************************************************************/ - -/** - * Optical flow rotation - * - * This parameter defines the yaw rotation of the optical flow relative to the vehicle body frame. - * Zero rotation is defined as X on flow board pointing towards front of vehicle. - * - * @value 0 No rotation - * @value 1 Yaw 45° - * @value 2 Yaw 90° - * @value 3 Yaw 135° - * @value 4 Yaw 180° - * @value 5 Yaw 225° - * @value 6 Yaw 270° - * @value 7 Yaw 315° - * - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); - -/** - * Minimum height above ground when reliant on optical flow. - * - * This parameter defines the minimum distance from ground at which the optical flow sensor operates reliably. - * The sensor may be usable below this height, but accuracy will progressively reduce to loss of focus. - * - * @unit m - * @min 0.0 - * @max 1.0 - * @increment 0.1 - * @decimal 1 - * @group Sensor Calibration - */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MINHGT, 0.08f); - -/** - * Maximum height above ground when reliant on optical flow. - * - * This parameter defines the maximum distance from ground at which the optical flow sensor operates reliably. - * The height setpoint will be limited to be no greater than this value when the navigation system - * is completely reliant on optical flow data and the height above ground estimate is valid. - * The sensor may be usable above this height, but accuracy will progressively degrade. - * - * @unit m - * @min 1.0 - * @max 100.0 - * @increment 0.1 - * @decimal 1 - * @group Sensor Calibration - */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXHGT, 100.f); - -/** - * Magnitude of maximum angular flow rate reliably measurable by the optical flow sensor. - * - * Optical flow data will not fused by the estimators if the magnitude of the flow rate exceeds this value and - * control loops will be instructed to limit ground speed such that the flow rate produced by movement over ground - * is less than 50% of this value. - * - * @unit rad/s - * @min 1.0 - * @decimal 2 - * @group Sensor Calibration - */ -PARAM_DEFINE_FLOAT(SENS_FLOW_MAXR, 8.f); - -/** - * Optical flow max rate. - * - * Optical flow data maximum publication rate. This is an upper bound, - * actual optical flow data rate is still dependent on the sensor. - * - * @min 1 - * @max 200 - * @group Sensors - * @unit Hz - * - * @reboot_required true - * - */ -PARAM_DEFINE_FLOAT(SENS_FLOW_RATE, 70.0f); diff --git a/src/modules/sensors/sensors/sensor_params_mag.c b/src/modules/sensors/sensors/sensor_params_mag.c deleted file mode 100644 index 9ea33c765c04..000000000000 --- a/src/modules/sensors/sensors/sensor_params_mag.c +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2020 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. - * - ****************************************************************************/ - -/** - * Bitfield selecting mag sides for calibration - * - * If set to two side calibration, only the offsets are estimated, the scale - * calibration is left unchanged. Thus an initial six side calibration is - * recommended. - * - * Bits: - * ORIENTATION_TAIL_DOWN = 1 - * ORIENTATION_NOSE_DOWN = 2 - * ORIENTATION_LEFT = 4 - * ORIENTATION_RIGHT = 8 - * ORIENTATION_UPSIDE_DOWN = 16 - * ORIENTATION_RIGHTSIDE_UP = 32 - * - * @min 34 - * @max 63 - * @value 34 Two side calibration - * @value 38 Three side calibration - * @value 63 Six side calibration - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_MAG_SIDES, 63); - -/** - * For legacy QGC support only - * - * Use SENS_MAG_SIDES instead - * - * @group Sensors - * @category Developer - */ -PARAM_DEFINE_INT32(CAL_MAG_SIDES, 63); - -/** - * Type of magnetometer compensation - * - * @value 0 Disabled - * @value 1 Throttle-based compensation - * @value 2 Current-based compensation (battery_status instance 0) - * @value 3 Current-based compensation (battery_status instance 1) - * - * @category system - * @group Sensor Calibration - */ -PARAM_DEFINE_INT32(CAL_MAG_COMP_TYP, 0); - -/** - * Automatically set external rotations. - * - * During calibration attempt to automatically determine the rotation of external magnetometers. - * - * @boolean - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_MAG_AUTOROT, 1); - -/** - * Magnetometer max rate. - * - * Magnetometer data maximum publication rate. This is an upper bound, - * actual magnetometer data rate is still dependent on the sensor. - * - * @min 1 - * @max 200 - * @group Sensors - * @unit Hz - * - * @reboot_required true - * - */ -PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 15.0f); - -/** - * Sensors hub mag mode - * - * @value 0 Publish all magnetometers - * @value 1 Publish primary magnetometer - * - * @category system - * @reboot_required true - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_MAG_MODE, 1); - -/** - * Magnetometer auto calibration - * - * Automatically initialize magnetometer calibration from bias estimate if available. - * - * @boolean - * - * @category system - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_MAG_AUTOCAL, 1); diff --git a/src/modules/sensors/sensors/sensors.cpp b/src/modules/sensors/sensors/sensors.cpp deleted file mode 100644 index 26cc3329da09..000000000000 --- a/src/modules/sensors/sensors/sensors.cpp +++ /dev/null @@ -1,799 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 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. - * - ****************************************************************************/ - -/** - * @file sensors.cpp - * - * @author Lorenz Meier - * @author Julian Oes - * @author Thomas Gubler - * @author Anton Babushkin - * @author Beat Küng - */ - -#include "sensors.hpp" - -Sensors::Sensors(bool hil_enabled) : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _hil_enabled(hil_enabled), - _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _voted_sensors_update(hil_enabled, _vehicle_imu_sub) -{ - _sensor_pub.advertise(); - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - _vehicle_acceleration.Start(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /* Differential pressure offset */ - _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); - _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); - _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - _vehicle_angular_velocity.Start(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - - param_find("SYS_FAC_CAL_MODE"); - - // Parameters controlling the on-board sensor thermal calibrator - param_find("SYS_CAL_TDEL"); - param_find("SYS_CAL_TMAX"); - param_find("SYS_CAL_TMIN"); - - _sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID; - - parameters_update(); - - InitializeVehicleIMU(); -} - -Sensors::~Sensors() -{ - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - _vehicle_acceleration.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - _vehicle_angular_velocity.Stop(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - - if (_vehicle_air_data) { - _vehicle_air_data->Stop(); - delete _vehicle_air_data; - } - -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - - if (_vehicle_gps_position) { - _vehicle_gps_position->Stop(); - delete _vehicle_gps_position; - } - -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - - if (_vehicle_magnetometer) { - _vehicle_magnetometer->Stop(); - delete _vehicle_magnetometer; - } - -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - - if (_vehicle_optical_flow) { - _vehicle_optical_flow->Stop(); - delete _vehicle_optical_flow; - } - -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - - for (auto &vehicle_imu : _vehicle_imu_list) { - if (vehicle_imu) { - vehicle_imu->Stop(); - delete vehicle_imu; - } - } - - perf_free(_loop_perf); -} - -bool Sensors::init() -{ - _vehicle_imu_sub[0].registerCallback(); - ScheduleNow(); - return true; -} - -int Sensors::parameters_update() -{ - if (_armed) { - return 0; - } - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /* Airspeed offset */ - param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); - param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); - param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - - _voted_sensors_update.parametersUpdate(); - - // 1. mark all existing sensor calibrations active even if sensor is missing - // this preserves the calibration in the event of a parameter export while the sensor is missing - // 2. ensure calibration slots are active for the number of sensors currently available - // this to done to eliminate differences in the active set of parameters before and after sensor calibration - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - // sensor_accel - { - const uint32_t device_id_accel = calibration::GetCalibrationParamInt32("ACC", "ID", i); - - if (device_id_accel != 0) { - calibration::Accelerometer accel_cal(device_id_accel); - } - - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().device_id != 0)) { - calibration::Accelerometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } - - - // sensor_gyro - { - const uint32_t device_id_gyro = calibration::GetCalibrationParamInt32("GYRO", "ID", i); - - if (device_id_gyro != 0) { - calibration::Gyroscope gyro_cal(device_id_gyro); - } - - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - - if (sensor_gyro_sub.advertised() && (sensor_gyro_sub.get().device_id != 0)) { - calibration::Gyroscope cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } - - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - // sensor_mag - { - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - - if (device_id_mag != 0) { - calibration::Magnetometer mag_cal(device_id_mag); - } - - uORB::SubscriptionData sensor_mag_sub{ORB_ID(sensor_mag), i}; - - if (sensor_mag_sub.advertised() && (sensor_mag_sub.get().device_id != 0)) { - calibration::Magnetometer cal; - cal.set_calibration_index(i); - cal.ParametersLoad(); - } - } -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - } - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - InitializeVehicleAirData(); -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - InitializeVehicleGPSPosition(); -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - InitializeVehicleMagnetometer(); -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - InitializeVehicleOpticalFlow(); -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - - return PX4_OK; -} - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) -void Sensors::diff_pres_poll() -{ - differential_pressure_s diff_pres{}; - - if (_diff_pres_sub.update(&diff_pres)) { - - if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) { - // ignore invalid data and reset accumulated - - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - return; - } - - vehicle_air_data_s air_data{}; - _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } - - // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; - _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? - - // accumulate average for publication - _diff_pres_timestamp_sum += diff_pres.timestamp_sample; - _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _diff_pres_temperature_sum += air_temperature_celsius; - _baro_pressure_sum += air_data.baro_pressure_pa; - _diff_pres_count++; - - if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) { - - // average data and apply calibration offset (SENS_DPRES_OFF) - const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; - const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; - const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; - - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - - - enum AIRSPEED_SENSOR_MODEL smodel; - - switch ((diff_pres.device_id >> 16) & 0xFF) { - case DRV_DIFF_PRESS_DEVTYPE_SDP31: - - // fallthrough - case DRV_DIFF_PRESS_DEVTYPE_SDP32: - - // fallthrough - case DRV_DIFF_PRESS_DEVTYPE_SDP33: - smodel = AIRSPEED_SENSOR_MODEL_SDP3X; - break; - - default: - smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; - break; - } - - float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - differential_pressure_pa, baro_pressure_pa, temperature); - - // assume that CAS = IAS as we don't have an CAS-scale here - float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); - - if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) { - - airspeed_s airspeed; - airspeed.timestamp_sample = timestamp_sample; - airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; - airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; - airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); - airspeed.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(airspeed); - - _airspeed_last_publish = airspeed.timestamp; - } - } - } -} - -void Sensors::adc_poll() -{ - /* only read if not in HIL mode */ - if (_hil_enabled) { - return; - } - -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - - if (_parameters.diff_pres_analog_scale > 0.0f) { - adc_report_s adc; - - if (_adc_report_sub.update(&adc)) { - /* Read add channels we got */ - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { - if (adc.channel_id[i] == -1) { - continue; // skip non-exist channels - } - - if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) { - - /* calculate airspeed, raw is the difference from */ - const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV; - - /** - * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor. Also assume a non- - * zero offset from the sensor if its connected. - * - * Notice: This won't work on devices which have PGA controlled - * vref. Those devices require no divider at all. - */ - if (voltage > 0.4f) { - const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale; - - differential_pressure_s diff_pres{}; - diff_pres.timestamp_sample = adc.timestamp; - diff_pres.differential_pressure_pa = diff_pres_pa_raw; - diff_pres.temperature = NAN; - diff_pres.timestamp = hrt_absolute_time(); - - _diff_pres_pub.publish(diff_pres); - } - } - } - } - } - -#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL -} -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED) - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) -void Sensors::InitializeVehicleAirData() -{ - if (_param_sys_has_baro.get()) { - if (_vehicle_air_data == nullptr) { - _vehicle_air_data = new VehicleAirData(); - - if (_vehicle_air_data) { - _vehicle_air_data->Start(); - } - } - } -} -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) -void Sensors::InitializeVehicleGPSPosition() -{ - if (_param_sys_has_gps.get()) { - if (_vehicle_gps_position == nullptr) { - _vehicle_gps_position = new VehicleGPSPosition(); - - if (_vehicle_gps_position) { - _vehicle_gps_position->Start(); - } - } - } -} -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -void Sensors::InitializeVehicleIMU() -{ - // create a VehicleIMU instance for each accel/gyro pair - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - if (_vehicle_imu_list[i] == nullptr) { - - uORB::Subscription accel_sub{ORB_ID(sensor_accel), i}; - uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i}; - - if (accel_sub.advertised() && gyro_sub.advertised()) { - // if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ - // otherwise each VehicleIMU runs in a corresponding INSx WQ - const bool multi_mode = (_param_sens_imu_mode.get() == 0); - const px4::wq_config_t &wq_config = multi_mode ? px4::ins_instance_to_wq(i) : px4::wq_configurations::INS0; - - VehicleIMU *imu = new VehicleIMU(i, i, i, wq_config); - - if (imu != nullptr) { - // Start VehicleIMU instance and store - if (imu->Start()) { - _vehicle_imu_list[i] = imu; - - } else { - delete imu; - } - } - - } else { - // abort on first failure, try again later - return; - } - } - } -} - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) -void Sensors::InitializeVehicleMagnetometer() -{ - if (_param_sys_has_mag.get()) { - if (_vehicle_magnetometer == nullptr) { - _vehicle_magnetometer = new VehicleMagnetometer(); - - if (_vehicle_magnetometer) { - _vehicle_magnetometer->Start(); - } - } - } -} -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) -void Sensors::InitializeVehicleOpticalFlow() -{ - if (_vehicle_optical_flow == nullptr) { - uORB::Subscription sensor_optical_flow_sub{ORB_ID(sensor_optical_flow)}; - - if (sensor_optical_flow_sub.advertised()) { - _vehicle_optical_flow = new VehicleOpticalFlow(); - - if (_vehicle_optical_flow) { - _vehicle_optical_flow->Start(); - } - } - } -} -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - -void Sensors::Run() -{ - if (should_exit()) { - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } - - exit_and_cleanup(); - return; - } - - perf_begin(_loop_perf); - - // check vehicle status for changes to publication state - if (_vcontrol_mode_sub.updated()) { - vehicle_control_mode_s vcontrol_mode{}; - - if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { - _armed = vcontrol_mode.flag_armed; - } - } - - // keep adding sensors as long as we are not armed, - // when not adding sensors poll for param updates - if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) { - - bool updated = false; - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - const int n_baro = orb_group_count(ORB_ID(sensor_baro)); - - if (n_baro != _n_baro) { - _n_baro = n_baro; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - const int n_gps = orb_group_count(ORB_ID(sensor_gps)); - - if (n_gps != _n_gps) { - _n_gps = n_gps; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - const int n_mag = orb_group_count(ORB_ID(sensor_mag)); - - if (n_mag != _n_mag) { - _n_mag = n_mag; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - const int n_optical_flow = orb_group_count(ORB_ID(sensor_optical_flow)); - - if (n_optical_flow != _n_optical_flow) { - _n_optical_flow = n_optical_flow; - updated = true; - } - -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - - - const int n_accel = orb_group_count(ORB_ID(sensor_accel)); - const int n_gyro = orb_group_count(ORB_ID(sensor_gyro)); - - if ((n_accel != _n_accel) || (n_gyro != _n_gyro) || updated) { - _n_accel = n_accel; - _n_gyro = n_gyro; - - parameters_update(); - } - - // sensor device id (not just orb_group_count) must be populated before IMU init can succeed - _voted_sensors_update.initializeSensors(); - InitializeVehicleIMU(); - - _last_config_update = hrt_absolute_time(); - - } else { - // check for parameter updates - if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); - - // update parameters from storage - parameters_update(); - updateParams(); - } - } - - _voted_sensors_update.sensorsPoll(_sensor_combined); - - if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) { - - _voted_sensors_update.setRelativeTimestamps(_sensor_combined); - _sensor_pub.publish(_sensor_combined); - _sensor_combined_prev_timestamp = _sensor_combined.timestamp; - } - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - // check analog airspeed - adc_poll(); - diff_pres_poll(); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - - // backup schedule as a watchdog timeout - ScheduleDelayed(10_ms); - - perf_end(_loop_perf); -} - -int Sensors::task_spawn(int argc, char *argv[]) -{ - bool hil_enabled = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'h': - hil_enabled = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - Sensors *instance = new Sensors(hil_enabled); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int Sensors::print_status() -{ - _voted_sensors_update.printStatus(); - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - - if (_vehicle_magnetometer) { - PX4_INFO_RAW("\n"); - _vehicle_magnetometer->PrintStatus(); - } - -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - - if (_vehicle_air_data) { - PX4_INFO_RAW("\n"); - _vehicle_air_data->PrintStatus(); - } - -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - PX4_INFO_RAW("\n"); - PX4_INFO_RAW("Airspeed status:\n"); - _airspeed_validator.print(); -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - - if (_vehicle_optical_flow) { - PX4_INFO_RAW("\n"); - _vehicle_optical_flow->PrintStatus(); - } - -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - PX4_INFO_RAW("\n"); - _vehicle_acceleration.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - PX4_INFO_RAW("\n"); - _vehicle_angular_velocity.PrintStatus(); -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - - if (_vehicle_gps_position) { - PX4_INFO_RAW("\n"); - _vehicle_gps_position->PrintStatus(); - } - -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - - PX4_INFO_RAW("\n"); - - for (auto &i : _vehicle_imu_list) { - if (i != nullptr) { - PX4_INFO_RAW("\n"); - i->PrintStatus(); - } - } - - return 0; -} - -int Sensors::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int Sensors::print_usage(const char *reason) -{ - if (reason) { - PX4_WARN("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -The sensors module is central to the whole system. It takes low-level output from drivers, turns -it into a more usable form, and publishes it for the rest of the system. - -The provided functionality includes: -- Read the output from the sensor drivers (`SensorGyro`, etc.). - If there are multiple of the same type, do voting and failover handling. - Then apply the board rotation and temperature calibration (if enabled). And finally publish the data; one of the - topics is `SensorCombined`, used by many parts of the system. -- Make sure the sensor drivers get the updated calibration parameters (scale & offset) when the parameters change or - on startup. The sensor drivers use the ioctl interface for parameter updates. For this to work properly, the - sensor drivers must already be running when `sensors` is started. -- Do sensor consistency checks and publish the `SensorsStatusImu` topic. - -### Implementation -It runs in its own thread and polls on the currently selected gyro topic. - -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("sensors", "system"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - - return 0; -} - -extern "C" __EXPORT int sensors_main(int argc, char *argv[]) -{ - return Sensors::main(argc, argv); -} diff --git a/src/modules/sensors/sensors/sensors.hpp b/src/modules/sensors/sensors/sensors.hpp deleted file mode 100644 index 102be087b141..000000000000 --- a/src/modules/sensors/sensors/sensors.hpp +++ /dev/null @@ -1,266 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 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. - * - ****************************************************************************/ - -/** - * @file sensors.hpp - * - * @author Lorenz Meier - * @author Julian Oes - * @author Thomas Gubler - * @author Anton Babushkin - * @author Beat Küng - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "voted_sensors_update.h" -#include "vehicle_imu/VehicleIMU.hpp" - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) -# include "vehicle_acceleration/VehicleAcceleration.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) -# include -# include -# include -# include -# include -# include -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) -# include -# include "vehicle_air_data/VehicleAirData.hpp" -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) -# include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) -# include "vehicle_gps_position/VehicleGPSPosition.hpp" -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) -# include "vehicle_magnetometer/VehicleMagnetometer.hpp" -# include -# include -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) -# include "vehicle_optical_flow/VehicleOpticalFlow.hpp" -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - -using namespace sensors; -using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f -class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem -{ -public: - explicit Sensors(bool hil_enabled); - ~Sensors() override; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - /** @see ModuleBase::run() */ - void Run() override; - - /** @see ModuleBase::print_status() */ - int print_status() override; - - bool init(); - -private: - - int parameters_update(); - - void InitializeVehicleAirData(); - - void InitializeVehicleGPSPosition(); - - void InitializeVehicleIMU(); - - void InitializeVehicleMagnetometer(); - - void InitializeVehicleOpticalFlow(); - - const bool _hil_enabled; /**< if true, HIL is active */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; - - VotedSensorsUpdate _voted_sensors_update; - - sensor_combined_s _sensor_combined{}; - - hrt_abstime _last_config_update{0}; - hrt_abstime _sensor_combined_prev_timestamp{0}; - - uint8_t _n_accel{0}; - uint8_t _n_gyro{0}; - - bool _armed{false}; /**< arming status of the vehicle */ - - uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(vehicle_imu), 0}, - {this, ORB_ID(vehicle_imu), 1}, - {this, ORB_ID(vehicle_imu), 2}, - {this, ORB_ID(vehicle_imu), 3} - }; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; - - uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; - -#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /** - * Poll the differential pressure sensor for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void diff_pres_poll(); - - /** - * Poll the ADC and update readings to suit. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void adc_poll(); - - uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; - - DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ - - float _diff_pres_pressure_sum{0.f}; - float _diff_pres_temperature_sum{0.f}; - float _baro_pressure_sum{0.f}; - - int _diff_pres_count{0}; - - uint64_t _airspeed_last_publish{0}; - uint64_t _diff_pres_timestamp_sum{0}; - -# ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; - uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; -# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL - - struct Parameters { - float diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - float diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - int32_t air_cmodel; - float air_tube_length; - float air_tube_diameter_mm; - } _parameters{}; /**< local copies of interesting parameters */ - - struct ParameterHandles { - param_t diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_t diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - param_t air_cmodel; - param_t air_tube_length; - param_t air_tube_diameter_mm; - } _parameter_handles{}; /**< handles for interesting parameters */ -#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED - -#if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) - VehicleAcceleration _vehicle_acceleration; -#endif // CONFIG_SENSORS_VEHICLE_ACCELERATION - -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - VehicleAirData *_vehicle_air_data {nullptr}; - uint8_t _n_baro{0}; -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA - -#if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) - VehicleAngularVelocity _vehicle_angular_velocity; -#endif // CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY - -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - VehicleMagnetometer *_vehicle_magnetometer {nullptr}; - uint8_t _n_mag{0}; -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - VehicleGPSPosition *_vehicle_gps_position {nullptr}; - uint8_t _n_gps{0}; -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION - -#if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) - VehicleOpticalFlow *_vehicle_optical_flow {nullptr}; - uint8_t _n_optical_flow{0}; -#endif // CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW - - DEFINE_PARAMETERS( -#if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) - (ParamBool) _param_sys_has_baro, -#endif // CONFIG_SENSORS_VEHICLE_AIR_DATA -#if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) - (ParamBool) _param_sys_has_gps, -#endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION -#if defined(CONFIG_SENSORS_VEHICLE_MAGNETOMETER) - (ParamInt) _param_sys_has_mag, -#endif // CONFIG_SENSORS_VEHICLE_MAGNETOMETER - (ParamBool) _param_sens_imu_mode - ) -}; \ No newline at end of file diff --git a/src/modules/sensors/sensors/vehicle_acceleration/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_acceleration/CMakeLists.txt deleted file mode 100644 index 37f3558992b6..000000000000 --- a/src/modules/sensors/sensors/vehicle_acceleration/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# 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_acceleration - VehicleAcceleration.cpp - VehicleAcceleration.hpp -) - -target_compile_options(vehicle_acceleration PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) - -target_link_libraries(vehicle_acceleration - PRIVATE - mathlib - px4_work_queue - sensor_calibration -) diff --git a/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.cpp deleted file mode 100644 index 05373e0e22d4..000000000000 --- a/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2022 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 "VehicleAcceleration.hpp" - -#include - -#include - -using namespace matrix; - -namespace sensors -{ - -VehicleAcceleration::VehicleAcceleration() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - _vehicle_acceleration_pub.advertise(); - - CheckAndUpdateFilters(); -} - -VehicleAcceleration::~VehicleAcceleration() -{ - Stop(); -} - -bool VehicleAcceleration::Start() -{ - // force initial updates - ParametersUpdate(true); - - // sensor_selection needed to change the active sensor if the primary stops updating - if (!_sensor_selection_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - if (!SensorSelectionUpdate(true)) { - _sensor_sub.registerCallback(); - } - - return true; -} - -void VehicleAcceleration::Stop() -{ - // clear all registered callbacks - _sensor_sub.unregisterCallback(); - _sensor_selection_sub.unregisterCallback(); - - Deinit(); -} - -void VehicleAcceleration::CheckAndUpdateFilters() -{ - bool sample_rate_changed = false; - - // get sample rate from vehicle_imu_status publication - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - const float sample_rate_hz = imu_status.get().accel_rate_hz; - - if (imu_status.advertised() && (imu_status.get().timestamp != 0) - && (imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id()) - && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) { - - // check if sample rate error is greater than 1% - if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) { - PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz); - _filter_sample_rate = sample_rate_hz; - sample_rate_changed = true; - - // determine number of sensor samples that will get closest to the desired rate - if (_param_imu_integ_rate.get() > 0) { - const float configured_interval_us = 1e6f / _param_imu_integ_rate.get(); - const float sample_interval_avg = 1e6f / sample_rate_hz; - const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f, - (float)sensor_accel_s::ORB_QUEUE_LENGTH); - - _sensor_sub.set_required_updates(samples); - - } else { - _sensor_sub.set_required_updates(1); - } - - break; - } - } - } - - // update software low pass filters - if (sample_rate_changed || (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.1f)) { - _lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get()); - _lp_filter.reset(_acceleration_prev); - } -} - -void VehicleAcceleration::SensorBiasUpdate(bool force) -{ - // find corresponding estimated sensor bias - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; - - if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { - _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); - } - } - - if (_estimator_sensor_bias_sub.updated() || force) { - estimator_sensor_bias_s bias; - - if (_estimator_sensor_bias_sub.copy(&bias)) { - if (bias.accel_device_id == _calibration.device_id()) { - _bias = Vector3f{bias.accel_bias}; - - } else { - _bias.zero(); - } - } - } -} - -bool VehicleAcceleration::SensorSelectionUpdate(bool force) -{ - if (_sensor_selection_sub.updated() || (_calibration.device_id() == 0) || force) { - sensor_selection_s sensor_selection{}; - _sensor_selection_sub.copy(&sensor_selection); - - if ((sensor_selection.accel_device_id != 0) && (_calibration.device_id() != sensor_selection.accel_device_id)) { - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_accel_sub{ORB_ID(sensor_accel), i}; - - const uint32_t device_id = sensor_accel_sub.get().device_id; - - if (sensor_accel_sub.advertised() && (sensor_accel_sub.get().timestamp != 0) - && (device_id != 0) && (device_id == sensor_selection.accel_device_id)) { - - if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - PX4_DEBUG("selected sensor changed %" PRIu32 " -> %" PRIu32 "", _calibration.device_id(), device_id); - - // clear bias and corrections - _bias.zero(); - - _calibration.set_device_id(device_id); - - CheckAndUpdateFilters(); - - return true; - } - } - } - - PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", sensor_selection.accel_device_id); - _calibration.set_device_id(0); - } - } - - return false; -} - -void VehicleAcceleration::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - - _calibration.ParametersUpdate(); - - CheckAndUpdateFilters(); - } -} - -void VehicleAcceleration::Run() -{ - // backup schedule - ScheduleDelayed(10_ms); - - // update corrections first to set _selected_sensor - bool selection_updated = SensorSelectionUpdate(); - - ParametersUpdate(); - - _calibration.SensorCorrectionsUpdate(selection_updated); - - SensorBiasUpdate(selection_updated); - - // require valid sensor sample rate to run - if (!PX4_ISFINITE(_filter_sample_rate)) { - CheckAndUpdateFilters(); - - if (!PX4_ISFINITE(_filter_sample_rate)) { - return; - } - } - - // process all outstanding messages - sensor_accel_s sensor_data; - - while (_sensor_sub.update(&sensor_data)) { - const Vector3f accel_raw{sensor_data.x, sensor_data.y, sensor_data.z}; - - if (accel_raw.isAllFinite()) { - // Apply calibration and filter - // - calibration offsets, scale factors, and thermal scale (if available) - // - estimated in run bias (if available) - // - biquad low-pass filter - const Vector3f accel_corrected = _calibration.Correct(accel_raw) - _bias; - const Vector3f accel_filtered = _lp_filter.apply(accel_corrected); - - _acceleration_prev = accel_corrected; - - // publish once all new samples are processed - if (!_sensor_sub.updated()) { - // Publish vehicle_acceleration - vehicle_acceleration_s v_acceleration; - v_acceleration.timestamp_sample = sensor_data.timestamp_sample; - accel_filtered.copyTo(v_acceleration.xyz); - v_acceleration.timestamp = hrt_absolute_time(); - _vehicle_acceleration_pub.publish(v_acceleration); - - return; - } - } - } -} - -void VehicleAcceleration::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_acceleration] selected sensor: %" PRIu32 ", rate: %.1f Hz, estimated bias: [%.4f %.4f %.4f]\n", - _calibration.device_id(), (double)_filter_sample_rate, - (double)_bias(0), (double)_bias(1), (double)_bias(2)); - - _calibration.PrintStatus(); -} - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.hpp deleted file mode 100644 index 68c065208230..000000000000 --- a/src/modules/sensors/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * 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 -#include -#include -#include - -using namespace time_literals; - -namespace sensors -{ - -class VehicleAcceleration : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - VehicleAcceleration(); - ~VehicleAcceleration() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - void Run() override; - - void CheckAndUpdateFilters(); - void ParametersUpdate(bool force = false); - void SensorBiasUpdate(bool force = false); - bool SensorSelectionUpdate(bool force = false); - - static constexpr int MAX_SENSOR_COUNT = 4; - - uORB::Publication _vehicle_acceleration_pub{ORB_ID(vehicle_acceleration)}; - - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_accel)}; - - calibration::Accelerometer _calibration{}; - - matrix::Vector3f _bias{}; - - matrix::Vector3f _acceleration_prev{}; - - float _filter_sample_rate{NAN}; - - math::LowPassFilter2p _lp_filter{}; - - DEFINE_PARAMETERS( - (ParamFloat) _param_imu_accel_cutoff, - (ParamInt) _param_imu_integ_rate - ) -}; - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_acceleration/imu_accel_parameters.c b/src/modules/sensors/sensors/vehicle_acceleration/imu_accel_parameters.c deleted file mode 100644 index c4fda14b94e5..000000000000 --- a/src/modules/sensors/sensors/vehicle_acceleration/imu_accel_parameters.c +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -/** -* Low pass filter cutoff frequency for accel -* -* The cutoff frequency for the 2nd order butterworth filter on the primary accelerometer. -* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_ACCEL_CUTOFF, 30.0f); diff --git a/src/modules/sensors/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_air_data/CMakeLists.txt deleted file mode 100644 index 909f249a7c01..000000000000 --- a/src/modules/sensors/sensors/vehicle_air_data/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020-2022 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_air_data - VehicleAirData.cpp - VehicleAirData.hpp -) -target_link_libraries(vehicle_air_data - PRIVATE - data_validator - px4_work_queue - sensor_calibration - PUBLIC - atmosphere -) diff --git a/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.cpp deleted file mode 100644 index 10f9bd4ad886..000000000000 --- a/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.cpp +++ /dev/null @@ -1,469 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "VehicleAirData.hpp" - -#include -#include -#include -#include - -/* #include */ -/* #include */ - -namespace sensors -{ - -using namespace matrix; -using namespace atmosphere; - -static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; - -VehicleAirData::VehicleAirData() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - _vehicle_air_data_pub.advertise(); - - _voter.set_timeout(SENSOR_TIMEOUT); - /* baro_drift_timestep = 0; */ -} - -VehicleAirData::~VehicleAirData() -{ - Stop(); - perf_free(_cycle_perf); -} - -bool VehicleAirData::Start() -{ - ScheduleNow(); - return true; -} - -void VehicleAirData::Stop() -{ - Deinit(); - - // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } -} - -void VehicleAirData::AirTemperatureUpdate() -{ - differential_pressure_s differential_pressure; - - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); - } -} - -bool VehicleAirData::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - - // update priority - for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) { - - const int32_t priority_old = _calibration[instance].priority(); - - _calibration[instance].ParametersUpdate(); - - const int32_t priority_new = _calibration[instance].priority(); - - if (priority_old != priority_new) { - if (_priority[instance] == priority_old) { - _priority[instance] = priority_new; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = priority_new - priority_old; - _priority[instance] = math::constrain(_priority[instance] + priority_change, 1, 100); - } - } - } - - return true; - } - - return false; -} - -void VehicleAirData::Run() -{ - perf_begin(_cycle_perf); - - const hrt_abstime time_now_us = hrt_absolute_time(); - - const bool parameter_update = ParametersUpdate(); - - AirTemperatureUpdate(); - - bool updated[MAX_SENSOR_COUNT] {}; - - for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - - const bool was_advertised = _advertised[uorb_index]; - - if (!_advertised[uorb_index]) { - // use data's timestamp to throttle advertisement checks - if ((_last_publication_timestamp[uorb_index] == 0) - || (time_now_us > _last_publication_timestamp[uorb_index] + 1_s)) { - - if (_sensor_sub[uorb_index].advertised()) { - _advertised[uorb_index] = true; - - } else { - _last_publication_timestamp[uorb_index] = time_now_us; - } - } - } - - if (_advertised[uorb_index]) { - sensor_baro_s report; - - while (_sensor_sub[uorb_index].update(&report)) { - - if (_calibration[uorb_index].device_id() != report.device_id) { - _calibration[uorb_index].set_device_id(report.device_id); - _priority[uorb_index] = _calibration[uorb_index].priority(); - } - - if (_calibration[uorb_index].enabled()) { - - if (!was_advertised) { - if (uorb_index > 0) { - /* the first always exists, but for each further sensor, add a new validator */ - if (!_voter.add_new_validator()) { - PX4_ERR("failed to add validator for %s %i", _calibration[uorb_index].SensorString(), uorb_index); - } - } - - if (_selected_sensor_sub_index < 0) { - _sensor_sub[uorb_index].registerCallback(); - } - - ParametersUpdate(true); - } - - // pressure corrected with offset (if available) - _calibration[uorb_index].SensorCorrectionsUpdate(); - const float pressure_corrected = _calibration[uorb_index].Correct(report.pressure); - const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - - float data_array[3] {pressure_corrected, report.temperature, getAltitudeFromPressure(pressure_corrected, pressure_sealevel_pa)}; - _voter.put(uorb_index, report.timestamp, data_array, report.error_count, _priority[uorb_index]); - - _timestamp_sample_sum[uorb_index] += report.timestamp_sample; - _data_sum[uorb_index] += pressure_corrected; - _temperature_sum[uorb_index] += report.temperature; - _data_sum_count[uorb_index]++; - - _last_data[uorb_index] = pressure_corrected; - - updated[uorb_index] = true; - } - } - } - } - - // check for the current best sensor - int best_index = 0; - _voter.get_best(time_now_us, &best_index); - - if (best_index >= 0) { - // handle selection change (don't process on same iteration as parameter update) - if ((_selected_sensor_sub_index != best_index) && !parameter_update) { - // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } - - if (_selected_sensor_sub_index >= 0) { - PX4_INFO("%s switch from #%" PRId8 " -> #%d", _calibration[_selected_sensor_sub_index].SensorString(), - _selected_sensor_sub_index, best_index); - } - - _selected_sensor_sub_index = best_index; - _sensor_sub[_selected_sensor_sub_index].registerCallback(); - } - } - - // Publish - if (_param_sens_baro_rate.get() > 0) { - int interval_us = 1e6f / _param_sens_baro_rate.get(); - - for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) { - if (updated[instance] && (_data_sum_count[instance] > 0)) { - - const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance]; - - if (timestamp_sample >= _last_publication_timestamp[instance] + interval_us) { - - bool publish = (time_now_us <= timestamp_sample + 1_s); - - if (publish) { - publish = (_selected_sensor_sub_index >= 0) - && (instance == _selected_sensor_sub_index) - && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR); - } - - if (publish) { - const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; - - const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; - const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); - - // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); - - // populate vehicle_air_data with and publish - vehicle_air_data_s out{}; - out.timestamp_sample = timestamp_sample; - out.baro_device_id = _calibration[instance].device_id(); - out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; - out.baro_pressure_pa = pressure_pa; - - /* // Adding faults to the barometer */ - /* param_t baro_fault = param_find("SENS_BARO_FAULT"); */ - /* int32_t baro_fault_flag; */ - /* param_get(baro_fault, &baro_fault_flag); */ - /**/ - /* if (baro_fault_flag == 1) */ - /* { */ - /* param_t baro_noise = param_find("SENS_BARO_NOISE"); */ - /* float_t baro_noise_flag; */ - /* param_get(baro_noise, &baro_noise_flag); */ - /**/ - /* if (abs(baro_noise_flag) > 0) */ - /* { */ - /* unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); */ - /* std::default_random_engine generator (seed); */ - /**/ - /* std::normal_distribution distribution (0.0,baro_noise_flag); */ - /* float dev = distribution(generator); */ - /* out.baro_pressure_pa += out.baro_pressure_pa*dev; */ - /* } */ - /**/ - /* param_t baro_bias_shift = param_find("SENS_BARO_SHIF"); */ - /* float_t baro_bias_shift_flag; */ - /* param_get(baro_bias_shift, &baro_bias_shift_flag); */ - /**/ - /* if (abs(baro_bias_shift_flag) > 0) */ - /* { */ - /* out.baro_pressure_pa += out.baro_pressure_pa*baro_bias_shift_flag; */ - /* } */ - /**/ - /* param_t baro_bias_scale = param_find("SENS_BARO_SCAL"); */ - /* float_t baro_bias_scale_flag; */ - /* param_get(baro_bias_scale, &baro_bias_scale_flag); */ - /**/ - /* if (abs(baro_bias_scale_flag) > 0) */ - /* { */ - /* out.baro_pressure_pa *= baro_bias_scale_flag; */ - /* } */ - /**/ - /* param_t baro_drift = param_find("SENS_BARO_DRIFT"); */ - /* float_t baro_drift_flag; */ - /* param_get(baro_drift, &baro_drift_flag); */ - /**/ - /* if (abs(baro_drift_flag) > 0) */ - /* { */ - /* out.baro_pressure_pa += 0.01f*baro_drift_flag*baro_drift_timestep/1000000; */ - /**/ - /* baro_drift_timestep += 1; */ - /* } */ - /* } */ - - out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); - out.calibration_count = _calibration[instance].calibration_count(); - out.timestamp = hrt_absolute_time(); - - _vehicle_air_data_pub.publish(out); - } - - _last_publication_timestamp[instance] = timestamp_sample; - - // reset - _timestamp_sample_sum[instance] = 0; - _data_sum[instance] = 0; - _temperature_sum[instance] = 0; - _data_sum_count[instance] = 0; - } - } - } - } - - if (!parameter_update) { - CheckFailover(time_now_us); - } - - UpdateStatus(); - - // reschedule timeout - ScheduleDelayed(50_ms); - - perf_end(_cycle_perf); -} - -void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) -{ - // check failover and report (save failover report for a cycle where parameters didn't update) - if (_last_failover_count != _voter.failover_count()) { - uint32_t flags = _voter.failover_state(); - int failover_index = _voter.failover_index(); - - if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { - if (failover_index >= 0 && failover_index < MAX_SENSOR_COUNT) { - - if (time_now_us > _last_error_message + 3_s) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t", - _calibration[failover_index].SensorString(), - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); - - events::px4::enums::sensor_failover_reason_t failover_reason{}; - - if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } - - if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } - - if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } - - /* EVENT - * @description - * Land immediately and check the system. - */ - events::send( - events::ID("sensor_failover_baro"), events::Log::Emergency, "Baro sensor #{1} failure: {2}", failover_index, - failover_reason); - - _last_error_message = time_now_us; - } - - // reduce priority of failed sensor to the minimum - _priority[failover_index] = 1; - } - } - - _last_failover_count = _voter.failover_count(); - } -} - -void VehicleAirData::UpdateStatus() -{ - if (_selected_sensor_sub_index >= 0) { - sensors_status_s sensors_status{}; - sensors_status.device_id_primary = _calibration[_selected_sensor_sub_index].device_id(); - - float mean{}; - int sensor_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_calibration[sensor_index].device_id() != 0) && (_calibration[sensor_index].enabled())) { - sensor_count++; - mean += _last_data[sensor_index]; - } - } - - if (sensor_count > 0) { - mean /= sensor_count; - } - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if (_calibration[sensor_index].device_id() != 0) { - - _sensor_diff[sensor_index] = 0.95f * _sensor_diff[sensor_index] + 0.05f * (_last_data[sensor_index] - mean); - - sensors_status.device_ids[sensor_index] = _calibration[sensor_index].device_id(); - sensors_status.inconsistency[sensor_index] = _sensor_diff[sensor_index]; - sensors_status.healthy[sensor_index] = (_voter.get_sensor_state(sensor_index) == DataValidator::ERROR_FLAG_NO_ERROR); - sensors_status.priority[sensor_index] = _voter.get_sensor_priority(sensor_index); - sensors_status.enabled[sensor_index] = _calibration[sensor_index].enabled(); - sensors_status.external[sensor_index] = _calibration[sensor_index].external(); - - } else { - sensors_status.inconsistency[sensor_index] = NAN; - } - } - - sensors_status.timestamp = hrt_absolute_time(); - _sensors_status_baro_pub.publish(sensors_status); - } -} - -void VehicleAirData::PrintStatus() -{ - if (_selected_sensor_sub_index >= 0) { - PX4_INFO_RAW("[vehicle_air_data] selected %s: %" PRIu32 " (%" PRId8 ")\n", - _calibration[_selected_sensor_sub_index].SensorString(), - _calibration[_selected_sensor_sub_index].device_id(), _selected_sensor_sub_index); - } - - _voter.print(); - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if (_advertised[i] && (_priority[i] > 0)) { - _calibration[i].PrintStatus(); - } - } -} - -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.hpp deleted file mode 100644 index 1de509105ee9..000000000000 --- a/src/modules/sensors/sensors/vehicle_air_data/VehicleAirData.hpp +++ /dev/null @@ -1,133 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "data_validator/DataValidatorGroup.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -namespace sensors -{ -class VehicleAirData : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - - VehicleAirData(); - ~VehicleAirData() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - void Run() override; - - void AirTemperatureUpdate(); - void CheckFailover(const hrt_abstime &time_now_us); - bool ParametersUpdate(bool force = false); - void UpdateStatus(); - - static constexpr int MAX_SENSOR_COUNT = 4; - - uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)}; - - uORB::Publication _vehicle_air_data_pub{ORB_ID(vehicle_air_data)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; - - uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(sensor_baro), 0}, - {this, ORB_ID(sensor_baro), 1}, - {this, ORB_ID(sensor_baro), 2}, - {this, ORB_ID(sensor_baro), 3}, - }; - - calibration::Barometer _calibration[MAX_SENSOR_COUNT]; - - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - hrt_abstime _last_error_message{0}; - orb_advert_t _mavlink_log_pub{nullptr}; - - DataValidatorGroup _voter{1}; - unsigned _last_failover_count{0}; - - uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0}; - float _data_sum[MAX_SENSOR_COUNT] {}; - float _temperature_sum[MAX_SENSOR_COUNT] {}; - int _data_sum_count[MAX_SENSOR_COUNT] {}; - hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; - - float _last_data[MAX_SENSOR_COUNT] {}; - bool _advertised[MAX_SENSOR_COUNT] {}; - - float _sensor_diff[MAX_SENSOR_COUNT] {}; // filtered differences between sensor instances - - uint8_t _priority[MAX_SENSOR_COUNT] {}; - - int8_t _selected_sensor_sub_index{-1}; - - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - - /* int baro_drift_timestep; */ - - DEFINE_PARAMETERS( - (ParamFloat) _param_sens_baro_qnh, - (ParamFloat) _param_sens_baro_rate - ) -}; -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_air_data/params.c b/src/modules/sensors/sensors/vehicle_air_data/params.c deleted file mode 100644 index fee8ac6263fc..000000000000 --- a/src/modules/sensors/sensors/vehicle_air_data/params.c +++ /dev/null @@ -1,55 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2022 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. - * - ****************************************************************************/ - -/** - * QNH for barometer - * - * @min 500 - * @max 1500 - * @group Sensors - * @unit hPa - */ -PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); - -/** - * Baro max rate. - * - * Barometric air data maximum publication rate. This is an upper bound, - * actual barometric data rate is still dependent on the sensor. - * - * @min 1 - * @max 200 - * @group Sensors - * @unit Hz - */ -PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f); diff --git a/src/modules/sensors/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_angular_velocity/CMakeLists.txt deleted file mode 100644 index 8ab38d47bd68..000000000000 --- a/src/modules/sensors/sensors/vehicle_angular_velocity/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -############################################################################ -# -# 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_velocity - VehicleAngularVelocity.cpp - VehicleAngularVelocity.hpp -) - -target_compile_options(vehicle_angular_velocity - PRIVATE - ${MAX_CUSTOM_OPT_LEVEL} - #-DDEBUG_BUILD -) - -target_link_libraries(vehicle_angular_velocity - PRIVATE - mathlib - px4_work_queue - sensor_calibration -) diff --git a/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp deleted file mode 100644 index 0264c930de54..000000000000 --- a/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ /dev/null @@ -1,968 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2022 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 - -#include - -using namespace matrix; - -namespace sensors -{ - -VehicleAngularVelocity::VehicleAngularVelocity() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) -{ - _vehicle_angular_velocity_pub.advertise(); -} - -VehicleAngularVelocity::~VehicleAngularVelocity() -{ - Stop(); - - perf_free(_cycle_perf); - perf_free(_filter_reset_perf); - perf_free(_selection_changed_perf); - -#if !defined(CONSTRAINED_FLASH) - delete[] _dynamic_notch_filter_esc_rpm; - perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_free(_dynamic_notch_filter_esc_rpm_init_perf); - perf_free(_dynamic_notch_filter_esc_rpm_update_perf); - - perf_free(_dynamic_notch_filter_fft_disable_perf); - perf_free(_dynamic_notch_filter_fft_update_perf); -#endif // CONSTRAINED_FLASH -} - -bool VehicleAngularVelocity::Start() -{ - // force initial updates - ParametersUpdate(true); - - // sensor_selection needed to change the active sensor if the primary stops updating - if (!_sensor_selection_sub.registerCallback()) { - PX4_ERR("callback registration failed"); - return false; - } - - if (!SensorSelectionUpdate(true)) { - ScheduleNow(); - } - - return true; -} - -void VehicleAngularVelocity::Stop() -{ - // clear all registered callbacks - _sensor_sub.unregisterCallback(); - _sensor_gyro_fifo_sub.unregisterCallback(); - _sensor_selection_sub.unregisterCallback(); - - Deinit(); -} - -bool VehicleAngularVelocity::UpdateSampleRate() -{ - float sample_rate_hz = NAN; - float publish_rate_hz = NAN; - - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - if (imu_status.get().gyro_device_id == _selected_sensor_device_id) { - sample_rate_hz = imu_status.get().gyro_raw_rate_hz; - publish_rate_hz = imu_status.get().gyro_rate_hz; - break; - } - } - - // calculate sensor update rate - if (PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 10) && (sample_rate_hz < 10'000) - && PX4_ISFINITE(publish_rate_hz) && (publish_rate_hz > 0) - ) { - // check if sample rate error is greater than 1% - const bool sample_rate_changed = (fabsf(sample_rate_hz - _filter_sample_rate_hz) / sample_rate_hz) > 0.01f; - - if (_update_sample_rate || sample_rate_changed - || (_filter_sample_rate_hz <= FLT_EPSILON) || !PX4_ISFINITE(_filter_sample_rate_hz)) { - - PX4_DEBUG("updating sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate_hz, (double)sample_rate_hz); - - if (sample_rate_changed || !PX4_ISFINITE(_filter_sample_rate_hz)) { - _reset_filters = true; - } - - _filter_sample_rate_hz = sample_rate_hz; - _update_sample_rate = false; - - if (_param_imu_gyro_ratemax.get() > 0.f) { - // determine number of sensor samples that will get closest to the desired rate - const float configured_interval_us = 1e6f / _param_imu_gyro_ratemax.get(); - const float publish_interval_us = 1e6f / publish_rate_hz; - - const uint8_t samples = roundf(configured_interval_us / publish_interval_us); - - if (_fifo_available) { - _sensor_gyro_fifo_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_fifo_s::ORB_QUEUE_LENGTH)); - - } else { - _sensor_sub.set_required_updates(math::constrain(samples, (uint8_t)1, sensor_gyro_s::ORB_QUEUE_LENGTH)); - } - - // publish interval (constrained 100 Hz - 8 kHz) - _publish_interval_min_us = math::constrain((int)roundf(configured_interval_us - (publish_interval_us * 0.5f)), 125, - 10000); - - } else { - _sensor_sub.set_required_updates(1); - _sensor_gyro_fifo_sub.set_required_updates(1); - _publish_interval_min_us = 0; - } - } - } - - return PX4_ISFINITE(_filter_sample_rate_hz) && (_filter_sample_rate_hz > 0); -} - -void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us) -{ - if ((_filter_sample_rate_hz > 0) && PX4_ISFINITE(_filter_sample_rate_hz)) { - - const Vector3f angular_velocity_uncalibrated{GetResetAngularVelocity()}; - const Vector3f angular_acceleration_uncalibrated{GetResetAngularAcceleration()}; - - for (int axis = 0; axis < 3; axis++) { - // angular velocity low pass - _lp_filter_velocity[axis].set_cutoff_frequency(_filter_sample_rate_hz, _param_imu_gyro_cutoff.get()); - _lp_filter_velocity[axis].reset(angular_velocity_uncalibrated(axis)); - - // angular velocity notch 0 - _notch_filter0_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf0_frq.get(), - _param_imu_gyro_nf0_bw.get()); - _notch_filter0_velocity[axis].reset(); - - // angular velocity notch 1 - _notch_filter1_velocity[axis].setParameters(_filter_sample_rate_hz, _param_imu_gyro_nf1_frq.get(), - _param_imu_gyro_nf1_bw.get()); - _notch_filter1_velocity[axis].reset(); - - // angular acceleration low pass - if ((_param_imu_dgyro_cutoff.get() > 0.f) - && (_lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) { - _lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis)); - - } else { - // disable filtering - _lp_filter_acceleration[axis].setAlpha(1.f); - } - } - - // force reset notch filters on any scale change - UpdateDynamicNotchEscRpm(time_now_us, true); - UpdateDynamicNotchFFT(time_now_us, true); - - _angular_velocity_raw_prev = angular_velocity_uncalibrated; - - _reset_filters = false; - perf_count(_filter_reset_perf); - } -} - -void VehicleAngularVelocity::SensorBiasUpdate(bool force) -{ - // find corresponding estimated sensor bias - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; - - if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { - _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance); - } - } - - if (_estimator_sensor_bias_sub.updated() || force) { - estimator_sensor_bias_s bias; - - if (_estimator_sensor_bias_sub.copy(&bias) && (bias.gyro_device_id == _selected_sensor_device_id)) { - _bias = Vector3f{bias.gyro_bias}; - - } else { - _bias.zero(); - } - } -} - -bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force) -{ - if (_sensor_selection_sub.updated() || (_selected_sensor_device_id == 0) || force) { - sensor_selection_s sensor_selection{}; - _sensor_selection_sub.copy(&sensor_selection); - - bool selected_device_id_valid = false; - uint32_t device_id = sensor_selection.gyro_device_id; - uint32_t device_id_first_valid_imu = 0; - - // use vehicle_imu_status to do basic sensor selection validation - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData imu_status{ORB_ID(vehicle_imu_status), i}; - - if (imu_status.advertised() - && (imu_status.get().timestamp != 0) && (time_now_us < imu_status.get().timestamp + 1_s) - && (imu_status.get().gyro_device_id != 0)) { - // vehicle_imu_status gyro valid - - if ((device_id != 0) && (imu_status.get().gyro_device_id == device_id)) { - selected_device_id_valid = true; - } - - // record first valid IMU as a backup option - if (device_id_first_valid_imu == 0) { - device_id_first_valid_imu = imu_status.get().gyro_device_id; - } - } - } - - // if no gyro selected or healthy then use fallback - if ((device_id == 0) || !selected_device_id_valid) { - device_id = device_id_first_valid_imu; - } - - if ((_selected_sensor_device_id != device_id) || force) { - - const bool device_id_valid = (device_id != 0); - - // see if the selected sensor publishes sensor_gyro_fifo - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_fifo_sub{ORB_ID(sensor_gyro_fifo), i}; - - if (sensor_gyro_fifo_sub.advertised() - && (sensor_gyro_fifo_sub.get().timestamp != 0) - && (sensor_gyro_fifo_sub.get().device_id != 0) - && (time_now_us < sensor_gyro_fifo_sub.get().timestamp + 1_s)) { - - // if no gyro was selected use the first valid sensor_gyro_fifo - if (!device_id_valid) { - device_id = sensor_gyro_fifo_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id); - } - - if (sensor_gyro_fifo_sub.get().device_id == device_id) { - if (_sensor_gyro_fifo_sub.ChangeInstance(i) && _sensor_gyro_fifo_sub.registerCallback()) { - // make sure non-FIFO sub is unregistered - _sensor_sub.unregisterCallback(); - - _calibration.set_device_id(sensor_gyro_fifo_sub.get().device_id); - - _selected_sensor_device_id = sensor_gyro_fifo_sub.get().device_id; - - _timestamp_sample_last = 0; - _filter_sample_rate_hz = 1.f / (sensor_gyro_fifo_sub.get().dt * 1e-6f); - _update_sample_rate = true; - _reset_filters = true; - _bias.zero(); - _fifo_available = true; - - perf_count(_selection_changed_perf); - PX4_DEBUG("selecting sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id); - return true; - - } else { - PX4_ERR("unable to register callback for sensor_gyro_fifo:%" PRIu8 " %" PRIu32, - i, sensor_gyro_fifo_sub.get().device_id); - } - } - } - } - - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - - if (sensor_gyro_sub.advertised() - && (sensor_gyro_sub.get().timestamp != 0) - && (sensor_gyro_sub.get().device_id != 0) - && (time_now_us < sensor_gyro_sub.get().timestamp + 1_s)) { - - // if no gyro was selected use the first valid sensor_gyro - if (!device_id_valid) { - device_id = sensor_gyro_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); - } - - if (sensor_gyro_sub.get().device_id == device_id) { - if (_sensor_sub.ChangeInstance(i) && _sensor_sub.registerCallback()) { - // make sure FIFO sub is unregistered - _sensor_gyro_fifo_sub.unregisterCallback(); - - _calibration.set_device_id(sensor_gyro_sub.get().device_id); - - _selected_sensor_device_id = sensor_gyro_sub.get().device_id; - - _timestamp_sample_last = 0; - _filter_sample_rate_hz = NAN; - _update_sample_rate = true; - _reset_filters = true; - _bias.zero(); - _fifo_available = false; - - perf_count(_selection_changed_perf); - PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, _selected_sensor_device_id); - return true; - - } else { - PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, - i, sensor_gyro_sub.get().device_id); - } - } - } - } - - if (device_id != 0) { - PX4_ERR("unable to find or subscribe to selected sensor (%" PRIu32 ")", device_id); - } - - _selected_sensor_device_id = 0; - } - } - - return false; -} - -void VehicleAngularVelocity::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - const bool nf0_enabled_prev = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); - const bool nf1_enabled_prev = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); - - updateParams(); - - const bool nf0_enabled = (_param_imu_gyro_nf0_frq.get() > 0.f) && (_param_imu_gyro_nf0_bw.get() > 0.f); - const bool nf1_enabled = (_param_imu_gyro_nf1_frq.get() > 0.f) && (_param_imu_gyro_nf1_bw.get() > 0.f); - - _calibration.ParametersUpdate(); - - // IMU_GYRO_RATEMAX - if (_param_imu_gyro_ratemax.get() <= 0) { - const int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get(); - _param_imu_gyro_ratemax.reset(); - PX4_WARN("IMU_GYRO_RATEMAX invalid (%" PRId32 "), resetting to default %" PRId32 ")", imu_gyro_ratemax, - _param_imu_gyro_ratemax.get()); - } - - // constrain IMU_GYRO_RATEMAX 50-10,000 Hz - const int32_t imu_gyro_ratemax = constrain(_param_imu_gyro_ratemax.get(), (int32_t)50, (int32_t)10'000); - - if (imu_gyro_ratemax != _param_imu_gyro_ratemax.get()) { - PX4_WARN("IMU_GYRO_RATEMAX updated %" PRId32 " -> %" PRIu32, _param_imu_gyro_ratemax.get(), imu_gyro_ratemax); - _param_imu_gyro_ratemax.set(imu_gyro_ratemax); - _param_imu_gyro_ratemax.commit_no_notification(); - } - - // gyro low pass cutoff frequency changed - for (auto &lp : _lp_filter_velocity) { - if (fabsf(lp.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f) { - _reset_filters = true; - break; - } - } - - // gyro notch filter 0 frequency or bandwidth changed - for (auto &nf : _notch_filter0_velocity) { - const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf0_frq.get()) > 0.01f); - const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf0_bw.get()) > 0.01f); - - if ((nf0_enabled_prev != nf0_enabled) || (nf0_enabled && (nf_freq_changed || nf_bw_changed))) { - _reset_filters = true; - break; - } - } - - // gyro notch filter 1 frequency or bandwidth changed - for (auto &nf : _notch_filter1_velocity) { - const bool nf_freq_changed = (fabsf(nf.getNotchFreq() - _param_imu_gyro_nf1_frq.get()) > 0.01f); - const bool nf_bw_changed = (fabsf(nf.getBandwidth() - _param_imu_gyro_nf1_bw.get()) > 0.01f); - - if ((nf1_enabled_prev != nf1_enabled) || (nf1_enabled && (nf_freq_changed || nf_bw_changed))) { - _reset_filters = true; - break; - } - } - - // gyro derivative low pass cutoff changed - for (auto &lp : _lp_filter_acceleration) { - if (fabsf(lp.getCutoffFreq() - _param_imu_dgyro_cutoff.get()) > 0.01f) { - _reset_filters = true; - break; - } - } - -#if !defined(CONSTRAINED_FLASH) - - if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) { - - const int32_t esc_rpm_harmonics = math::constrain(_param_imu_gyro_dnf_hmc.get(), (int32_t)1, (int32_t)10); - - if (_dynamic_notch_filter_esc_rpm && (esc_rpm_harmonics != _esc_rpm_harmonics)) { - delete[] _dynamic_notch_filter_esc_rpm; - _dynamic_notch_filter_esc_rpm = nullptr; - _esc_rpm_harmonics = 0; - } - - if (_dynamic_notch_filter_esc_rpm == nullptr) { - - _dynamic_notch_filter_esc_rpm = new NotchFilterHarmonic[esc_rpm_harmonics]; - - if (_dynamic_notch_filter_esc_rpm) { - _esc_rpm_harmonics = esc_rpm_harmonics; - - if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM disable"); - } - - if (_dynamic_notch_filter_esc_rpm_init_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_init_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM init"); - } - - if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) { - _dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT, - MODULE_NAME": gyro dynamic notch filter ESC RPM update"); - } - - } else { - _esc_rpm_harmonics = 0; - - perf_free(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_free(_dynamic_notch_filter_esc_rpm_init_perf); - perf_free(_dynamic_notch_filter_esc_rpm_update_perf); - - _dynamic_notch_filter_esc_rpm_disable_perf = nullptr; - _dynamic_notch_filter_esc_rpm_init_perf = nullptr; - _dynamic_notch_filter_esc_rpm_update_perf = nullptr; - } - } - - } else { - DisableDynamicNotchEscRpm(); - } - - if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) { - if (_dynamic_notch_filter_fft_disable_perf == nullptr) { - _dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable"); - _dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update"); - } - - } else { - DisableDynamicNotchFFT(); - } - -#endif // !CONSTRAINED_FLASH - } -} - -Vector3f VehicleAngularVelocity::GetResetAngularVelocity() const -{ - if (_last_publish != 0) { - // angular velocity filtering is performed on raw unscaled data - // start with last valid vehicle body frame angular velocity and compute equivalent raw data (for current sensor selection) - Vector3f angular_velocity_uncalibrated{_calibration.Uncorrect(_angular_velocity + _bias)}; - - if (angular_velocity_uncalibrated.isAllFinite()) { - return angular_velocity_uncalibrated; - } - } - - return Vector3f{0.f, 0.f, 0.f}; -} - -Vector3f VehicleAngularVelocity::GetResetAngularAcceleration() const -{ - if (_last_publish != 0) { - // angular acceleration filtering is performed on unscaled angular velocity data - // start with last valid vehicle body frame angular acceleration and compute equivalent raw data (for current sensor selection) - Vector3f angular_acceleration{_calibration.rotation().I() *_angular_acceleration}; - - if (angular_acceleration.isAllFinite()) { - return angular_acceleration; - } - } - - return Vector3f{0.f, 0.f, 0.f}; -} - -void VehicleAngularVelocity::DisableDynamicNotchEscRpm() -{ -#if !defined(CONSTRAINED_FLASH) - - if (_dynamic_notch_filter_esc_rpm) { - for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) { - for (int axis = 0; axis < 3; axis++) { - for (int esc = 0; esc < MAX_NUM_ESCS; esc++) { - _dynamic_notch_filter_esc_rpm[harmonic][axis][esc].disable(); - _esc_available.set(esc, false); - perf_count(_dynamic_notch_filter_esc_rpm_disable_perf); - } - } - } - } - -#endif // !CONSTRAINED_FLASH -} - -void VehicleAngularVelocity::DisableDynamicNotchFFT() -{ -#if !defined(CONSTRAINED_FLASH) - - if (_dynamic_notch_fft_available) { - for (int axis = 0; axis < 3; axis++) { - for (int peak = 0; peak < MAX_NUM_FFT_PEAKS; peak++) { - _dynamic_notch_filter_fft[axis][peak].disable(); - perf_count(_dynamic_notch_filter_fft_disable_perf); - } - } - - _dynamic_notch_fft_available = false; - } - -#endif // !CONSTRAINED_FLASH -} - -void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force) -{ -#if !defined(CONSTRAINED_FLASH) - const bool enabled = _dynamic_notch_filter_esc_rpm && (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm); - - if (enabled && (_esc_status_sub.updated() || force)) { - - bool axis_init[3] {false, false, false}; - - esc_status_s esc_status; - - if (_esc_status_sub.copy(&esc_status) && (time_now_us < esc_status.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { - - const float bandwidth_hz = _param_imu_gyro_dnf_bw.get(); - const float freq_min = math::max(_param_imu_gyro_dnf_min.get(), bandwidth_hz); - - for (size_t esc = 0; esc < math::min(esc_status.esc_count, (uint8_t)MAX_NUM_ESCS); esc++) { - const esc_report_s &esc_report = esc_status.esc[esc]; - - const bool esc_connected = (esc_status.esc_online_flags & (1 << esc)) || (esc_report.esc_rpm != 0); - - // only update if ESC RPM range seems valid - if (esc_connected && (time_now_us < esc_report.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { - - const float esc_hz = abs(esc_report.esc_rpm) / 60.f; - - const bool force_update = force || !_esc_available[esc]; // force parameter update or notch was previously disabled - - for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) { - // as RPM drops leave the notch filter "parked" at the minimum rather than disabling - // keep harmonics separated by half the notch filter bandwidth - const float frequency_hz = math::max(esc_hz * (harmonic + 1), freq_min + (harmonic * 0.5f * bandwidth_hz)); - - // update filter parameters if frequency changed or forced - for (int axis = 0; axis < 3; axis++) { - auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc]; - - const float notch_freq_delta = fabsf(nf.getNotchFreq() - frequency_hz); - - const bool notch_freq_changed = (notch_freq_delta > 0.1f); - - // only allow initializing one new filter per axis each iteration - const bool allow_update = !axis_init[axis] || (nf.initialized() && notch_freq_delta < nf.getBandwidth()); - - if ((force_update || notch_freq_changed) && allow_update) { - if (nf.setParameters(_filter_sample_rate_hz, frequency_hz, bandwidth_hz)) { - perf_count(_dynamic_notch_filter_esc_rpm_update_perf); - - if (!nf.initialized()) { - perf_count(_dynamic_notch_filter_esc_rpm_init_perf); - axis_init[axis] = true; - } - } - } - } - } - - _esc_available.set(esc, true); - _last_esc_rpm_notch_update[esc] = esc_report.timestamp; - } - } - } - - // check notch filter timeout - for (size_t esc = 0; esc < MAX_NUM_ESCS; esc++) { - if (_esc_available[esc] && (time_now_us > _last_esc_rpm_notch_update[esc] + DYNAMIC_NOTCH_FITLER_TIMEOUT)) { - bool all_disabled = true; - - // disable notch filters from highest frequency to lowest - for (int harmonic = _esc_rpm_harmonics - 1; harmonic >= 0; harmonic--) { - for (int axis = 0; axis < 3; axis++) { - auto &nf = _dynamic_notch_filter_esc_rpm[harmonic][axis][esc]; - - if (nf.getNotchFreq() > 0.f) { - if (nf.initialized() && !axis_init[axis]) { - nf.disable(); - perf_count(_dynamic_notch_filter_esc_rpm_disable_perf); - axis_init[axis] = true; - } - } - - if (nf.getNotchFreq() > 0.f) { - all_disabled = false; - } - } - } - - if (all_disabled) { - _esc_available.set(esc, false); - } - } - } - } - -#endif // !CONSTRAINED_FLASH -} - -void VehicleAngularVelocity::UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force) -{ -#if !defined(CONSTRAINED_FLASH) - const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::FFT; - - if (enabled && (_sensor_gyro_fft_sub.updated() || force)) { - - if (!_dynamic_notch_fft_available) { - // force update filters if previously disabled - force = true; - } - - sensor_gyro_fft_s sensor_gyro_fft; - - if (_sensor_gyro_fft_sub.copy(&sensor_gyro_fft) - && (sensor_gyro_fft.device_id == _selected_sensor_device_id) - && (time_now_us < sensor_gyro_fft.timestamp + DYNAMIC_NOTCH_FITLER_TIMEOUT) - && ((fabsf(sensor_gyro_fft.sensor_sample_rate_hz - _filter_sample_rate_hz) / _filter_sample_rate_hz) < 0.02f)) { - - static constexpr float peak_freq_min = 10.f; // lower bound TODO: configurable? - - const float bandwidth = math::constrain(sensor_gyro_fft.resolution_hz, 8.f, 30.f); // TODO: base on numerical limits? - - float *peak_frequencies[] {sensor_gyro_fft.peak_frequencies_x, sensor_gyro_fft.peak_frequencies_y, sensor_gyro_fft.peak_frequencies_z}; - - for (int axis = 0; axis < 3; axis++) { - for (int peak = 0; peak < MAX_NUM_FFT_PEAKS; peak++) { - - const float peak_freq = peak_frequencies[axis][peak]; - - auto &nf = _dynamic_notch_filter_fft[axis][peak]; - - if (peak_freq > peak_freq_min) { - // update filter parameters if frequency changed or forced - if (force || !nf.initialized() || (fabsf(nf.getNotchFreq() - peak_freq) > 0.1f)) { - nf.setParameters(_filter_sample_rate_hz, peak_freq, bandwidth); - perf_count(_dynamic_notch_filter_fft_update_perf); - } - - _dynamic_notch_fft_available = true; - - } else { - // disable this notch filter (if it isn't already) - if (nf.getNotchFreq() > 0.f) { - nf.disable(); - perf_count(_dynamic_notch_filter_fft_disable_perf); - } - } - } - } - - } else { - DisableDynamicNotchFFT(); - } - } - -#endif // !CONSTRAINED_FLASH -} - -float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int N) -{ -#if !defined(CONSTRAINED_FLASH) - - // Apply dynamic notch filter from ESC RPM - if (_dynamic_notch_filter_esc_rpm) { - for (int esc = 0; esc < MAX_NUM_ESCS; esc++) { - if (_esc_available[esc]) { - for (int harmonic = 0; harmonic < _esc_rpm_harmonics; harmonic++) { - if (_dynamic_notch_filter_esc_rpm[harmonic][axis][esc].getNotchFreq() > 0.f) { - _dynamic_notch_filter_esc_rpm[harmonic][axis][esc].applyArray(data, N); - } - } - } - } - } - - // Apply dynamic notch filter from FFT - if (_dynamic_notch_fft_available) { - for (int peak = MAX_NUM_FFT_PEAKS - 1; peak >= 0; peak--) { - if (_dynamic_notch_filter_fft[axis][peak].getNotchFreq() > 0.f) { - _dynamic_notch_filter_fft[axis][peak].applyArray(data, N); - } - } - } - -#endif // !CONSTRAINED_FLASH - - // Apply general notch filter 0 (IMU_GYRO_NF0_FRQ) - if (_notch_filter0_velocity[axis].getNotchFreq() > 0.f) { - _notch_filter0_velocity[axis].applyArray(data, N); - } - - // Apply general notch filter 1 (IMU_GYRO_NF1_FRQ) - if (_notch_filter1_velocity[axis].getNotchFreq() > 0.f) { - _notch_filter1_velocity[axis].applyArray(data, N); - } - - // Apply general low-pass filter (IMU_GYRO_CUTOFF) - _lp_filter_velocity[axis].applyArray(data, N); - - // return last filtered sample - return data[N - 1]; -} - -float VehicleAngularVelocity::FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N) -{ - // angular acceleration: Differentiate & apply specific angular acceleration (D-term) low-pass (IMU_DGYRO_CUTOFF) - float angular_acceleration_filtered = 0.f; - - for (int n = 0; n < N; n++) { - const float angular_acceleration = (data[n] - _angular_velocity_raw_prev(axis)) * inverse_dt_s; - angular_acceleration_filtered = _lp_filter_acceleration[axis].update(angular_acceleration); - _angular_velocity_raw_prev(axis) = data[n]; - } - - return angular_acceleration_filtered; -} - -void VehicleAngularVelocity::Run() -{ - perf_begin(_cycle_perf); - - // backup schedule - ScheduleDelayed(10_ms); - - const hrt_abstime time_now_us = hrt_absolute_time(); - - ParametersUpdate(); - - // update corrections first to set _selected_sensor - const bool selection_updated = SensorSelectionUpdate(time_now_us); - - if (selection_updated || _update_sample_rate) { - if (!UpdateSampleRate()) { - // sensor sample rate required to run - perf_end(_cycle_perf); - return; - } - } - - _calibration.SensorCorrectionsUpdate(selection_updated); - - SensorBiasUpdate(selection_updated); - - if (_reset_filters) { - ResetFilters(time_now_us); - - if (_reset_filters) { - // not safe to run until filters configured - perf_end(_cycle_perf); - return; - } - } - - UpdateDynamicNotchEscRpm(time_now_us); - UpdateDynamicNotchFFT(time_now_us); - - if (_fifo_available) { - // process all outstanding fifo messages - sensor_gyro_fifo_s sensor_fifo_data; - - while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { - const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; - const int N = sensor_fifo_data.samples; - static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); - - if ((sensor_fifo_data.dt > 0) && (N > 0) && (N <= FIFO_SIZE_MAX)) { - Vector3f angular_velocity_uncalibrated; - Vector3f angular_acceleration_uncalibrated; - - int16_t *raw_data_array[] {sensor_fifo_data.x, sensor_fifo_data.y, sensor_fifo_data.z}; - - for (int axis = 0; axis < 3; axis++) { - // copy raw int16 sensor samples to float array for filtering - float data[FIFO_SIZE_MAX]; - - for (int n = 0; n < N; n++) { - data[n] = sensor_fifo_data.scale * raw_data_array[axis][n]; - } - - // save last filtered sample - angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data, N); - angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data, N); - } - - // Publish - if (!_sensor_gyro_fifo_sub.updated()) { - if (CalibrateAndPublish(sensor_fifo_data.timestamp_sample, - angular_velocity_uncalibrated, - angular_acceleration_uncalibrated)) { - - perf_end(_cycle_perf); - return; - } - } - } - } - - } else { - // process all outstanding messages - sensor_gyro_s sensor_data; - - while (_sensor_sub.update(&sensor_data)) { - if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) { - - if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { - _timestamp_sample_last = sensor_data.timestamp_sample - 1e6f / _filter_sample_rate_hz; - } - - const float inverse_dt_s = 1.f / math::constrain(((sensor_data.timestamp_sample - _timestamp_sample_last) * 1e-6f), - 0.00002f, 0.02f); - _timestamp_sample_last = sensor_data.timestamp_sample; - - Vector3f angular_velocity_uncalibrated; - Vector3f angular_acceleration_uncalibrated; - - float raw_data_array[] {sensor_data.x, sensor_data.y, sensor_data.z}; - - for (int axis = 0; axis < 3; axis++) { - // copy sensor sample to float array for filtering - float data[1] {raw_data_array[axis]}; - - // save last filtered sample - angular_velocity_uncalibrated(axis) = FilterAngularVelocity(axis, data); - angular_acceleration_uncalibrated(axis) = FilterAngularAcceleration(axis, inverse_dt_s, data); - } - - // Publish - if (!_sensor_sub.updated()) { - if (CalibrateAndPublish(sensor_data.timestamp_sample, - angular_velocity_uncalibrated, - angular_acceleration_uncalibrated)) { - - perf_end(_cycle_perf); - return; - } - } - } - } - } - - // force reselection on timeout - if (time_now_us > _last_publish + 500_ms) { - SensorSelectionUpdate(true); - } - - perf_end(_cycle_perf); -} - -bool VehicleAngularVelocity::CalibrateAndPublish(const hrt_abstime ×tamp_sample, - const Vector3f &angular_velocity_uncalibrated, - const Vector3f &angular_acceleration_uncalibrated) -{ - if (timestamp_sample >= _last_publish + _publish_interval_min_us) { - - // Publish vehicle_angular_velocity - vehicle_angular_velocity_s angular_velocity; - angular_velocity.timestamp_sample = timestamp_sample; - - // Angular velocity: rotate sensor frame to board, scale raw data to SI, apply calibration, and remove in-run estimated bias - _angular_velocity = _calibration.Correct(angular_velocity_uncalibrated) - _bias; - _angular_velocity.copyTo(angular_velocity.xyz); - - // Angular acceleration: rotate sensor frame to board, scale raw data to SI, apply any additional configured rotation - _angular_acceleration = _calibration.rotation() * angular_acceleration_uncalibrated; - _angular_acceleration.copyTo(angular_velocity.xyz_derivative); - - angular_velocity.timestamp = hrt_absolute_time(); - _vehicle_angular_velocity_pub.publish(angular_velocity); - - // shift last publish time forward, but don't let it get further behind than the interval - _last_publish = math::constrain(_last_publish + _publish_interval_min_us, - timestamp_sample - _publish_interval_min_us, timestamp_sample); - - return true; - } - - return false; -} - -void VehicleAngularVelocity::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_angular_velocity] selected sensor: %" PRIu32 - ", rate: %.1f Hz %s, estimated bias: [%.5f %.5f %.5f]\n", - _calibration.device_id(), (double)_filter_sample_rate_hz, _fifo_available ? "FIFO" : "", - (double)_bias(0), (double)_bias(1), (double)_bias(2)); - - _calibration.PrintStatus(); - - perf_print_counter(_cycle_perf); - perf_print_counter(_filter_reset_perf); - perf_print_counter(_selection_changed_perf); -#if !defined(CONSTRAINED_FLASH) - perf_print_counter(_dynamic_notch_filter_esc_rpm_disable_perf); - perf_print_counter(_dynamic_notch_filter_esc_rpm_init_perf); - perf_print_counter(_dynamic_notch_filter_esc_rpm_update_perf); - - perf_print_counter(_dynamic_notch_filter_fft_disable_perf); - perf_print_counter(_dynamic_notch_filter_fft_update_perf); -#endif // CONSTRAINED_FLASH -} - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp deleted file mode 100644 index 8aae8a3c03e9..000000000000 --- a/src/modules/sensors/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ /dev/null @@ -1,201 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2022 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -namespace sensors -{ - -class VehicleAngularVelocity : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - VehicleAngularVelocity(); - ~VehicleAngularVelocity() override; - - void PrintStatus(); - bool Start(); - void Stop(); - -private: - void Run() override; - - bool CalibrateAndPublish(const hrt_abstime ×tamp_sample, const matrix::Vector3f &angular_velocity_uncalibrated, - const matrix::Vector3f &angular_acceleration_uncalibrated); - - inline float FilterAngularVelocity(int axis, float data[], int N = 1); - inline float FilterAngularAcceleration(int axis, float inverse_dt_s, float data[], int N = 1); - - void DisableDynamicNotchEscRpm(); - void DisableDynamicNotchFFT(); - void ParametersUpdate(bool force = false); - - void ResetFilters(const hrt_abstime &time_now_us); - void SensorBiasUpdate(bool force = false); - bool SensorSelectionUpdate(const hrt_abstime &time_now_us, bool force = false); - void UpdateDynamicNotchEscRpm(const hrt_abstime &time_now_us, bool force = false); - void UpdateDynamicNotchFFT(const hrt_abstime &time_now_us, bool force = false); - bool UpdateSampleRate(); - - // scaled appropriately for current sensor - matrix::Vector3f GetResetAngularVelocity() const; - matrix::Vector3f GetResetAngularAcceleration() const; - - static constexpr int MAX_SENSOR_COUNT = 4; - - uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - - uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; - uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; -#if !defined(CONSTRAINED_FLASH) - uORB::Subscription _esc_status_sub {ORB_ID(esc_status)}; - uORB::Subscription _sensor_gyro_fft_sub {ORB_ID(sensor_gyro_fft)}; -#endif // !CONSTRAINED_FLASH - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - uORB::SubscriptionCallbackWorkItem _sensor_sub{this, ORB_ID(sensor_gyro)}; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_fifo_sub{this, ORB_ID(sensor_gyro_fifo)}; - - calibration::Gyroscope _calibration{}; - - matrix::Vector3f _bias{}; - - matrix::Vector3f _angular_velocity{}; - matrix::Vector3f _angular_acceleration{}; - - matrix::Vector3f _angular_velocity_raw_prev{}; - hrt_abstime _timestamp_sample_last{0}; - - hrt_abstime _publish_interval_min_us{0}; - hrt_abstime _last_publish{0}; - - float _filter_sample_rate_hz{NAN}; - - // angular velocity filters - math::LowPassFilter2p _lp_filter_velocity[3] {}; - math::NotchFilter _notch_filter0_velocity[3] {}; - math::NotchFilter _notch_filter1_velocity[3] {}; - -#if !defined(CONSTRAINED_FLASH) - - enum DynamicNotch { - EscRpm = 1, - FFT = 2, - }; - - static constexpr hrt_abstime DYNAMIC_NOTCH_FITLER_TIMEOUT = 3_s; - - // ESC RPM - static constexpr int MAX_NUM_ESCS = sizeof(esc_status_s::esc) / sizeof(esc_status_s::esc[0]); - - using NotchFilterHarmonic = math::NotchFilter[3][MAX_NUM_ESCS]; - NotchFilterHarmonic *_dynamic_notch_filter_esc_rpm{nullptr}; - - int _esc_rpm_harmonics{0}; - px4::Bitset _esc_available{}; - hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESCS] {}; - - perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr}; - perf_counter_t _dynamic_notch_filter_esc_rpm_init_perf{nullptr}; - perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr}; - - // FFT - static constexpr int MAX_NUM_FFT_PEAKS = sizeof(sensor_gyro_fft_s::peak_frequencies_x) - / sizeof(sensor_gyro_fft_s::peak_frequencies_x[0]); - - math::NotchFilter _dynamic_notch_filter_fft[3][MAX_NUM_FFT_PEAKS] {}; - - perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr}; - perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr}; - - bool _dynamic_notch_fft_available{false}; -#endif // !CONSTRAINED_FLASH - - // angular acceleration filter - AlphaFilter _lp_filter_acceleration[3] {}; - - uint32_t _selected_sensor_device_id{0}; - - bool _reset_filters{true}; - bool _fifo_available{false}; - bool _update_sample_rate{true}; - - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": gyro filter")}; - perf_counter_t _filter_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro filter reset")}; - perf_counter_t _selection_changed_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro selection changed")}; - - DEFINE_PARAMETERS( -#if !defined(CONSTRAINED_FLASH) - (ParamInt) _param_imu_gyro_dnf_en, - (ParamInt) _param_imu_gyro_dnf_hmc, - (ParamFloat) _param_imu_gyro_dnf_bw, - (ParamFloat) _param_imu_gyro_dnf_min, -#endif // !CONSTRAINED_FLASH - (ParamFloat) _param_imu_gyro_cutoff, - (ParamFloat) _param_imu_gyro_nf0_frq, - (ParamFloat) _param_imu_gyro_nf0_bw, - (ParamFloat) _param_imu_gyro_nf1_frq, - (ParamFloat) _param_imu_gyro_nf1_bw, - (ParamInt) _param_imu_gyro_ratemax, - (ParamFloat) _param_imu_dgyro_cutoff - ) -}; - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_angular_velocity/imu_gyro_parameters.c b/src/modules/sensors/sensors/vehicle_angular_velocity/imu_gyro_parameters.c deleted file mode 100644 index cb5f320bc33b..000000000000 --- a/src/modules/sensors/sensors/vehicle_angular_velocity/imu_gyro_parameters.c +++ /dev/null @@ -1,206 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -/** -* Notch filter frequency for gyro -* -* The center frequency for the 2nd order notch filter on the primary gyro. -* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. -* This only affects the signal sent to the controllers, not the estimators. -* Applies to both angular velocity and angular acceleration sent to the controllers. -* See "IMU_GYRO_NF0_BW" to set the bandwidth of the filter. -* -* A value of 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_FRQ, 0.0f); - -/** -* Notch filter bandwidth for gyro -* -* The frequency width of the stop band for the 2nd order notch filter on the primary gyro. -* See "IMU_GYRO_NF0_FRQ" to activate the filter and to set the notch frequency. -* Applies to both angular velocity and angular acceleration sent to the controllers. -* -* @min 0 -* @max 100 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF0_BW, 20.0f); - -/** -* Notch filter 2 frequency for gyro -* -* The center frequency for the 2nd order notch filter on the primary gyro. -* This filter can be enabled to avoid feedback amplification of structural resonances at a specific frequency. -* This only affects the signal sent to the controllers, not the estimators. -* Applies to both angular velocity and angular acceleration sent to the controllers. -* See "IMU_GYRO_NF1_BW" to set the bandwidth of the filter. -* -* A value of 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_FRQ, 0.0f); - -/** -* Notch filter 1 bandwidth for gyro -* -* The frequency width of the stop band for the 2nd order notch filter on the primary gyro. -* See "IMU_GYRO_NF1_FRQ" to activate the filter and to set the notch frequency. -* Applies to both angular velocity and angular acceleration sent to the controllers. -* -* @min 0 -* @max 100 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_NF1_BW, 20.0f); - - -/** -* Low pass filter cutoff frequency for gyro -* -* The cutoff frequency for the 2nd order butterworth filter on the primary gyro. -* This only affects the angular velocity sent to the controllers, not the estimators. -* It applies also to the angular acceleration (D-Term filter), see IMU_DGYRO_CUTOFF. -* -* A value of 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 40.0f); - -/** -* Gyro control data maximum publication rate (inner loop rate) -* -* The maximum rate the gyro control data (vehicle_angular_velocity) will be -* allowed to publish at. This is the loop rate for the rate controller and outputs. -* -* Note: sensor data is always read and filtered at the full raw rate (eg commonly 8 kHz) regardless of this setting. -* -* @min 100 -* @max 2000 -* @value 100 100 Hz -* @value 250 250 Hz -* @value 400 400 Hz -* @value 800 800 Hz -* @value 1000 1000 Hz -* @value 2000 2000 Hz -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400); - -/** -* Cutoff frequency for angular acceleration (D-Term filter) -* -* The cutoff frequency for the 2nd order butterworth filter used on -* the time derivative of the measured angular velocity, also known as -* the D-term filter in the rate controller. The D-term uses the derivative of -* the rate and thus is the most susceptible to noise. Therefore, using -* a D-term filter allows to increase IMU_GYRO_CUTOFF, which -* leads to reduced control latency and permits to increase the P gains. -* -* A value of 0 disables the filter. -* -* @min 0 -* @max 1000 -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f); - -/** -* IMU gyro dynamic notch filtering -* -* Enable bank of dynamically updating notch filters. -* Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN). -* @group Sensors -* @min 0 -* @max 3 -* @bit 0 ESC RPM -* @bit 1 FFT -*/ -PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0); - -/** -* IMU gyro ESC notch filter bandwidth -* -* Bandwidth per notch filter when using dynamic notch filtering with ESC RPM. -* -* @group Sensors -* @unit Hz -* @min 5 -* @max 30 -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 15.f); - -/** -* IMU gyro dynamic notch filter harmonics -* -* ESC RPM number of harmonics (multiples of RPM) for ESC RPM dynamic notch filtering. -* -* @group Sensors -* @min 1 -* @max 7 -*/ -PARAM_DEFINE_INT32(IMU_GYRO_DNF_HMC, 3); - -/** -* IMU gyro dynamic notch filter minimum frequency -* -* Minimum notch filter frequency in Hz. -* -* @group Sensors -* @unit Hz -*/ -PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_MIN, 25.f); diff --git a/src/modules/sensors/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_gps_position/CMakeLists.txt deleted file mode 100644 index 12b625ead625..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 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_gps_position - VehicleGPSPosition.cpp - VehicleGPSPosition.hpp - gps_blending.cpp - gps_blending.hpp -) -target_link_libraries(vehicle_gps_position - PRIVATE - geo - px4_work_queue -) - -px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position) diff --git a/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.cpp deleted file mode 100644 index 964b5fdecc4a..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ /dev/null @@ -1,153 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "VehicleGPSPosition.hpp" - -#include -#include -#include - -namespace sensors -{ -VehicleGPSPosition::VehicleGPSPosition() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - _vehicle_gps_position_pub.advertise(); -} - -VehicleGPSPosition::~VehicleGPSPosition() -{ - Stop(); - perf_free(_cycle_perf); -} - -bool VehicleGPSPosition::Start() -{ - // force initial updates - ParametersUpdate(true); - - ScheduleNow(); - - return true; -} - -void VehicleGPSPosition::Stop() -{ - Deinit(); - - // clear all registered callbacks - for (auto &sub : _sensor_gps_sub) { - sub.unregisterCallback(); - } -} - -void VehicleGPSPosition::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - - if (_param_sens_gps_mask.get() == 0) { - _sensor_gps_sub[0].registerCallback(); - - } else { - for (auto &sub : _sensor_gps_sub) { - sub.registerCallback(); - } - } - - _gps_blending.setBlendingUseSpeedAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC); - _gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC); - _gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC); - _gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get()); - _gps_blending.setPrimaryInstance(_param_sens_gps_prime.get()); - } -} - -void VehicleGPSPosition::Run() -{ - perf_begin(_cycle_perf); - ParametersUpdate(); - - // Check all GPS instance - bool any_gps_updated = false; - bool gps_updated = false; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - gps_updated = _sensor_gps_sub[i].updated(); - - sensor_gps_s gps_data; - - if (gps_updated) { - any_gps_updated = true; - - _sensor_gps_sub[i].copy(&gps_data); - _gps_blending.setGpsData(gps_data, i); - - if (!_sensor_gps_sub[i].registered()) { - _sensor_gps_sub[i].registerCallback(); - } - } - } - - if (any_gps_updated) { - _gps_blending.update(hrt_absolute_time()); - - if (_gps_blending.isNewOutputDataAvailable()) { - sensor_gps_s gps_output{_gps_blending.getOutputGpsData()}; - - // clear device_id if blending - if (_gps_blending.getSelectedGps() == GpsBlending::GPS_MAX_RECEIVERS_BLEND) { - gps_output.device_id = 0; - } - - _vehicle_gps_position_pub.publish(gps_output); - } - } - - ScheduleDelayed(300_ms); // backup schedule - - perf_end(_cycle_perf); -} - -void VehicleGPSPosition::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps()); -} - -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.hpp deleted file mode 100644 index 971f24ae4af3..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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 "gps_blending.hpp" - -using namespace time_literals; - -namespace sensors -{ -class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - - VehicleGPSPosition(); - ~VehicleGPSPosition() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - void Run() override; - - void ParametersUpdate(bool force = false); - - // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm - static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1; - static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2; - static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4; - - // define max number of GPS receivers supported - static constexpr int GPS_MAX_RECEIVERS = 2; - static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND, - "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); - - uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] { /**< sensor data subscription */ - {this, ORB_ID(sensor_gps), 0}, - {this, ORB_ID(sensor_gps), 1}, - }; - - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - GpsBlending _gps_blending; - - DEFINE_PARAMETERS( - (ParamInt) _param_sens_gps_mask, - (ParamFloat) _param_sens_gps_tau, - (ParamInt) _param_sens_gps_prime - ) -}; -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.cpp deleted file mode 100644 index 3673ca14bb6c..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.cpp +++ /dev/null @@ -1,607 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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. - * - ****************************************************************************/ - -/** - * @file gps_blending.cpp - */ - -#include "gps_blending.hpp" - - -void GpsBlending::update(uint64_t hrt_now_us) -{ - _is_new_output_data_available = false; - - // blend multiple receivers if available - if (!blend_gps_data(hrt_now_us)) { - // Only use selected receiver data if it has been updated - uint8_t gps_select_index = 0; - - // Find the single "best" GPS from the data we have - // First, find the GPS(s) with the best fix - uint8_t best_fix = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type > best_fix) { - best_fix = _gps_state[i].fix_type; - } - } - - // Second, compare GPS's with best fix and take the one with most satellites - uint8_t max_sats = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) { - max_sats = _gps_state[i].satellites_used; - gps_select_index = i; - } - } - - // Check for new data on selected GPS, and clear blend offsets - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - _NE_pos_offset_m[i].zero(); - _hgt_offset_m[i] = 0.0; - } - - // Only use a secondary instance if the fallback is allowed - if ((_primary_instance > -1) - && (gps_select_index != _primary_instance) - && !_fallback_allowed) { - gps_select_index = _primary_instance; - } - - _selected_gps = gps_select_index; - _is_new_output_data_available = _gps_updated[gps_select_index]; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - _gps_updated[gps_select_index] = false; - } - } -} - -bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) -{ - /* - * If both receivers have the same update rate, use the oldest non-zero time. - * If two receivers with different update rates are used, use the slowest. - * If time difference is excessive, use newest to prevent a disconnected receiver - * from blocking updates. - */ - - // Calculate the time step for each receiver with some filtering to reduce the effects of jitter - // Find the largest and smallest time step. - float dt_max = 0.0f; - float dt_min = GPS_TIMEOUT_S; - _np_gps_suitable_for_blending = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - - float raw_dt = 0.f; - - if (_gps_state[i].timestamp > _time_prev_us[i]) { - raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]); - } - - float present_dt = 0.f; - - if (hrt_now_us > _gps_state[i].timestamp) { - present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp); - } - - if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { - _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; - - } else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) { - // Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt - _gps_state[i].timestamp = 0; - _gps_state[i].fix_type = 0; - _gps_state[i].satellites_used = 0; - _gps_state[i].vel_ned_valid = 0; - - if (i == _primary_instance) { - // Allow using a secondary instance when the primary - // receiver has timed out - _fallback_allowed = true; - } - - continue; - } - - // Only count GPSs with at least a 2D fix for blending purposes - if (_gps_state[i].fix_type < 2) { - continue; - } - - if (_gps_dt[i] > dt_max) { - dt_max = _gps_dt[i]; - _gps_slowest_index = i; - } - - if (_gps_dt[i] < dt_min) { - dt_min = _gps_dt[i]; - } - - _np_gps_suitable_for_blending++; - } - - // Find the receiver that is last be updated - uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message - uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - // Find largest and smallest times - if (_gps_state[i].timestamp > max_us) { - max_us = _gps_state[i].timestamp; - _gps_newest_index = i; - } - - if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) { - min_us = _gps_state[i].timestamp; - } - } - - if (_np_gps_suitable_for_blending < 2) { - // Less than 2 receivers left, so fall out of blending - return false; - } - - /* - * If the largest dt is less than 20% greater than the smallest, then we have receivers - * running at the same rate then we wait until we have two messages with an arrival time - * difference that is less than 50% of the smallest time step and use the time stamp from - * the newest data. - * Else we have two receivers at different update rates and use the slowest receiver - * as the timing reference. - */ - bool gps_new_output_data = false; - - if ((dt_max - dt_min) < 0.2f * dt_min) { - // both receivers assumed to be running at the same rate - if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) { - // data arrival within a short time window enables the two measurements to be blended - _gps_time_ref_index = _gps_newest_index; - gps_new_output_data = true; - } - - } else { - // both receivers running at different rates - _gps_time_ref_index = _gps_slowest_index; - - if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) { - // blend data at the rate of the slower receiver - gps_new_output_data = true; - } - } - - if (gps_new_output_data) { - // calculate the sum squared speed accuracy across all GPS sensors - float speed_accuracy_sum_sq = 0.0f; - - if (_blend_use_spd_acc) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) { - speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s; - } - } - } - - // calculate the sum squared horizontal position accuracy across all GPS sensors - float horizontal_accuracy_sum_sq = 0.0f; - - if (_blend_use_hpos_acc) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) { - horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph; - } - } - } - - // calculate the sum squared vertical position accuracy across all GPS sensors - float vertical_accuracy_sum_sq = 0.0f; - - if (_blend_use_vpos_acc) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) { - vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv; - } - } - } - - // Check if we can do blending using reported accuracy - bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f - || speed_accuracy_sum_sq > 0.0f); - - // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead - if (!can_do_blending) { - return false; - } - - float sum_of_all_weights = 0.0f; - - // calculate a weighting using the reported speed accuracy - float spd_blend_weights[GPS_MAX_RECEIVERS_BLEND] {}; - - if (speed_accuracy_sum_sq > 0.0f && _blend_use_spd_acc) { - // calculate the weights using the inverse of the variances - float sum_of_spd_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s >= 0.001f) { - spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s); - sum_of_spd_weights += spd_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_spd_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights; - } - - sum_of_all_weights += 1.0f; - } - } - - // calculate a weighting using the reported horizontal position - float hpos_blend_weights[GPS_MAX_RECEIVERS_BLEND] {}; - - if (horizontal_accuracy_sum_sq > 0.0f && _blend_use_hpos_acc) { - // calculate the weights using the inverse of the variances - float sum_of_hpos_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) { - hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph); - sum_of_hpos_weights += hpos_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_hpos_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights; - } - - sum_of_all_weights += 1.0f; - } - } - - // calculate a weighting using the reported vertical position accuracy - float vpos_blend_weights[GPS_MAX_RECEIVERS_BLEND] = {}; - - if (vertical_accuracy_sum_sq > 0.0f && _blend_use_vpos_acc) { - // calculate the weights using the inverse of the variances - float sum_of_vpos_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) { - vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv); - sum_of_vpos_weights += vpos_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_vpos_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights; - } - - sum_of_all_weights += 1.0f; - }; - } - - // blend weight for each GPS. The blend weights must sum to 1.0 across all instances. - float blend_weights[GPS_MAX_RECEIVERS_BLEND]; - - // calculate an overall weight - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; - } - - // With updated weights we can calculate a blended GPS solution and - // offsets for each physical receiver - sensor_gps_s gps_blended_state = gps_blend_states(blend_weights); - - update_gps_offsets(gps_blended_state); - - // calculate a blended output from the offset corrected receiver data - // publish if blending was successful - calc_gps_blend_output(gps_blended_state, blend_weights); - - _gps_blended_state = gps_blended_state; - _selected_gps = GPS_MAX_RECEIVERS_BLEND; - _is_new_output_data_available = true; - } - - return true; -} - -sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const -{ - // Use the GPS with the highest weighting as the reference position - float best_weight = 0.0f; - - // index of the physical receiver with the lowest reported error - uint8_t gps_best_index = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (blend_weights[i] > best_weight) { - best_weight = blend_weights[i]; - gps_best_index = i; - } - } - - // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. - sensor_gps_s gps_blended_state{_gps_state[gps_best_index]}; // start with best GPS for all other misc fields - - // zerp all fields that are an accumulated blend below - gps_blended_state.timestamp = 0; - gps_blended_state.timestamp_sample = 0; - gps_blended_state.vel_m_s = 0; - gps_blended_state.vel_n_m_s = 0; - gps_blended_state.vel_e_m_s = 0; - gps_blended_state.vel_d_m_s = 0; - - // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers - // If any receiver contributing has an invalid velocity, then report blended velocity as invalid - if (blend_weights[i] > 0.0f) { - - // blend the timing data - gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]); - gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]); - - // calculate a blended average speed and velocity vector - gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * blend_weights[i]; - gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * blend_weights[i]; - gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * blend_weights[i]; - gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * blend_weights[i]; - - - // use the lowest value - if (_gps_state[i].eph > 0.0f - && _gps_state[i].eph < gps_blended_state.eph) { - gps_blended_state.eph = _gps_state[i].eph; - } - - if (_gps_state[i].epv > 0.0f - && _gps_state[i].epv < gps_blended_state.epv) { - gps_blended_state.epv = _gps_state[i].epv; - } - - if (_gps_state[i].s_variance_m_s > 0.0f - && _gps_state[i].s_variance_m_s < gps_blended_state.s_variance_m_s) { - gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s; - } - - if (_gps_state[i].hdop > 0 - && _gps_state[i].hdop < gps_blended_state.hdop) { - gps_blended_state.hdop = _gps_state[i].hdop; - } - - if (_gps_state[i].vdop > 0 - && _gps_state[i].vdop < gps_blended_state.vdop) { - gps_blended_state.vdop = _gps_state[i].vdop; - } - - - // use the highest status - if (_gps_state[i].fix_type > gps_blended_state.fix_type) { - gps_blended_state.fix_type = _gps_state[i].fix_type; - } - - if (_gps_state[i].satellites_used > gps_blended_state.satellites_used) { - gps_blended_state.satellites_used = _gps_state[i].satellites_used; - } - - if (_gps_state[i].vel_ned_valid) { - gps_blended_state.vel_ned_valid = true; - } - } - - // TODO read parameters for individual GPS antenna positions and blend - // Vector3f temp_antenna_offset = _antenna_offset[i]; - // temp_antenna_offset *= blend_weights[i]; - // _blended_antenna_offset += temp_antenna_offset; - } - - /* - * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state. - * This is statistically the most likely location, but may not be stable enough for direct use by the EKF. - */ - - // Convert each GPS position to a local NEU offset relative to the reference position - Vector2f blended_NE_offset_m{0, 0}; - double blended_alt_offset_m = 0.0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) { - // calculate the horizontal offset - Vector2f horiz_offset{}; - get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, - _gps_state[i].latitude_deg, _gps_state[i].longitude_deg, - &horiz_offset(0), &horiz_offset(1)); - - // sum weighted offsets - blended_NE_offset_m += horiz_offset * blend_weights[i]; - - // calculate vertical offset, meters - double vert_offset_m = _gps_state[i].altitude_msl_m - gps_blended_state.altitude_msl_m; - - // sum weighted offsets - blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; - } - } - - // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = gps_blended_state.latitude_deg; - const double lon_deg_now = gps_blended_state.longitude_deg; - double lat_deg_res = 0; - double lon_deg_res = 0; - add_vector_to_global_position(lat_deg_now, lon_deg_now, - blended_NE_offset_m(0), blended_NE_offset_m(1), - &lat_deg_res, &lon_deg_res); - gps_blended_state.latitude_deg = lat_deg_res; - gps_blended_state.longitude_deg = lon_deg_res; - gps_blended_state.altitude_msl_m += blended_alt_offset_m; - - // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value - int8_t gps_best_yaw_index = -1; - float best_yaw_weight = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (PX4_ISFINITE(_gps_state[i].heading) && (blend_weights[i] > best_yaw_weight)) { - best_yaw_weight = blend_weights[i]; - gps_best_yaw_index = i; - } - } - - if (gps_best_yaw_index >= 0) { - gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading; - gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset; - gps_blended_state.heading_accuracy = _gps_state[gps_best_yaw_index].heading_accuracy; - } - - return gps_blended_state; -} - -void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) -{ - // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset - // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering - float alpha[GPS_MAX_RECEIVERS_BLEND] {}; - float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f); - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (_gps_state[i].timestamp > _time_prev_us[i]) { - // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter - alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), - 0.0f, 1.0f); - - _time_prev_us[i] = _gps_state[i].timestamp; - } - } - - // Calculate a filtered position delta for each GPS relative to the blended solution state - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - Vector2f offset; - get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, - gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, - &offset(0), &offset(1)); - - _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); - - _hgt_offset_m[i] = (gps_blended_state.altitude_msl_m - _gps_state[i].altitude_msl_m) * (double)alpha[i] + - _hgt_offset_m[i] * (1.0 - (double)alpha[i]); - } - - // calculate offset limits from the largest difference between receivers - Vector2f max_ne_offset{}; - double max_alt_offset = 0.0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - for (uint8_t j = i; j < GPS_MAX_RECEIVERS_BLEND; j++) { - if (i != j) { - Vector2f offset; - get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, - _gps_state[j].latitude_deg, _gps_state[j].longitude_deg, - &offset(0), &offset(1)); - max_ne_offset(0) = fmax(max_ne_offset(0), fabsf(offset(0))); - max_ne_offset(1) = fmax(max_ne_offset(1), fabsf(offset(1))); - max_alt_offset = fmax(max_alt_offset, fabs(_gps_state[i].altitude_msl_m - _gps_state[j].altitude_msl_m)); - } - } - } - - // apply offset limits - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); - _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); - _hgt_offset_m[i] = constrain(_hgt_offset_m[i], -max_alt_offset, max_alt_offset); - } -} - -void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, - float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const -{ - // Convert each GPS position to a local NEU offset relative to the reference position - // which is defined as the positon of the blended solution calculated from non offset corrected data - Vector2f blended_NE_offset_m{0, 0}; - double blended_alt_offset_m = 0.0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { - if (blend_weights[i] > 0.0f) { - - // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_orig = _gps_state[i].latitude_deg; - const double lon_deg_orig = _gps_state[i].longitude_deg; - double lat_deg_offset_res = 0; - double lon_deg_offset_res = 0; - add_vector_to_global_position(lat_deg_orig, lon_deg_orig, - _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), - &lat_deg_offset_res, &lon_deg_offset_res); - - double alt_offset_m = _gps_state[i].altitude_msl_m + _hgt_offset_m[i]; - - - // calculate the horizontal offset - Vector2f horiz_offset{}; - get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, - lat_deg_offset_res, lon_deg_offset_res, - &horiz_offset(0), &horiz_offset(1)); - - // sum weighted offsets - blended_NE_offset_m += horiz_offset * blend_weights[i]; - - // calculate vertical offset - double vert_offset_m = alt_offset_m - gps_blended_state.altitude_msl_m; - - // sum weighted offsets - blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; - } - } - - // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = gps_blended_state.latitude_deg; - const double lon_deg_now = gps_blended_state.longitude_deg; - double lat_deg_res = 0; - double lon_deg_res = 0; - add_vector_to_global_position(lat_deg_now, lon_deg_now, - blended_NE_offset_m(0), blended_NE_offset_m(1), - &lat_deg_res, &lon_deg_res); - - gps_blended_state.latitude_deg = lat_deg_res; - gps_blended_state.longitude_deg = lon_deg_res; - gps_blended_state.altitude_msl_m = gps_blended_state.altitude_msl_m + blended_alt_offset_m; -} diff --git a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.hpp deleted file mode 100644 index 6d33a643cd9a..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending.hpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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. - * - ****************************************************************************/ - -/** - * @file gps_blending.hpp - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include - -using matrix::Vector2f; -using math::constrain; - -using namespace time_literals; - -class GpsBlending -{ -public: - // Set the GPS timeout to 2s, after which a receiver will be ignored - static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s; - static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f); - - - GpsBlending() = default; - ~GpsBlending() = default; - - // define max number of GPS receivers supported for blending - static constexpr int GPS_MAX_RECEIVERS_BLEND = 2; - - void setGpsData(const sensor_gps_s &gps_data, int instance) - { - if (instance < GPS_MAX_RECEIVERS_BLEND) { - _gps_state[instance] = gps_data; - _gps_updated[instance] = true; - } - } - void setBlendingUseSpeedAccuracy(bool enabled) { _blend_use_spd_acc = enabled; } - void setBlendingUseHPosAccuracy(bool enabled) { _blend_use_hpos_acc = enabled; } - void setBlendingUseVPosAccuracy(bool enabled) { _blend_use_vpos_acc = enabled; } - void setBlendingTimeConstant(float tau) { _blending_time_constant = tau; } - void setPrimaryInstance(int primary) { _primary_instance = primary; } - - void update(uint64_t hrt_now_us); - - bool isNewOutputDataAvailable() const { return _is_new_output_data_available; } - int getNumberOfGpsSuitableForBlending() const { return _np_gps_suitable_for_blending; } - const sensor_gps_s &getOutputGpsData() const - { - if (_selected_gps < GPS_MAX_RECEIVERS_BLEND) { - return _gps_state[_selected_gps]; - - } else { - return _gps_blended_state; - } - } - int getSelectedGps() const { return _selected_gps; } - -private: - /* - * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical - * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers - * have significant position differences, variation in receiver estimated accuracy will cause undesirable - * variation in the position solution. - */ - bool blend_gps_data(uint64_t hrt_now_us); - - /* - * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated - * by calc_blend_weights() - */ - sensor_gps_s gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; - - /* - * The location in gps_blended_state will move around as the relative accuracy changes. - * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is - * calculated. - */ - void update_gps_offsets(const sensor_gps_s &gps_blended_state); - - /* - Calculate GPS output that is a blend of the offset corrected physical receiver data - */ - void calc_gps_blend_output(sensor_gps_s &gps_blended_state, float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const; - - sensor_gps_s _gps_state[GPS_MAX_RECEIVERS_BLEND] {}; ///< internal state data for the physical GPS - sensor_gps_s _gps_blended_state {}; - bool _gps_updated[GPS_MAX_RECEIVERS_BLEND] {}; - int _selected_gps{0}; - int _np_gps_suitable_for_blending{0}; - int _primary_instance{0}; ///< if -1, there is no primary isntance and the best receiver is used // TODO: use device_id - bool _fallback_allowed{false}; - - bool _is_new_output_data_available{false}; - - matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) - double _hgt_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (meters) - - uint64_t _time_prev_us[GPS_MAX_RECEIVERS_BLEND] {}; ///< the previous value of time_us for that GPS instance - used to detect new data. - uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update - uint8_t _gps_newest_index{0}; ///< index of the physical receiver with the newest data - uint8_t _gps_slowest_index{0}; ///< index of the physical receiver with the slowest update rate - float _gps_dt[GPS_MAX_RECEIVERS_BLEND] {}; ///< average time step in seconds. - - bool _blend_use_spd_acc{false}; - bool _blend_use_hpos_acc{false}; - bool _blend_use_vpos_acc{false}; - - float _blending_time_constant{0.f}; -}; diff --git a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/sensors/vehicle_gps_position/gps_blending_test.cpp deleted file mode 100644 index af5b1feab706..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/gps_blending_test.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2021 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. - * - ****************************************************************************/ - -/** - * Test code for the GPS blending logic - * Run this test only using make tests TESTFILTER=GpsBlending - * - * @author Mathieu Bresciani - */ - -#include -#include - -#include "gps_blending.hpp" - -using matrix::Vector3f; - -class GpsBlendingTest : public ::testing::Test -{ -public: - sensor_gps_s getDefaultGpsData(); - void runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data, int instance); - void runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data0, sensor_gps_s &gps_data1); - - uint64_t _time_now_us{1000000}; -}; - -sensor_gps_s GpsBlendingTest::getDefaultGpsData() -{ - sensor_gps_s gps_data{}; - gps_data.timestamp = _time_now_us - 10e3; - gps_data.time_utc_usec = 0; - gps_data.latitude_deg = 47.0; - gps_data.longitude_deg = 9.0; - gps_data.altitude_msl_m = 800.0; - gps_data.altitude_ellipsoid_m = 800.0; - gps_data.s_variance_m_s = 0.2f; - gps_data.c_variance_rad = 0.5f; - gps_data.eph = 0.7f; - gps_data.epv = 1.2f; - gps_data.hdop = 1.f; - gps_data.vdop = 1.f; - gps_data.noise_per_ms = 20; - gps_data.jamming_indicator = 40; - gps_data.vel_m_s = 1.f; - gps_data.vel_n_m_s = 1.f; - gps_data.vel_e_m_s = 1.f; - gps_data.vel_d_m_s = 1.f; - gps_data.cog_rad = 0.f; - gps_data.timestamp_time_relative = 0; - gps_data.heading = NAN; - gps_data.heading_offset = 0.f; - gps_data.fix_type = 4; - gps_data.vel_ned_valid = true; - gps_data.satellites_used = 8; - - return gps_data; -} - -void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data, int instance) -{ - const float dt = 0.1; - const uint64_t dt_us = static_cast(dt * 1e6f); - - for (int k = 0; k < static_cast(duration_s / dt); k++) { - gps_blending.setGpsData(gps_data, instance); - gps_blending.update(_time_now_us); - - _time_now_us += dt_us; - gps_data.timestamp += dt_us; - } -} - -void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, sensor_gps_s &gps_data0, - sensor_gps_s &gps_data1) -{ - const float dt = 0.1; - const uint64_t dt_us = static_cast(dt * 1e6f); - - for (int k = 0; k < static_cast(duration_s / dt); k++) { - gps_blending.setGpsData(gps_data0, 0); - gps_blending.setGpsData(gps_data1, 1); - - gps_blending.update(_time_now_us); - - _time_now_us += dt_us; - gps_data0.timestamp += dt_us; - gps_data1.timestamp += dt_us; - } -} - -TEST_F(GpsBlendingTest, noData) -{ - GpsBlending gps_blending; - - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); - - gps_blending.update(_time_now_us); - - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); -} - -TEST_F(GpsBlendingTest, singleReceiver) -{ - GpsBlending gps_blending; - - gps_blending.setPrimaryInstance(-1); - sensor_gps_s gps_data = getDefaultGpsData(); - - gps_blending.setGpsData(gps_data, 1); - gps_blending.update(_time_now_us); - - _time_now_us += 200e3; - gps_data.timestamp = _time_now_us - 10e3; - gps_blending.setGpsData(gps_data, 1); - gps_blending.update(_time_now_us); - - EXPECT_EQ(gps_blending.getSelectedGps(), 1); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - - // BUT IF: a second update is called without data - gps_blending.update(_time_now_us); - - // THEN: no new data should be available - EXPECT_EQ(gps_blending.getSelectedGps(), 1); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); - EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); -} - -TEST_F(GpsBlendingTest, dualReceiverNoBlending) -{ - GpsBlending gps_blending; - - // GIVEN: two receivers with the same prioity - gps_blending.setPrimaryInstance(-1); - sensor_gps_s gps_data0 = getDefaultGpsData(); - sensor_gps_s gps_data1 = getDefaultGpsData(); - - gps_data1.satellites_used = gps_data0.satellites_used + 2; // gps1 has more satellites than gps0 - gps_blending.setGpsData(gps_data0, 0); - gps_blending.setGpsData(gps_data1, 1); - gps_blending.update(_time_now_us); - - // THEN: gps1 should be selected because it has more satellites - EXPECT_EQ(gps_blending.getSelectedGps(), 1); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - - gps_data1.satellites_used = gps_data0.satellites_used - 2; // gps1 has less satellites than gps0 - gps_blending.setGpsData(gps_data0, 0); - gps_blending.setGpsData(gps_data1, 1); - gps_blending.update(_time_now_us); - - // THEN: gps0 should be selected because it has more satellites - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); -} - -TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) -{ - GpsBlending gps_blending; - - sensor_gps_s gps_data0 = getDefaultGpsData(); - sensor_gps_s gps_data1 = getDefaultGpsData(); - - gps_blending.setBlendingUseHPosAccuracy(true); - - gps_data1.eph = gps_data0.eph / 2.f; - gps_blending.setGpsData(gps_data0, 0); - gps_blending.setGpsData(gps_data1, 1); - gps_blending.update(_time_now_us); - - // THEN: the blended instance should be selected (2) - // and the eph should be adjusted - EXPECT_EQ(gps_blending.getSelectedGps(), 2); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - EXPECT_LT(gps_blending.getOutputGpsData().eph, gps_data0.eph); - EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than - EXPECT_EQ(gps_blending.getOutputGpsData().timestamp, gps_data0.timestamp); - EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample); - EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); - EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); - EXPECT_EQ(gps_blending.getOutputGpsData().altitude_msl_m, gps_data0.altitude_msl_m); -} - -TEST_F(GpsBlendingTest, dualReceiverFailover) -{ - GpsBlending gps_blending; - - // GIVEN: a dual GPS setup with the first instance (0) - // set as primary - gps_blending.setPrimaryInstance(0); - gps_blending.setBlendingUseSpeedAccuracy(false); - gps_blending.setBlendingUseHPosAccuracy(false); - gps_blending.setBlendingUseVPosAccuracy(false); - - // WHEN: only the secondary receiver is available - sensor_gps_s gps_data1 = getDefaultGpsData(); - - const float duration_s = 10.f; - runSeconds(duration_s, gps_blending, gps_data1, 1); - - // THEN: the primary instance should be selected even if - // not available. No data is then available - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 1); - EXPECT_FALSE(gps_blending.isNewOutputDataAvailable()); - - // BUT WHEN: the data of the primary receiver is avaialbe - sensor_gps_s gps_data0 = getDefaultGpsData(); - gps_blending.setGpsData(gps_data0, 0); - gps_blending.update(_time_now_us); - - // THEN: the primary instance is selected and the data - // is available - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_EQ(gps_blending.getNumberOfGpsSuitableForBlending(), 2); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - - runSeconds(duration_s, gps_blending, gps_data0, gps_data1); - - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - - // BUT WHEN: the primary receiver isn't available anymore - runSeconds(duration_s, gps_blending, gps_data1, 1); - - // THEN: the data of the secondary receiver can be used - EXPECT_EQ(gps_blending.getSelectedGps(), 1); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); - - // AND IF: the primary receiver is available again and has - // better metrics than the secondary one - gps_data0.timestamp = gps_data1.timestamp; - gps_data0.satellites_used = gps_data1.satellites_used + 2; - - runSeconds(1.f, gps_blending, gps_data0, gps_data1); - - // THEN: the primary receiver should be used again - EXPECT_EQ(gps_blending.getSelectedGps(), 0); - EXPECT_TRUE(gps_blending.isNewOutputDataAvailable()); -} diff --git a/src/modules/sensors/sensors/vehicle_gps_position/params.c b/src/modules/sensors/sensors/vehicle_gps_position/params.c deleted file mode 100644 index 651e6e15fa5f..000000000000 --- a/src/modules/sensors/sensors/vehicle_gps_position/params.c +++ /dev/null @@ -1,82 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -/** - * Multi GPS Blending Control Mask. - * - * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance. - * 0 : Set to true to use speed accuracy - * 1 : Set to true to use horizontal position accuracy - * 2 : Set to true to use vertical position accuracy - * - * @group Sensors - * @min 0 - * @max 7 - * @bit 0 use speed accuracy - * @bit 1 use hpos accuracy - * @bit 2 use vpos accuracy - */ -PARAM_DEFINE_INT32(SENS_GPS_MASK, 7); - -/** - * Multi GPS Blending Time Constant - * - * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences. - * - * - * @group Sensors - * @min 1.0 - * @max 100.0 - * @unit s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); - -/** - * Multi GPS primary instance - * - * When no blending is active, this defines the preferred GPS receiver instance. - * The GPS selection logic waits until the primary receiver is available to - * send data to the EKF even if a secondary instance is already available. - * The secondary instance is then only used if the primary one times out. - * - * To have an equal priority of all the instances, set this parameter to -1 and - * the best receiver will be used. - * - * This parameter has no effect if blending is active. - * - * @group Sensors - * @min -1 - * @max 1 - */ -PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0); diff --git a/src/modules/sensors/sensors/vehicle_imu/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_imu/CMakeLists.txt deleted file mode 100644 index fe6ddeb8a4a1..000000000000 --- a/src/modules/sensors/sensors/vehicle_imu/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020-2021 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_imu - VehicleIMU.cpp - VehicleIMU.hpp -) - -target_compile_options(vehicle_imu - PRIVATE - ${MAX_CUSTOM_OPT_LEVEL} - #-DDEBUG_BUILD -) - -target_link_libraries(vehicle_imu PRIVATE px4_work_queue sensor_calibration) diff --git a/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.cpp deleted file mode 100644 index 788282d6d39c..000000000000 --- a/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.cpp +++ /dev/null @@ -1,1070 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "VehicleIMU.hpp" - -#include -#include -#include -#include - -using namespace matrix; - -using math::constrain; - -namespace sensors -{ - -VehicleIMU::VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config) : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, config), - _sensor_accel_sub(ORB_ID(sensor_accel), accel_index), - _sensor_gyro_sub(this, ORB_ID(sensor_gyro), gyro_index), - _instance(instance) -{ - _imu_integration_interval_us = 1e6f / _param_imu_integ_rate.get(); - - _accel_integrator.set_reset_interval(_imu_integration_interval_us); - _accel_integrator.set_reset_samples(sensor_accel_s::ORB_QUEUE_LENGTH); - - _gyro_integrator.set_reset_interval(_imu_integration_interval_us); - _gyro_integrator.set_reset_samples(sensor_gyro_s::ORB_QUEUE_LENGTH); - -#if defined(ENABLE_LOCKSTEP_SCHEDULER) - // currently with lockstep every raw sample needs a corresponding vehicle_imu publication - _sensor_gyro_sub.set_required_updates(1); -#else - // schedule conservatively until the actual accel & gyro rates are known - _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); -#endif - - // advertise immediately to ensure consistent ordering - _vehicle_imu_pub.advertise(); - _vehicle_imu_status_pub.advertise(); - - accel_drift_timestep = 0; - gyro_drift_timestep = 0; -} - -VehicleIMU::~VehicleIMU() -{ - Stop(); - - perf_free(_accel_generation_gap_perf); - perf_free(_gyro_generation_gap_perf); - - _vehicle_imu_pub.unadvertise(); - _vehicle_imu_status_pub.unadvertise(); -} - -bool VehicleIMU::Start() -{ - // force initial updates - ParametersUpdate(true); - - _sensor_gyro_sub.registerCallback(); - ScheduleNow(); - return true; -} - -void VehicleIMU::Stop() -{ - // clear all registered callbacks - _sensor_gyro_sub.unregisterCallback(); - - Deinit(); -} - -bool VehicleIMU::ParametersUpdate(bool force) -{ - bool updated = false; - - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - const auto imu_integ_rate_prev = _param_imu_integ_rate.get(); - - updateParams(); - - updated = true; - - const auto accel_calibration_count = _accel_calibration.calibration_count(); - const auto gyro_calibration_count = _gyro_calibration.calibration_count(); - _accel_calibration.ParametersUpdate(); - _gyro_calibration.ParametersUpdate(); - - if (accel_calibration_count != _accel_calibration.calibration_count()) { - // if calibration changed reset any existing learned calibration - _accel_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - - for (auto &learned_cal : _accel_learned_calibration) { - learned_cal = {}; - } - } - - if (gyro_calibration_count != _gyro_calibration.calibration_count()) { - // if calibration changed reset any existing learned calibration - _gyro_cal_available = false; - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - - for (auto &learned_cal : _gyro_learned_calibration) { - learned_cal = {}; - } - } - - // constrain IMU integration time 1-20 milliseconds (50-1000 Hz) - const int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(), (int32_t)50, (int32_t)1000); - - if (imu_integration_rate_hz != _param_imu_integ_rate.get()) { - PX4_WARN("IMU_INTEG_RATE updated %" PRId32 " -> %" PRIu32, _param_imu_integ_rate.get(), imu_integration_rate_hz); - _param_imu_integ_rate.set(imu_integration_rate_hz); - _param_imu_integ_rate.commit_no_notification(); - } - - _imu_integration_interval_us = 1e6f / imu_integration_rate_hz; - - if (_param_imu_integ_rate.get() != imu_integ_rate_prev) { - // force update - _update_integrator_config = true; - } - } - - return updated; -} - -void VehicleIMU::Run() -{ - const hrt_abstime now_us = hrt_absolute_time(); - - const bool parameters_updated = ParametersUpdate(); - - if (!_accel_calibration.enabled() || !_gyro_calibration.enabled()) { - _sensor_gyro_sub.unregisterCallback(); - ScheduleDelayed(1_s); - return; - } - - // backup schedule - ScheduleDelayed(_backup_schedule_timeout_us); - - // check vehicle status for changes to armed state - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _armed = vehicle_control_mode.flag_armed; - } - } - - // reset data gap monitor - _data_gap = false; - - while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { - bool updated = false; - - bool consume_all_gyro = !_intervals_configured || _data_gap; - - // monitor scheduling latency and force catch up with latest gyro if falling behind - if (_intervals_configured && !consume_all_gyro && _sensor_gyro_sub.updated() - && _gyro_publish_latency_mean_us.valid() && _gyro_mean_interval_us.valid() - && (_gyro_publish_latency_mean_us.mean() > _gyro_mean_interval_us.mean()) - ) { - PX4_DEBUG("%d gyro publish latency %.6f > %.6f us (mean interval)", - _instance, - (double)_gyro_publish_latency_mean_us.mean(), - (double)_gyro_mean_interval_us.mean() - ); - - consume_all_gyro = true; - } - - // update gyro until integrator ready and not falling behind - if (!_gyro_integrator.integral_ready() || consume_all_gyro) { - if (UpdateGyro()) { - updated = true; - } - } - - - // update accel until integrator ready and caught up to gyro - while (_sensor_accel_sub.updated() - && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap - || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us))) - ) { - - if (UpdateAccel()) { - updated = true; - } - } - - // reconfigure integrators if calculated sensor intervals have changed - if (_update_integrator_config || !_intervals_configured) { - UpdateIntegratorConfiguration(); - } - - // check for additional updates and that we're fully caught up before publishing - if ((consume_all_gyro || _data_gap) && _sensor_gyro_sub.updated()) { - continue; - } - - // publish if both accel & gyro integrators are ready - if (_intervals_configured && _accel_integrator.integral_ready() && _gyro_integrator.integral_ready()) { - if (Publish()) { - break; - } - } - - // finish if there are no more updates, but didn't publish - if (!updated) { - break; - } - } - - if (_param_sens_imu_autocal.get() && !parameters_updated) { - if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) - && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) { - - SensorCalibrationUpdate(); - _in_flight_calibration_check_timestamp_last = now_us; - - } else if (!_armed) { - SensorCalibrationSaveAccel(); - SensorCalibrationSaveGyro(); - } - } -} - -bool VehicleIMU::UpdateAccel() -{ - bool updated = false; - - // integrate queued accel - sensor_accel_s accel; - - if (_sensor_accel_sub.update(&accel)) { - if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) { - _data_gap = true; - perf_count(_accel_generation_gap_perf); - - } else { - // collect sample interval average for filters - if (accel.timestamp_sample > _accel_timestamp_sample_last) { - if (_accel_timestamp_sample_last != 0) { - const float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last; - - _accel_mean_interval_us.update(interval_us); - _accel_fifo_mean_interval_us.update(interval_us / math::max(accel.samples, (uint8_t)1)); - - // check measured interval periodically - if (_accel_mean_interval_us.valid() && (_accel_mean_interval_us.count() % 10 == 0)) { - - const float interval_mean = _accel_mean_interval_us.mean(); - - // update sample rate if previously invalid or changed by more than 1 standard deviation - const bool diff_exceeds_stddev = sq(interval_mean - _accel_interval_us) > _accel_mean_interval_us.variance(); - - if (!PX4_ISFINITE(_accel_interval_us) || diff_exceeds_stddev) { - // update integrator configuration if interval has changed by more than 10% - _update_integrator_config = true; - } - } - } - - } else { - PX4_ERR("%d - accel %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64, - _instance, accel.device_id, accel.timestamp_sample, _accel_timestamp_sample_last); - } - - if (accel.timestamp < accel.timestamp_sample) { - PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", - _instance, accel.device_id, accel.timestamp, accel.timestamp_sample); - } - } - - _accel_last_generation = _sensor_accel_sub.get_last_generation(); - - _accel_calibration.set_device_id(accel.device_id); - - if (accel.error_count != _status.accel_error_count) { - _publish_status = true; - _status.accel_error_count = accel.error_count; - } - - - // temperature average - if (PX4_ISFINITE(accel.temperature)) { - if ((_accel_temperature_sum_count == 0) || !PX4_ISFINITE(_accel_temperature_sum)) { - _accel_temperature_sum = accel.temperature; - _accel_temperature_sum_count = 1; - - } else { - _accel_temperature_sum += accel.temperature; - _accel_temperature_sum_count += 1; - } - } - - - const float dt = (accel.timestamp_sample - _accel_timestamp_sample_last) * 1e-6f; - _accel_timestamp_sample_last = accel.timestamp_sample; - - const Vector3f accel_raw{accel.x, accel.y, accel.z}; - _raw_accel_mean.update(accel_raw); - _accel_integrator.put(accel_raw, dt); - - updated = true; - - if (accel.clip_counter[0] > 0 || accel.clip_counter[1] > 0 || accel.clip_counter[2] > 0) { - // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_accel_calibration.rotation() * - Vector3f{(float)accel.clip_counter[0], (float)accel.clip_counter[1], (float)accel.clip_counter[2]}}; - - // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); - - _status.accel_clipping[0] += clip_x; - _status.accel_clipping[1] += clip_y; - _status.accel_clipping[2] += clip_z; - - if (clip_x > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_X; - } - - if (clip_y > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Y; - } - - if (clip_z > 0) { - _delta_velocity_clipping |= vehicle_imu_s::CLIPPING_Z; - } - - _publish_status = true; - - if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) { - // start notifying the user periodically if there's significant continuous clipping - const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; - - if (clipping_total > _last_accel_clipping_notify_total_count + 1000) { - mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance); - /* EVENT - * @description Land now, and check the vehicle setup. - * Clipping can lead to fly-aways. - */ - events::send(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical, - "Accel {1} clipping, not safe to fly!", _instance); - _last_accel_clipping_notify_time = accel.timestamp_sample; - _last_accel_clipping_notify_total_count = clipping_total; - } - } - } - } - - return updated; -} - -bool VehicleIMU::UpdateGyro() -{ - bool updated = false; - - // integrate queued gyro - sensor_gyro_s gyro; - - if (_sensor_gyro_sub.update(&gyro)) { - if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) { - _data_gap = true; - perf_count(_gyro_generation_gap_perf); - - } else { - // collect sample interval average for filters - if (gyro.timestamp_sample > _gyro_timestamp_sample_last) { - if (_gyro_timestamp_sample_last != 0) { - - const float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last; - - _gyro_mean_interval_us.update(interval_us); - _gyro_fifo_mean_interval_us.update(interval_us / math::max(gyro.samples, (uint8_t)1)); - - // check measured interval periodically - if (_gyro_mean_interval_us.valid() && (_gyro_mean_interval_us.count() % 10 == 0)) { - const float interval_mean = _gyro_mean_interval_us.mean(); - - // update sample rate if previously invalid or changed by more than 1 standard deviation - const bool diff_exceeds_stddev = sq(interval_mean - _gyro_interval_us) > _gyro_mean_interval_us.variance(); - - if (!PX4_ISFINITE(_gyro_interval_us) || diff_exceeds_stddev) { - // update integrator configuration if interval has changed by more than 10% - _update_integrator_config = true; - } - } - } - - } else { - PX4_ERR("%d - gyro %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64, - _instance, gyro.device_id, gyro.timestamp_sample, _gyro_timestamp_sample_last); - } - - if (gyro.timestamp < gyro.timestamp_sample) { - PX4_ERR("%d - gyro %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")", - _instance, gyro.device_id, gyro.timestamp, gyro.timestamp_sample); - } - } - - _gyro_last_generation = _sensor_gyro_sub.get_last_generation(); - - const float dt = (gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; - - _gyro_timestamp_sample_last = gyro.timestamp_sample; - _gyro_timestamp_last = gyro.timestamp; - - _gyro_calibration.set_device_id(gyro.device_id); - - if (gyro.error_count != _status.gyro_error_count) { - _publish_status = true; - _status.gyro_error_count = gyro.error_count; - } - - // temperature average - if (PX4_ISFINITE(gyro.temperature)) { - if ((_gyro_temperature_sum_count == 0) || !PX4_ISFINITE(_gyro_temperature_sum)) { - _gyro_temperature_sum = gyro.temperature; - _gyro_temperature_sum_count = 1; - - } else { - _gyro_temperature_sum += gyro.temperature; - _gyro_temperature_sum_count += 1; - } - } - - const Vector3f gyro_raw{gyro.x, gyro.y, gyro.z}; - _raw_gyro_mean.update(gyro_raw); - _gyro_integrator.put(gyro_raw, dt); - - updated = true; - - if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) { - // rotate sensor clip counts into vehicle body frame - const Vector3f clipping{_gyro_calibration.rotation() * - Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}}; - - // round to get reasonble clip counts per axis (after board rotation) - const uint8_t clip_x = roundf(fabsf(clipping(0))); - const uint8_t clip_y = roundf(fabsf(clipping(1))); - const uint8_t clip_z = roundf(fabsf(clipping(2))); - - _status.gyro_clipping[0] += clip_x; - _status.gyro_clipping[1] += clip_y; - _status.gyro_clipping[2] += clip_z; - - if (clip_x > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_X; - } - - if (clip_y > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y; - } - - if (clip_z > 0) { - _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z; - } - - _publish_status = true; - - if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) { - // start notifying the user periodically if there's significant continuous clipping - const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2]; - - if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) { - mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance); - /* EVENT - * @description Land now, and check the vehicle setup. - * Clipping can lead to fly-aways. - */ - events::send(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical, - "Gyro {1} clipping, not safe to fly!", _instance); - _last_gyro_clipping_notify_time = gyro.timestamp_sample; - _last_gyro_clipping_notify_total_count = clipping_total; - } - } - } - } - - return updated; -} - -float generate_wgn(float std_dev) -{ - // generate white Gaussian noise sample with specified standard deviation (std_dev) - - static float V1, V2, S; - static bool phase = true; - float X; - - if (phase) { - do { - float U1 = (float)rand() / (float)RAND_MAX; - float U2 = (float)rand() / (float)RAND_MAX; - V1 = 2.0f * U1 - 1.0f; - V2 = 2.0f * U2 - 1.0f; - S = V1 * V1 + V2 * V2; - } while (S >= 1.0f || fabsf(S) < 1e-8f); - - X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); - - } else { - X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); - } - - phase = !phase; - return X * std_dev; // Scale the output by the standard deviation -} - -bool VehicleIMU::Publish() -{ - bool updated = false; - - vehicle_imu_s imu; - Vector3f delta_angle; - Vector3f delta_velocity; - - const Vector3f accumulated_coning_corrections = _gyro_integrator.accumulated_coning_corrections(); - - if (_accel_integrator.reset(delta_velocity, imu.delta_velocity_dt) - && _gyro_integrator.reset(delta_angle, imu.delta_angle_dt)) { - - if (_accel_calibration.enabled() && _gyro_calibration.enabled()) { - - // delta angle: apply offsets, scale, and board rotation - _gyro_calibration.SensorCorrectionsUpdate(); - const float gyro_dt_s = 1.e-6f * imu.delta_angle_dt; - const Vector3f angular_velocity{_gyro_calibration.Correct(delta_angle / gyro_dt_s)}; - UpdateGyroVibrationMetrics(angular_velocity); - const Vector3f delta_angle_corrected{angular_velocity * gyro_dt_s}; - - // accumulate delta angle coning corrections - _coning_norm_accum += accumulated_coning_corrections.norm() * gyro_dt_s; - _coning_norm_accum_total_time_s += gyro_dt_s; - - - // delta velocity: apply offsets, scale, and board rotation - _accel_calibration.SensorCorrectionsUpdate(); - const float accel_dt_s = 1.e-6f * imu.delta_velocity_dt; - const Vector3f acceleration{_accel_calibration.Correct(delta_velocity / accel_dt_s)}; - UpdateAccelVibrationMetrics(acceleration); - const Vector3f delta_velocity_corrected{acceleration * accel_dt_s}; - - // vehicle_imu_status - // publish before vehicle_imu so that error counts are available synchronously if needed - const bool status_publish_interval_exceeded = (hrt_elapsed_time(&_status.timestamp) >= kIMUStatusPublishingInterval); - - if (_raw_accel_mean.valid() && _raw_gyro_mean.valid() - && _accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid() - && (_publish_status || status_publish_interval_exceeded) - ) { - - // Accel - { - _status.accel_device_id = _accel_calibration.device_id(); - - _status.accel_rate_hz = 1e6f / _accel_mean_interval_us.mean(); - _status.accel_raw_rate_hz = 1e6f / _accel_fifo_mean_interval_us.mean(); // FIFO - - // accel mean and variance - const Dcmf &R = _accel_calibration.rotation(); - Vector3f(R * _raw_accel_mean.mean()).copyTo(_status.mean_accel); - - // variance from R * COV * R^T - const Matrix3f cov = R * _raw_accel_mean.covariance() * R.transpose(); - cov.diag().copyTo(_status.var_accel); - - // temperature - if ((_accel_temperature_sum_count > 0) && PX4_ISFINITE(_accel_temperature_sum)) { - _status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count; - - } else { - _status.temperature_accel = NAN; - } - } - - // Gyro - { - _status.gyro_device_id = _gyro_calibration.device_id(); - - _status.gyro_rate_hz = 1e6f / _gyro_mean_interval_us.mean(); - _status.gyro_raw_rate_hz = 1e6f / _gyro_fifo_mean_interval_us.mean(); // FIFO - - // gyro mean and variance - const Dcmf &R = _gyro_calibration.rotation(); - Vector3f(R * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro); - - // variance from R * COV * R^T - const Matrix3f cov = R * _raw_gyro_mean.covariance() * R.transpose(); - cov.diag().copyTo(_status.var_gyro); - - - // Gyro delta angle coning metric = length of coning corrections averaged since last status publication - _status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s; - _coning_norm_accum = 0; - _coning_norm_accum_total_time_s = 0; - - // temperature - if ((_gyro_temperature_sum_count > 0) && PX4_ISFINITE(_gyro_temperature_sum)) { - _status.temperature_gyro = _gyro_temperature_sum / _gyro_temperature_sum_count; - - } else { - _status.temperature_gyro = NAN; - } - } - - // publish - _status.timestamp = hrt_absolute_time(); - _vehicle_imu_status_pub.publish(_status); - - _publish_status = false; - - if (status_publish_interval_exceeded) { - _raw_accel_mean.reset(); - _accel_temperature_sum = NAN; - _accel_temperature_sum_count = 0; - - _raw_gyro_mean.reset(); - _gyro_temperature_sum = NAN; - _gyro_temperature_sum_count = 0; - } - } - - // publish vehicle_imu - imu.timestamp_sample = _gyro_timestamp_sample_last; - imu.accel_device_id = _accel_calibration.device_id(); - imu.gyro_device_id = _gyro_calibration.device_id(); - delta_angle_corrected.copyTo(imu.delta_angle); - delta_velocity_corrected.copyTo(imu.delta_velocity); - - // Adding faults to the accelerometer - param_t accel_fault = param_find("SENS_ACCEL_FAULT"); - int32_t accel_fault_flag; - param_get(accel_fault, &accel_fault_flag); - - if (accel_fault_flag == 1) - { - param_t accel_noise = param_find("SENS_ACCEL_NOISE"); - float_t accel_noise_flag; - param_get(accel_noise, &accel_noise_flag); - - if (abs(accel_noise_flag) > 0) - { - imu.delta_velocity[0] += imu.delta_velocity[0]*generate_wgn(accel_noise_flag); - imu.delta_velocity[1] += imu.delta_velocity[1]*generate_wgn(accel_noise_flag); - imu.delta_velocity[2] += imu.delta_velocity[2]*generate_wgn(accel_noise_flag); - } - - param_t accel_bias_shift = param_find("SENS_ACCEL_SHIF"); - float_t accel_bias_shift_flag; - param_get(accel_bias_shift, &accel_bias_shift_flag); - - if (abs(accel_bias_shift_flag) > 0) - { - imu.delta_velocity[0] += imu.delta_velocity[0]*accel_bias_shift_flag; - imu.delta_velocity[1] += imu.delta_velocity[1]*accel_bias_shift_flag; - imu.delta_velocity[2] += imu.delta_velocity[2]*accel_bias_shift_flag; - } - - param_t accel_bias_scale = param_find("SENS_ACCEL_SCAL"); - float_t accel_bias_scale_flag; - param_get(accel_bias_scale, &accel_bias_scale_flag); - - if (abs(accel_bias_scale_flag) > 0) - { - imu.delta_velocity[0] *= accel_bias_scale_flag; - imu.delta_velocity[1] *= accel_bias_scale_flag; - imu.delta_velocity[2] *= accel_bias_scale_flag; - } - - param_t accel_drift = param_find("SENS_ACCEL_DRIFT"); - float_t accel_drift_flag; - param_get(accel_drift, &accel_drift_flag); - - if (abs(accel_drift_flag) > 0) - { - imu.delta_velocity[0] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; - imu.delta_velocity[1] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; - imu.delta_velocity[2] += 0.01f*accel_drift_flag*accel_drift_timestep/1000000; - - accel_drift_timestep += 1; - } - - param_t accel_zero = param_find("SENS_ACCEL_ZERO"); - int32_t accel_zero_flag; - param_get(accel_zero, &accel_zero_flag); - - if (accel_zero_flag == 1) - { - imu.delta_velocity[0] = 0; - imu.delta_velocity[1] = 0; - imu.delta_velocity[2] = 0; - } - } - - // Adding faults to the gyroscope - param_t gyro_fault = param_find("SENS_GYRO_FAULT"); - int32_t gyro_fault_flag; - param_get(gyro_fault, &gyro_fault_flag); - - if (gyro_fault_flag == 1) - { - param_t gyro_noise = param_find("SENS_GYRO_NOISE"); - float_t gyro_noise_flag; - param_get(gyro_noise, &gyro_noise_flag); - - if (abs(gyro_noise_flag) > 0) - { - imu.delta_angle[0] += imu.delta_angle[0]*generate_wgn(gyro_noise_flag); - imu.delta_angle[1] += imu.delta_angle[0]*generate_wgn(gyro_noise_flag); - imu.delta_angle[2] += imu.delta_angle[0]*generate_wgn(gyro_noise_flag); - } - - param_t gyro_bias_shift = param_find("SENS_GYRO_SHIF"); - float_t gyro_bias_shift_flag; - param_get(gyro_bias_shift, &gyro_bias_shift_flag); - - if (abs(gyro_bias_shift_flag) > 0) - { - imu.delta_angle[0] += imu.delta_angle[0]*gyro_bias_shift_flag; - imu.delta_angle[1] += imu.delta_angle[1]*gyro_bias_shift_flag; - imu.delta_angle[2] += imu.delta_angle[2]*gyro_bias_shift_flag; - } - - param_t gyro_bias_scale = param_find("SENS_GYRO_SCAL"); - float_t gyro_bias_scale_flag; - param_get(gyro_bias_scale, &gyro_bias_scale_flag); - - if (abs(gyro_bias_scale_flag) > 0) - { - imu.delta_angle[0] *= gyro_bias_scale_flag; - imu.delta_angle[1] *= gyro_bias_scale_flag; - imu.delta_angle[2] *= gyro_bias_scale_flag; - } - - param_t gyro_drift = param_find("SENS_GYRO_DRIFT"); - float_t gyro_drift_flag; - param_get(gyro_drift, &gyro_drift_flag); - - if (abs(gyro_drift_flag) > 0) - { - imu.delta_angle[0] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; - imu.delta_angle[1] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; - imu.delta_angle[2] += 0.01f*gyro_drift_flag*gyro_drift_timestep/1000000; - - gyro_drift_timestep += 1; - } - - param_t gyro_zero = param_find("SENS_GYRO_ZERO"); - int32_t gyro_zero_flag; - param_get(gyro_zero, &gyro_zero_flag); - - if (gyro_zero_flag == 1) - { - imu.delta_angle[0] = 10000; - imu.delta_angle[1] = 10000; - imu.delta_angle[2] = 10000; - } - } - - imu.delta_velocity_clipping = _delta_velocity_clipping; - imu.accel_calibration_count = _accel_calibration.calibration_count(); - imu.gyro_calibration_count = _gyro_calibration.calibration_count(); - imu.timestamp = hrt_absolute_time(); - _vehicle_imu_pub.publish(imu); - - // reset clip counts - _delta_velocity_clipping = 0; - - // record gyro publication latency and integrated samples - _gyro_publish_latency_mean_us.update(imu.timestamp - _gyro_timestamp_last); - _gyro_update_latency_mean_us.update(imu.timestamp - _gyro_timestamp_sample_last); - - updated = true; - } - } - - return updated; -} - -void VehicleIMU::UpdateIntegratorConfiguration() -{ - if (_accel_mean_interval_us.valid() && _gyro_mean_interval_us.valid()) { - - const float accel_interval_us = _accel_mean_interval_us.mean(); - const float gyro_interval_us = _gyro_mean_interval_us.mean(); - - // determine number of sensor samples that will get closest to the desired integration interval - uint8_t gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us)); - - // if gyro samples exceeds queue depth, instead round to nearest even integer to improve scheduling options - if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { - gyro_integral_samples = math::max(1, (int)roundf(_imu_integration_interval_us / gyro_interval_us / 2) * 2); - } - - uint32_t integration_interval_us = roundf(gyro_integral_samples * gyro_interval_us); - - // accel follows gyro as closely as possible - uint8_t accel_integral_samples = math::max(1, (int)roundf(integration_interval_us / accel_interval_us)); - - // let the gyro set the configuration and scheduling - // relaxed minimum integration time required - _accel_integrator.set_reset_interval(roundf((accel_integral_samples - 0.5f) * accel_interval_us)); - _accel_integrator.set_reset_samples(accel_integral_samples); - - _gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * gyro_interval_us)); - _gyro_integrator.set_reset_samples(gyro_integral_samples); - - _backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * accel_interval_us, - sensor_gyro_s::ORB_QUEUE_LENGTH * gyro_interval_us) / 2, 1000, 20000); - - // gyro: find largest integer multiple of gyro_integral_samples - for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) { - if (gyro_integral_samples > sensor_gyro_s::ORB_QUEUE_LENGTH) { - gyro_integral_samples /= 2; - } - - if (gyro_integral_samples % n == 0) { - _sensor_gyro_sub.set_required_updates(n); - _sensor_gyro_sub.registerCallback(); - - _intervals_configured = true; - _update_integrator_config = false; - - PX4_DEBUG("accel (%" PRIu32 "), gyro (%" PRIu32 "), accel samples: %" PRIu8 ", gyro samples: %" PRIu8 - ", accel interval: %.1f, gyro interval: %.1f sub samples: %d", - _accel_calibration.device_id(), _gyro_calibration.device_id(), accel_integral_samples, gyro_integral_samples, - (double)accel_interval_us, (double)_gyro_interval_us, n); - - _accel_interval_us = accel_interval_us; - _gyro_interval_us = gyro_interval_us; - - break; - } - } - } -} - -void VehicleIMU::UpdateAccelVibrationMetrics(const Vector3f &acceleration) -{ - // Accel high frequency vibe = filtered length of (acceleration - acceleration_prev) - _status.accel_vibration_metric = 0.99f * _status.accel_vibration_metric - + 0.01f * Vector3f(acceleration - _acceleration_prev).norm(); - - _acceleration_prev = acceleration; -} - -void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity) -{ - // Gyro high frequency vibe = filtered length of (angular_velocity - angular_velocity_prev) - _status.gyro_vibration_metric = 0.99f * _status.gyro_vibration_metric - + 0.01f * Vector3f(angular_velocity - _angular_velocity_prev).norm(); - - _angular_velocity_prev = angular_velocity; -} - -void VehicleIMU::PrintStatus() -{ - PX4_INFO_RAW("[vehicle_imu] %" PRIu8 " - Accel: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro: %" PRIu32 - ", interval: %.1f us (SD %.1f us)\n", - _instance, - _accel_calibration.device_id(), (double)_accel_mean_interval_us.mean(), - (double)_accel_mean_interval_us.standard_deviation(), - _gyro_calibration.device_id(), (double)_gyro_mean_interval_us.mean(), - (double)_gyro_mean_interval_us.standard_deviation()); - -#if defined(DEBUG_BUILD) - PX4_INFO_RAW("[vehicle_imu] %" PRIu8 - " - gyro update sample latency: %.1f us (SD %.1f us), publish latency %.1f us (SD %.1f us)\n", - _instance, - (double)_gyro_update_latency_mean_us.mean(), (double)_gyro_update_latency_mean_us.standard_deviation(), - (double)_gyro_publish_latency_mean_us.mean(), (double)_gyro_publish_latency_mean_us.standard_deviation() - ); -#endif // DEBUG_BUILD - - perf_print_counter(_accel_generation_gap_perf); - perf_print_counter(_gyro_generation_gap_perf); - - _accel_calibration.PrintStatus(); - _gyro_calibration.PrintStatus(); -} - -void VehicleIMU::SensorCalibrationUpdate() -{ - for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) { - estimator_sensor_bias_s estimator_sensor_bias; - - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias) - && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) { - - // find corresponding accel bias - if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable - && (estimator_sensor_bias.accel_device_id != 0) - && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.accel_bias}; - - _accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias); - _accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance}; - _accel_learned_calibration[i].valid = true; - _accel_cal_available = true; - } - - // find corresponding gyro calibration - if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable - && (estimator_sensor_bias.gyro_device_id != 0) - && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) { - - const Vector3f bias{estimator_sensor_bias.gyro_bias}; - - _gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias); - _gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance}; - _gyro_learned_calibration[i].valid = true; - _gyro_cal_available = true; - } - } - } -} - -void VehicleIMU::SensorCalibrationSaveAccel() -{ - if (_accel_cal_available) { - const Vector3f cal_orig{_accel_calibration.offset()}; - bool initialised = false; - Vector3f offset_estimate{}; - Vector3f bias_variance{}; - - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_accel_learned_calibration[i].valid) { - if (!initialised) { - bias_variance = _accel_learned_calibration[i].bias_variance; - offset_estimate = _accel_learned_calibration[i].offset; - initialised = true; - - } else { - for (int axis_index = 0; axis_index < 3; axis_index++) { - const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index); - const float k1 = bias_variance(axis_index) / sum_of_variances; - const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances; - offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index); - bias_variance(axis_index) *= k2; - } - } - - // reset - _accel_learned_calibration[i] = {}; - } - } - - if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) { - if (_accel_calibration.set_offset(offset_estimate)) { - PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(), - (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), - (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - - // save parameters with preferred calibration slot to current sensor index - if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) { - param_notify_changes(); - } - - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - } - } - - // reset - _accel_cal_available = false; - } -} - -void VehicleIMU::SensorCalibrationSaveGyro() -{ - if (_gyro_cal_available) { - const Vector3f cal_orig{_gyro_calibration.offset()}; - bool initialised = false; - Vector3f offset_estimate{}; - Vector3f bias_variance{}; - - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if (_gyro_learned_calibration[i].valid) { - if (!initialised) { - bias_variance = _gyro_learned_calibration[i].bias_variance; - offset_estimate = _gyro_learned_calibration[i].offset; - initialised = true; - - } else { - for (int axis_index = 0; axis_index < 3; axis_index++) { - const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index); - const float k1 = bias_variance(axis_index) / sum_of_variances; - const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances; - offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index); - bias_variance(axis_index) *= k2; - } - } - - // reset - _gyro_learned_calibration[i] = {}; - } - } - - if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) { - if (_gyro_calibration.set_offset(offset_estimate)) { - PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])", - _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(), - (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2), - (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2)); - - // save parameters with preferred calibration slot to current sensor index - if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) { - param_notify_changes(); - } - - _in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US; - } - } - - // reset - _gyro_cal_available = false; - } -} - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.hpp deleted file mode 100644 index 257311c3de48..000000000000 --- a/src/modules/sensors/sensors/vehicle_imu/VehicleIMU.hpp +++ /dev/null @@ -1,207 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -namespace sensors -{ - -class VehicleIMU : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - VehicleIMU() = delete; - VehicleIMU(int instance, uint8_t accel_index, uint8_t gyro_index, const px4::wq_config_t &config); - - ~VehicleIMU() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - bool ParametersUpdate(bool force = false); - bool Publish(); - void Run() override; - - bool UpdateAccel(); - bool UpdateGyro(); - - void UpdateIntegratorConfiguration(); - - inline void UpdateAccelVibrationMetrics(const matrix::Vector3f &acceleration); - inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity); - - void SensorCalibrationUpdate(); - void SensorCalibrationSaveAccel(); - void SensorCalibrationSaveGyro(); - - // return the square of two floating point numbers - static constexpr float sq(float var) { return var * var; } - - uORB::PublicationMulti _vehicle_imu_pub{ORB_ID(vehicle_imu)}; - uORB::PublicationMulti _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)}; - - static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - // Used to check, save and use learned magnetometer biases - uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; - - uORB::Subscription _sensor_accel_sub; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub; - - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - - calibration::Accelerometer _accel_calibration{}; - calibration::Gyroscope _gyro_calibration{}; - - sensors::Integrator _accel_integrator{}; - sensors::IntegratorConing _gyro_integrator{}; - - uint32_t _imu_integration_interval_us{5000}; - - hrt_abstime _accel_timestamp_sample_last{0}; - hrt_abstime _gyro_timestamp_sample_last{0}; - hrt_abstime _gyro_timestamp_last{0}; - - math::WelfordMeanVector _raw_accel_mean{}; - math::WelfordMeanVector _raw_gyro_mean{}; - - math::WelfordMean _accel_mean_interval_us{}; - math::WelfordMean _accel_fifo_mean_interval_us{}; - - math::WelfordMean _gyro_mean_interval_us{}; - math::WelfordMean _gyro_fifo_mean_interval_us{}; - - math::WelfordMean _gyro_update_latency_mean_us{}; - math::WelfordMean _gyro_publish_latency_mean_us{}; - - float _accel_interval_us{NAN}; - float _gyro_interval_us{NAN}; - - unsigned _accel_last_generation{0}; - unsigned _gyro_last_generation{0}; - - float _accel_temperature_sum{NAN}; - float _gyro_temperature_sum{NAN}; - - int _accel_temperature_sum_count{0}; - int _gyro_temperature_sum_count{0}; - - matrix::Vector3f _acceleration_prev{}; // acceleration from the previous IMU measurement for vibration metrics - matrix::Vector3f _angular_velocity_prev{}; // angular velocity from the previous IMU measurement for vibration metrics - - vehicle_imu_status_s _status{}; - - float _coning_norm_accum{0}; - float _coning_norm_accum_total_time_s{0}; - - uint8_t _delta_angle_clipping{0}; - uint8_t _delta_velocity_clipping{0}; - - hrt_abstime _last_accel_clipping_notify_time{0}; - hrt_abstime _last_gyro_clipping_notify_time{0}; - - uint64_t _last_accel_clipping_notify_total_count{0}; - uint64_t _last_gyro_clipping_notify_total_count{0}; - - orb_advert_t _mavlink_log_pub{nullptr}; - - uint32_t _backup_schedule_timeout_us{20000}; - - bool _data_gap{false}; - bool _update_integrator_config{true}; - bool _intervals_configured{false}; - bool _publish_status{true}; - - const uint8_t _instance; - - bool _armed{false}; - - bool _accel_cal_available{false}; - bool _gyro_cal_available{false}; - - struct InFlightCalibration { - matrix::Vector3f offset{}; - matrix::Vector3f bias_variance{}; - bool valid{false}; - }; - - InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {}; - - static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s}; - - hrt_abstime _in_flight_calibration_check_timestamp_last{0}; - - perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")}; - perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")}; - - DEFINE_PARAMETERS( - (ParamInt) _param_imu_integ_rate, - (ParamBool) _param_sens_imu_autocal - ) - - int accel_drift_timestep, gyro_drift_timestep; -}; - -} // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_imu/imu_parameters.c b/src/modules/sensors/sensors/vehicle_imu/imu_parameters.c deleted file mode 100644 index ebe8a465a672..000000000000 --- a/src/modules/sensors/sensors/vehicle_imu/imu_parameters.c +++ /dev/null @@ -1,62 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020 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. - * - ****************************************************************************/ - -/** -* IMU integration rate. -* -* The rate at which raw IMU data is integrated to produce delta angles and delta velocities. -* Recommended to set this to a multiple of the estimator update period (currently 10 ms for ekf2). -* -* @min 100 -* @max 1000 -* @value 100 100 Hz -* @value 200 200 Hz -* @value 250 250 Hz -* @value 400 400 Hz -* @unit Hz -* @reboot_required true -* @group Sensors -*/ -PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200); - -/** - * IMU auto calibration - * - * Automatically initialize IMU (accel/gyro) calibration from bias estimates if available. - * - * @boolean - * - * @category system - * @group Sensors - */ -PARAM_DEFINE_INT32(SENS_IMU_AUTOCAL, 1); diff --git a/src/modules/sensors/sensors/vehicle_magnetometer/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_magnetometer/CMakeLists.txt deleted file mode 100644 index 0fad202f5019..000000000000 --- a/src/modules/sensors/sensors/vehicle_magnetometer/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 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_magnetometer - VehicleMagnetometer.cpp - VehicleMagnetometer.hpp -) - -target_link_libraries(vehicle_magnetometer - PRIVATE - data_validator - px4_work_queue - sensor_calibration -) diff --git a/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp deleted file mode 100644 index 97e37e6d1a4f..000000000000 --- a/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ /dev/null @@ -1,768 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "VehicleMagnetometer.hpp" - -#include -#include -#include -#include - -namespace sensors -{ - -using namespace matrix; - -static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; - -VehicleMagnetometer::VehicleMagnetometer() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - param_find("SENS_MAG_SIDES"); - param_find("SENS_MAG_AUTOROT"); - - _voter.set_timeout(SENSOR_TIMEOUT); - _voter.set_equal_value_threshold(1000); - - ParametersUpdate(true); - - _vehicle_magnetometer_pub[0].advertise(); - _sensor_preflight_mag_pub.advertise(); - - // if publishing multiple mags advertise instances immediately for existing calibrations - if (!_param_sens_mag_mode.get()) { - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i); - - if (device_id_mag != 0) { - _vehicle_magnetometer_pub[i].advertise(); - } - } - } - -} - -VehicleMagnetometer::~VehicleMagnetometer() -{ - Stop(); - perf_free(_cycle_perf); -} - -bool VehicleMagnetometer::Start() -{ - ScheduleNow(); - return true; -} - -void VehicleMagnetometer::Stop() -{ - Deinit(); - - // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } -} - -bool VehicleMagnetometer::ParametersUpdate(bool force) -{ - // Check if parameters have changed - if (_parameter_update_sub.updated() || force) { - // clear update - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - updateParams(); - - // Legacy QGC support: CAL_MAG_SIDES required to display the correct UI - // Force it to be a copy of the new SENS_MAG_SIDES - if (_param_cal_mag_sides.get() != _param_sens_mag_sides.get()) { - _param_cal_mag_sides.set(_param_sens_mag_sides.get()); - _param_cal_mag_sides.commit(); - } - - // Mag compensation type - MagCompensationType mag_comp_typ = static_cast(_param_mag_comp_typ.get()); - - if (mag_comp_typ != _mag_comp_type) { - // check mag power compensation type (change battery current subscription instance if necessary) - switch (mag_comp_typ) { - case MagCompensationType::Current_inst0: - _battery_status_sub.ChangeInstance(0); - break; - - case MagCompensationType::Current_inst1: - _battery_status_sub.ChangeInstance(1); - break; - - case MagCompensationType::Throttle: - break; - - default: - - // ensure power compensation is disabled - for (auto &cal : _calibration) { - cal.UpdatePower(0.f); - } - - break; - } - } - - _mag_comp_type = mag_comp_typ; - - - if (!_armed) { - bool calibration_updated = false; - - // update mag priority (CAL_MAGx_PRIO) - for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) { - bool clear_online_mag_cal = false; - - const auto calibration_count = _calibration[mag].calibration_count(); - const int32_t priority_old = _calibration[mag].priority(); - _calibration[mag].ParametersUpdate(); - const int32_t priority_new = _calibration[mag].priority(); - - if (_calibration[mag].enabled()) { - if (priority_old != priority_new) { - if (_priority[mag] == priority_old) { - _priority[mag] = priority_new; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = priority_new - priority_old; - _priority[mag] = math::constrain(_priority[mag] + priority_change, 1, 100); - } - } - - if (calibration_count != _calibration[mag].calibration_count()) { - calibration_updated = true; - clear_online_mag_cal = true; - } - - } else { - _priority[mag] = 0; - clear_online_mag_cal = true; - } - - if (clear_online_mag_cal) { - // clear any mag bias estimate - _calibration_estimator_bias[mag].zero(); - - for (auto &cal : _mag_cal) { - // clear any in flight mag calibration - if (cal.device_id == _calibration[mag].device_id()) { - cal = {}; - } - } - } - } - - if (calibration_updated) { - _last_calibration_update = hrt_absolute_time(); - } - } - - return true; - } - - return false; -} - -void VehicleMagnetometer::UpdateMagBiasEstimate() -{ - if (_magnetometer_bias_estimate_sub.updated()) { - // Continuous mag calibration is running when not armed - magnetometer_bias_estimate_s mag_bias_est; - - if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) { - bool parameters_notify = false; - - for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) { - - const Vector3f bias{mag_bias_est.bias_x[mag_index], - mag_bias_est.bias_y[mag_index], - mag_bias_est.bias_z[mag_index]}; - - _calibration_estimator_bias[mag_index] = bias; - - // set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available - if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index] - && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) { - - const Vector3f old_offset = _calibration[mag_index].offset(); - - // set initial mag calibration - const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]); - - if (_calibration[mag_index].set_offset(offset)) { - // save parameters with preferred calibration slot to current sensor index - _calibration[mag_index].ParametersSave(mag_index); - - PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]", - mag_index, _calibration[mag_index].device_id(), - (double)old_offset(0), (double)old_offset(1), (double)old_offset(2), - (double)_calibration[mag_index].offset()(0), - (double)_calibration[mag_index].offset()(1), - (double)_calibration[mag_index].offset()(2)); - - _calibration_estimator_bias[mag_index].zero(); - - parameters_notify = true; - } - } - } - } - - if (parameters_notify) { - param_notify_changes(); - _last_calibration_update = hrt_absolute_time(); - } - } - } -} - -void VehicleMagnetometer::UpdateMagCalibration() -{ - // State variance assumed for magnetometer bias storage. - // This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value. - // Larger values cause a larger fraction of the learned biases to be used. - static constexpr float magb_vref = 2.5e-6f; - static constexpr float min_var_allowed = magb_vref * 0.01f; - static constexpr float max_var_allowed = magb_vref * 500.f; - - if (_armed) { - static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]); - - for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) { - estimator_sensor_bias_s estimator_sensor_bias; - - if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) { - - const Vector3f bias{estimator_sensor_bias.mag_bias}; - const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; - - const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) - && (estimator_sensor_bias.mag_device_id != 0) - && estimator_sensor_bias.mag_bias_valid - && estimator_sensor_bias.mag_bias_stable - && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); - - if (valid) { - // find corresponding mag calibration - for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) { - - _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; - - // readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias) - const Vector3f mag_cal_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + - _calibration_estimator_bias[mag_index]); - - if ((mag_cal_offset - _mag_cal[i].offset).longerThan(0.001f)) { - const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; - - PX4_DEBUG("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", - mag_index, _calibration[mag_index].device_id(), i, - (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), - (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), - (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); - } - - _mag_cal[i].offset = mag_cal_offset; - _mag_cal[i].variance = bias_variance; - - _in_flight_mag_cal_available = true; - break; - } - } - } - } - } - - } else if (_in_flight_mag_cal_available) { - // not armed and mag cal available - bool calibration_param_save_needed = false; - // iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme - Vector3f state_variance{magb_vref, magb_vref, magb_vref}; - - for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { - // apply all valid saved offsets - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) { - const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; - - // calculate weighting using ratio of variances and update stored bias values - const Vector3f &observation{_mag_cal[i].offset}; - const Vector3f &obs_variance{_mag_cal[i].variance}; - - const Vector3f innovation{mag_cal_orig - observation}; - const Vector3f innovation_variance{state_variance + obs_variance}; - const Vector3f kalman_gain{state_variance.edivide(innovation_variance)}; - - // new offset - const Vector3f mag_cal_offset{mag_cal_orig - innovation.emult(kalman_gain)}; - - for (int axis_index = 0; axis_index < 3; axis_index++) { - state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f); - } - - if (_calibration[mag_index].set_offset(mag_cal_offset)) { - - PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])", - mag_index, _calibration[mag_index].device_id(), i, - (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), - (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), - (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); - - _calibration[mag_index].ParametersSave(); - - _calibration_estimator_bias[mag_index].zero(); - - calibration_param_save_needed = true; - - } else { - // new offset not saved - PX4_DEBUG("%d (%" PRIu32 ") EST:%d rejected: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])", - mag_index, _calibration[mag_index].device_id(), i, - (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), - (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), - (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); - } - } - } - } - - if (calibration_param_save_needed) { - param_notify_changes(); - _last_calibration_update = hrt_absolute_time(); - } - - // clear all - for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { - _mag_cal[i] = {}; - } - - _in_flight_mag_cal_available = false; - } -} - -void VehicleMagnetometer::UpdatePowerCompensation() -{ - if (_mag_comp_type != MagCompensationType::Disabled) { - // update power signal for mag compensation - if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) { - vehicle_thrust_setpoint_s vehicle_thrust_setpoint; - - if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) { - const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - _calibration[i].UpdatePower(thrust_setpoint.length()); - } - } - - } else if ((_mag_comp_type == MagCompensationType::Current_inst0) - || (_mag_comp_type == MagCompensationType::Current_inst1)) { - - battery_status_s bat_stat; - - if (_battery_status_sub.update(&bat_stat)) { - float power = bat_stat.current_a * 0.001f; // current in [kA] - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - _calibration[i].UpdatePower(power); - } - } - - } else { - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - _calibration[i].UpdatePower(0.f); - } - } - } -} - -void VehicleMagnetometer::Run() -{ - perf_begin(_cycle_perf); - - const hrt_abstime time_now_us = hrt_absolute_time(); - - const bool parameter_update = ParametersUpdate(); - - // check vehicle status for changes to armed state - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _armed = vehicle_control_mode.flag_armed; - } - } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - _calibration[i].SensorCorrectionsUpdate(); - } - - UpdatePowerCompensation(); - - UpdateMagBiasEstimate(); - - bool updated[MAX_SENSOR_COUNT] {}; - - for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - - const bool was_advertised = _advertised[uorb_index]; - - if (!_advertised[uorb_index]) { - // use data's timestamp to throttle advertisement checks - if ((_last_publication_timestamp[uorb_index] == 0) - || (time_now_us > _last_publication_timestamp[uorb_index] + 1_s)) { - - if (_sensor_sub[uorb_index].advertised()) { - _advertised[uorb_index] = true; - - } else { - _last_publication_timestamp[uorb_index] = time_now_us; - } - } - } - - if (_advertised[uorb_index]) { - sensor_mag_s report; - - while (_sensor_sub[uorb_index].update(&report)) { - - if (_calibration[uorb_index].device_id() != report.device_id) { - _calibration[uorb_index].set_device_id(report.device_id); - _priority[uorb_index] = _calibration[uorb_index].priority(); - } - - if (_calibration[uorb_index].enabled()) { - - if (!was_advertised) { - if (uorb_index > 0) { - /* the first always exists, but for each further sensor, add a new validator */ - if (!_voter.add_new_validator()) { - PX4_ERR("failed to add validator for %s %i", _calibration[uorb_index].SensorString(), uorb_index); - } - } - - if (_selected_sensor_sub_index < 0) { - _sensor_sub[uorb_index].registerCallback(); - } - - ParametersUpdate(true); - } - - const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}) - _calibration_estimator_bias[uorb_index]}; - - float mag_array[3] {vect(0), vect(1), vect(2)}; - _voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]); - - _timestamp_sample_sum[uorb_index] += report.timestamp_sample; - _data_sum[uorb_index] += vect; - _data_sum_count[uorb_index]++; - - _last_data[uorb_index] = vect; - - updated[uorb_index] = true; - } - } - } - } - - // check for the current best sensor - int best_index = 0; - _voter.get_best(time_now_us, &best_index); - - if (best_index >= 0) { - // handle selection change (don't process on same iteration as parameter update) - if ((_selected_sensor_sub_index != best_index) && !parameter_update) { - // clear all registered callbacks - for (auto &sub : _sensor_sub) { - sub.unregisterCallback(); - } - - if (_param_sens_mag_mode.get()) { - if (_selected_sensor_sub_index >= 0) { - PX4_INFO("%s switch from #%" PRId8 " -> #%d", _calibration[_selected_sensor_sub_index].SensorString(), - _selected_sensor_sub_index, best_index); - } - } - - _selected_sensor_sub_index = best_index; - _sensor_sub[_selected_sensor_sub_index].registerCallback(); - } - } - - // Publish - if (_param_sens_mag_rate.get() > 0) { - int interval_us = 1e6f / _param_sens_mag_rate.get(); - const bool multi_mode = (_param_sens_mag_mode.get() == 0); - - for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) { - if (updated[instance] && (_data_sum_count[instance] > 0)) { - - const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _data_sum_count[instance]; - - if (timestamp_sample >= _last_publication_timestamp[instance] + interval_us) { - - bool publish = (time_now_us <= timestamp_sample + 1_s); - - if (!multi_mode && publish) { - publish = (_selected_sensor_sub_index >= 0) - && (instance == _selected_sensor_sub_index) - && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR); - } - - if (publish) { - Vector3f magnetometer_data = _data_sum[instance] / _data_sum_count[instance]; - - // populate vehicle_magnetometer and publish - vehicle_magnetometer_s out{}; - out.timestamp_sample = timestamp_sample; - out.device_id = _calibration[instance].device_id(); - magnetometer_data.copyTo(out.magnetometer_ga); - - - out.calibration_count = _calibration[instance].calibration_count(); - out.timestamp = hrt_absolute_time(); - - if (multi_mode) { - - if (!_vehicle_magnetometer_pub[instance].advertised()) { - // prefer to maintain vehicle_magneometer instance numbering in sensor order - for (int mag_instance = 0; mag_instance < instance; mag_instance++) { - if (_calibration[instance].enabled()) { - _vehicle_magnetometer_pub[mag_instance].advertise(); - } - } - } - - _vehicle_magnetometer_pub[instance].publish(out); - - } else { - // otherwise only ever publish the first instance - _vehicle_magnetometer_pub[0].publish(out); - } - } - - _last_publication_timestamp[instance] = timestamp_sample; - - // reset - _timestamp_sample_sum[instance] = 0; - _data_sum[instance].zero(); - _data_sum_count[instance] = 0; - } - } - } - } - - if (!parameter_update && _param_sens_mag_mode.get()) { - CheckFailover(time_now_us); - } - - if (!_armed) { - calcMagInconsistency(); - } - - UpdateMagCalibration(); - - UpdateStatus(); - - // reschedule timeout - ScheduleDelayed(50_ms); - - perf_end(_cycle_perf); -} - -void VehicleMagnetometer::CheckFailover(const hrt_abstime &time_now_us) -{ - // check failover and report (save failover report for a cycle where parameters didn't update) - if (_last_failover_count != _voter.failover_count()) { - uint32_t flags = _voter.failover_state(); - int failover_index = _voter.failover_index(); - - if (flags != DataValidator::ERROR_FLAG_NO_ERROR) { - if (failover_index >= 0 && failover_index < MAX_SENSOR_COUNT) { - - if (time_now_us > _last_error_message + 3_s) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t", - _calibration[failover_index].SensorString(), - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); - - events::px4::enums::sensor_failover_reason_t failover_reason{}; - - if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } - - if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } - - if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } - - /* EVENT - * @description - * Land immediately and check the system. - */ - events::send( - events::ID("sensor_failover_mag"), events::Log::Emergency, "Magnetometer sensor #{1} failure: {2}", failover_index, - failover_reason); - - _last_error_message = time_now_us; - } - - // reduce priority of failed sensor to the minimum - _priority[failover_index] = 1; - } - } - - _last_failover_count = _voter.failover_count(); - } -} - -void VehicleMagnetometer::calcMagInconsistency() -{ - if (_selected_sensor_sub_index >= 0) { - sensor_preflight_mag_s preflt{}; - - const Vector3f primary_mag(_last_data[_selected_sensor_sub_index]); // primary mag field vector - - float mag_angle_diff_max = 0.0f; // the maximum angle difference - unsigned check_index = 0; // the number of sensors the primary has been checked against - - // Check each sensor against the primary - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - // check that the sensor we are checking against is not the same as the primary - if (_advertised[i] && (_priority[i] > 0) && (i != _selected_sensor_sub_index)) { - // calculate angle to 3D magnetic field vector of the primary sensor - Vector3f current_mag{_last_data[i]}; - - float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle(); - - // complementary filter to not fail/pass on single outliers - _mag_angle_diff[check_index] *= 0.95f; - _mag_angle_diff[check_index] += 0.05f * angle_error; - - mag_angle_diff_max = math::max(mag_angle_diff_max, _mag_angle_diff[check_index]); - - // increment the check index - check_index++; - } - - // check to see if the maximum number of checks has been reached and break - if (check_index >= 2) { - break; - } - } - - // get the vector length of the largest difference and write to the combined sensor struct - // will be zero if there is only one magnetometer and hence nothing to compare - preflt.mag_inconsistency_angle = mag_angle_diff_max; - - preflt.timestamp = hrt_absolute_time(); - _sensor_preflight_mag_pub.publish(preflt); - } -} - -void VehicleMagnetometer::UpdateStatus() -{ - if (_selected_sensor_sub_index >= 0) { - sensors_status_s sensors_status{}; - sensors_status.device_id_primary = _calibration[_selected_sensor_sub_index].device_id(); - - matrix::Vector3f mean{}; - int sensor_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_calibration[sensor_index].device_id() != 0) && (_calibration[sensor_index].enabled())) { - sensor_count++; - mean += _last_data[sensor_index]; - } - } - - if (sensor_count > 0) { - mean /= sensor_count; - } - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if (_calibration[sensor_index].device_id() != 0) { - - _sensor_diff[sensor_index] = 0.95f * _sensor_diff[sensor_index] + 0.05f * (_last_data[sensor_index] - mean); - - sensors_status.device_ids[sensor_index] = _calibration[sensor_index].device_id(); - sensors_status.inconsistency[sensor_index] = _sensor_diff[sensor_index].norm(); - sensors_status.healthy[sensor_index] = (_voter.get_sensor_state(sensor_index) == DataValidator::ERROR_FLAG_NO_ERROR); - sensors_status.priority[sensor_index] = _voter.get_sensor_priority(sensor_index); - sensors_status.enabled[sensor_index] = _calibration[sensor_index].enabled(); - sensors_status.external[sensor_index] = _calibration[sensor_index].external(); - - } else { - sensors_status.inconsistency[sensor_index] = NAN; - } - } - - sensors_status.timestamp = hrt_absolute_time(); - _sensors_status_mag_pub.publish(sensors_status); - } -} - -void VehicleMagnetometer::PrintStatus() -{ - if (_selected_sensor_sub_index >= 0) { - PX4_INFO_RAW("[vehicle_magnetometer] selected %s: %" PRIu32 " (%" PRId8 ")\n", - _calibration[_selected_sensor_sub_index].SensorString(), - _calibration[_selected_sensor_sub_index].device_id(), _selected_sensor_sub_index); - } - - _voter.print(); - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if (_advertised[i] && (_priority[i] > 0)) { - _calibration[i].PrintStatus(); - } - } -} - -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp deleted file mode 100644 index 39105938a65c..000000000000 --- a/src/modules/sensors/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ /dev/null @@ -1,188 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2020-2022 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 "data_validator/DataValidatorGroup.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace time_literals; - -namespace sensors -{ -class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - - VehicleMagnetometer(); - ~VehicleMagnetometer() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - void Run() override; - - void CheckFailover(const hrt_abstime &time_now_us); - bool ParametersUpdate(bool force = false); - void UpdateStatus(); - - void Publish(uint8_t instance, bool multi = false); - - /** - * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers - */ - void calcMagInconsistency(); - - void UpdateMagBiasEstimate(); - void UpdateMagCalibration(); - void UpdatePowerCompensation(); - - static constexpr int MAX_SENSOR_COUNT = 4; - - uORB::Publication _sensors_status_mag_pub{ORB_ID(sensors_status_mag)}; - - uORB::Publication _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)}; - - uORB::PublicationMulti _vehicle_magnetometer_pub[MAX_SENSOR_COUNT] { - {ORB_ID(vehicle_magnetometer)}, - {ORB_ID(vehicle_magnetometer)}, - {ORB_ID(vehicle_magnetometer)}, - {ORB_ID(vehicle_magnetometer)}, - }; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; - uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; - uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - - // Used to check, save and use learned magnetometer biases - uORB::SubscriptionMultiArray _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias}; - - bool _in_flight_mag_cal_available{false}; ///< from navigation filter - - struct MagCal { - uint32_t device_id{0}; - matrix::Vector3f offset{}; - matrix::Vector3f variance{}; - } _mag_cal[ORB_MULTI_MAX_INSTANCES] {}; - - uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(sensor_mag), 0}, - {this, ORB_ID(sensor_mag), 1}, - {this, ORB_ID(sensor_mag), 2}, - {this, ORB_ID(sensor_mag), 3} - }; - - hrt_abstime _last_calibration_update{0}; - - matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {}; - - calibration::Magnetometer _calibration[MAX_SENSOR_COUNT]; - - // Magnetometer interference compensation - enum class MagCompensationType { - Disabled = 0, - Throttle, - Current_inst0, - Current_inst1 - }; - - MagCompensationType _mag_comp_type{MagCompensationType::Disabled}; - - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - hrt_abstime _last_error_message{0}; - orb_advert_t _mavlink_log_pub{nullptr}; - - DataValidatorGroup _voter{1}; - unsigned _last_failover_count{0}; - - uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {}; - matrix::Vector3f _data_sum[MAX_SENSOR_COUNT] {}; - - int _data_sum_count[MAX_SENSOR_COUNT] {}; - hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; - - matrix::Vector3f _last_data[MAX_SENSOR_COUNT] {}; - bool _advertised[MAX_SENSOR_COUNT] {}; - - matrix::Vector3f _sensor_diff[MAX_SENSOR_COUNT] {}; // filtered differences between sensor instances - float _mag_angle_diff[2] {}; /**< filtered mag angle differences between sensor instances (Ga) */ - - uint8_t _priority[MAX_SENSOR_COUNT] {}; - - int8_t _selected_sensor_sub_index{-1}; - - bool _armed{false}; - - DEFINE_PARAMETERS( - (ParamInt) _param_mag_comp_typ, - (ParamBool) _param_sens_mag_mode, - (ParamFloat) _param_sens_mag_rate, - (ParamBool) _param_sens_mag_autocal, - (ParamInt) _param_cal_mag_sides, - (ParamInt) _param_sens_mag_sides - ) - -}; -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py b/src/modules/sensors/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py deleted file mode 100644 index 91319c7d1f44..000000000000 --- a/src/modules/sensors/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py +++ /dev/null @@ -1,272 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -""" -File: mag_compensation.py -Author: Tanja Baumann -Email: tanja@auterion.com -Github: https://github.com/baumanta -Description: - Computes linear coefficients for mag compensation from thrust and current -Usage: - python mag_compensation.py /path/to/log/logfile.ulg current --instance 1 - -Remark: - If your logfile does not contain some of the topics, e.g.battery_status/current_a - you will have to comment out the corresponding parts in the script -""" - - -import matplotlib.pylab as plt -from mpl_toolkits.mplot3d import Axes3D -from pyulog import ULog -from pyulog.px4 import PX4ULog -from pylab import * -import numpy as np -import textwrap as tw -import argparse - -#arguments -parser = argparse.ArgumentParser(description='Calculate compensation parameters from ulog') - -parser.add_argument('logfile', type=str, nargs='?', default=[], - help='full path to ulog file') -parser.add_argument('type', type=str, nargs='?', choices=['current', 'thrust'], default=[], - help='Power signal used for compensation, supported is "current" or "thrust".') -parser.add_argument('--instance', type=int, nargs='?', default=0, - help='instance of the current or thrust signal to use (0 or 1)') - -args = parser.parse_args() -log_name = args.logfile -comp_type = args.type -comp_instance = args.instance - -#Load the log data (produced by pyulog) -log = ULog(log_name) -pxlog = PX4ULog(log) - -def get_data(topic_name, variable_name, index): - try: - dataset = log.get_dataset(topic_name, index) - return dataset.data[variable_name] - except: - return [] - -def ms2s_list(time_ms_list): - if len(time_ms_list) > 0: - return 1e-6 * time_ms_list - else: - return time_ms_list - -# Select msgs and copy into arrays -armed = get_data('vehicle_status', 'arming_state', 0) -t_armed = ms2s_list(get_data('vehicle_status', 'timestamp', 0)) - -if comp_type == "thrust": - power = get_data('vehicle_rates_setpoint', 'thrust_body[2]', comp_instance) - power_t = ms2s_list(get_data('vehicle_rates_setpoint', 'timestamp', comp_instance)) - comp_type_param = 1 - factor = 1 - unit = "[G]" -elif comp_type == "current": - power = get_data('battery_status', 'current_a', comp_instance) - power = np.true_divide(power, 1000) #kA - power_t = ms2s_list(get_data('battery_status', 'timestamp', comp_instance)) - comp_type_param = 2 + comp_instance - factor = -1 - unit = "[G/kA]" -else: - print("unknown compensation type {}. Supported is either 'thrust' or 'current'.".format(comp_type)) - sys.exit(1) - -if len(power) == 0: - print("could not retrieve power signal from log, zero data points") - sys.exit(1) - -mag0X_body = get_data('sensor_mag', 'x', 0) -mag0Y_body = get_data('sensor_mag', 'y', 0) -mag0Z_body = get_data('sensor_mag', 'z', 0) -t_mag0 = ms2s_list(get_data('sensor_mag', 'timestamp', 0)) -mag0_ID = get_data('sensor_mag', 'device_id', 0) - -mag1X_body = get_data('sensor_mag', 'x', 1) -mag1Y_body = get_data('sensor_mag', 'y', 1) -mag1Z_body = get_data('sensor_mag', 'z', 1) -t_mag1 = ms2s_list(get_data('sensor_mag', 'timestamp', 1)) -mag1_ID = get_data('sensor_mag', 'device_id', 1) - -mag2X_body = get_data('sensor_mag', 'x', 2) -mag2Y_body = get_data('sensor_mag', 'y', 2) -mag2Z_body = get_data('sensor_mag', 'z', 2) -t_mag2 = ms2s_list(get_data('sensor_mag', 'timestamp', 2)) -mag2_ID = get_data('sensor_mag', 'device_id', 2) - -mag3X_body = get_data('sensor_mag', 'x', 3) -mag3Y_body = get_data('sensor_mag', 'y', 3) -mag3Z_body = get_data('sensor_mag', 'z', 3) -t_mag3 = ms2s_list(get_data('sensor_mag', 'timestamp', 3)) -mag3_ID = get_data('sensor_mag', 'device_id', 3) - -magX_body = [] -magY_body = [] -magZ_body = [] -mag_id = [] -t_mag = [] - -if len(mag0X_body) > 0: - magX_body.append(mag0X_body) - magY_body.append(mag0Y_body) - magZ_body.append(mag0Z_body) - t_mag.append(t_mag0) - mag_id.append(mag0_ID[0]) - -if len(mag1X_body) > 0: - magX_body.append(mag1X_body) - magY_body.append(mag1Y_body) - magZ_body.append(mag1Z_body) - t_mag.append(t_mag1) - mag_id.append(mag1_ID[0]) - -if len(mag2X_body) > 0: - magX_body.append(mag2X_body) - magY_body.append(mag2Y_body) - magZ_body.append(mag2Z_body) - t_mag.append(t_mag2) - mag_id.append(mag2_ID[0]) - -if len(mag3X_body) > 0: - magX_body.append(mag3X_body) - magY_body.append(mag3Y_body) - magZ_body.append(mag3Z_body) - t_mag.append(t_mag3) - mag_id.append(mag3_ID[0]) - -n_mag = len(magX_body) - -#log index does not necessarily match mag calibration instance number -calibration_instance = [] -instance_found = False -for idx in range(n_mag): - instance_found = False - for j in range(4): - if mag_id[idx] == log.initial_parameters["CAL_MAG{}_ID".format(j)]: - calibration_instance.append(j) - instance_found = True - if not instance_found: - print('Mag {} calibration instance not found, run compass calibration first.'.format(mag_id[idx])) - -#get first arming sequence from data -start_time = 0 -stop_time = 0 -for i in range(len(armed)-1): - if armed[i] == 1 and armed[i+1] == 2: - start_time = t_armed[i+1] - if armed[i] == 2 and armed[i+1] == 1: - stop_time = t_armed[i+1] - break - -#cut unarmed sequences from mag data -index_start = 0 -index_stop = 0 - -for idx in range(n_mag): - for i in range(len(t_mag[idx])): - if t_mag[idx][i] > start_time: - index_start = i - break - - for i in range(len(t_mag[idx])): - if t_mag[idx][i] > stop_time: - index_stop = i -1 - break - - t_mag[idx] = t_mag[idx][index_start:index_stop] - magX_body[idx] = magX_body[idx][index_start:index_stop] - magY_body[idx] = magY_body[idx][index_start:index_stop] - magZ_body[idx] = magZ_body[idx][index_start:index_stop] - - -#resample data -power_resampled = [] - -for idx in range(n_mag): - power_resampled.append(interp(t_mag[idx], power_t, power)) - -#fit linear to get coefficients -px = [] -py = [] -pz = [] - -for idx in range(n_mag): - px_temp, res_x, _, _, _ = polyfit(power_resampled[idx], magX_body[idx], 1,full = True) - py_temp, res_y, _, _, _ = polyfit(power_resampled[idx], magY_body[idx], 1,full = True) - pz_temp, res_z, _, _, _ = polyfit(power_resampled[idx], magZ_body[idx], 1, full = True) - - px.append(px_temp) - py.append(py_temp) - pz.append(pz_temp) - -#print to console -for idx in range(n_mag): - print('Mag{} device ID {} (calibration instance {})'.format(idx, mag_id[idx], calibration_instance[idx])) -print('\033[91m \n{}-based compensation: \033[0m'.format(comp_type)) -print('\nparam set CAL_MAG_COMP_TYP {}'.format(comp_type_param)) -for idx in range(n_mag): - print('\nparam set CAL_MAG{}_XCOMP {:.3f}'.format(calibration_instance[idx], factor * px[idx][0])) - print('param set CAL_MAG{}_YCOMP {:.3f}'.format(calibration_instance[idx], factor * py[idx][0])) - print('param set CAL_MAG{}_ZCOMP {:.3f}'.format(calibration_instance[idx], factor * pz[idx][0])) - -#plot data - -for idx in range(n_mag): - fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k') - fig.suptitle('Compensation Parameter Fit \n{} \nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold') - - plt.subplot(1,3,1) - plt.plot(power_resampled[idx], magX_body[idx], 'yo', power_resampled[idx], px[idx][0]*power_resampled[idx]+px[idx][1], '--k') - plt.xlabel('current [kA]') - plt.ylabel('mag X [G]') - - plt.subplot(1,3,2) - plt.plot(power_resampled[idx], magY_body[idx], 'yo', power_resampled[idx], py[idx][0]*power_resampled[idx]+py[idx][1], '--k') - plt.xlabel('current [kA]') - plt.ylabel('mag Y [G]') - - plt.subplot(1,3,3) - plt.plot(power_resampled[idx], magZ_body[idx], 'yo', power_resampled[idx], pz[idx][0]*power_resampled[idx]+pz[idx][1], '--k') - plt.xlabel('current [kA]') - plt.ylabel('mag Z [G]') - - # display results - plt.figtext(0.24, 0.03, 'CAL_MAG{}_XCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * px[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') - plt.figtext(0.51, 0.03, 'CAL_MAG{}_YCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * py[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') - plt.figtext(0.79, 0.03, 'CAL_MAG{}_ZCOMP: {:.3f} {}'.format(calibration_instance[idx],factor * pz[idx][0],unit), horizontalalignment='center', fontsize=12, multialignment='left', bbox=dict(boxstyle="round", facecolor='#D8D8D8', ec="0.5", pad=0.5, alpha=1), fontweight='bold') - - -#compensation comparison plots -for idx in range(n_mag): - fig = plt.figure(num=None, figsize=(25, 14), dpi=80, facecolor='w', edgecolor='k') - fig.suptitle('Original Data vs. Compensation \n{}\nmag {} ID: {} (calibration instance {})'.format(log_name, idx, mag_id[idx], calibration_instance[idx]), fontsize=14, fontweight='bold') - - plt.subplot(3,1,1) - original_x, = plt.plot(t_mag[idx], magX_body[idx], label='original') - power_x, = plt.plot(t_mag[idx],magX_body[idx] - px[idx][0] * power_resampled[idx], label='compensated') - plt.legend(handles=[original_x, power_x]) - plt.xlabel('Time [s]') - plt.ylabel('Mag X corrected[G]') - - plt.subplot(3,1,2) - original_y, = plt.plot(t_mag[idx], magY_body[idx], label='original') - power_y, = plt.plot(t_mag[idx],magY_body[idx] - py[idx][0] * power_resampled[idx], label='compensated') - plt.legend(handles=[original_y, power_y]) - plt.xlabel('Time [s]') - plt.ylabel('Mag Y corrected[G]') - - plt.subplot(3,1,3) - original_z, = plt.plot(t_mag[idx], magZ_body[idx], label='original') - power_z, = plt.plot(t_mag[idx],magZ_body[idx] - pz[idx][0] * power_resampled[idx], label='compensated') - plt.legend(handles=[original_z, power_z]) - plt.xlabel('Time [s]') - plt.ylabel('Mag Z corrected[G]') - -plt.show() diff --git a/src/modules/sensors/sensors/vehicle_optical_flow/CMakeLists.txt b/src/modules/sensors/sensors/vehicle_optical_flow/CMakeLists.txt deleted file mode 100644 index ba8761aa0023..000000000000 --- a/src/modules/sensors/sensors/vehicle_optical_flow/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -############################################################################ -# -# Copyright (c) 2020 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_optical_flow - VehicleOpticalFlow.cpp - VehicleOpticalFlow.hpp -) -target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue) diff --git a/src/modules/sensors/sensors/vehicle_optical_flow/RingBuffer.hpp b/src/modules/sensors/sensors/vehicle_optical_flow/RingBuffer.hpp deleted file mode 100644 index 591088bc3019..000000000000 --- a/src/modules/sensors/sensors/vehicle_optical_flow/RingBuffer.hpp +++ /dev/null @@ -1,217 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2022 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. - * - ****************************************************************************/ - -/** - * @file RingBuffer.hpp - * Template RingBuffer - */ - -#include -#include -#include - -template -class RingBuffer -{ -public: - void push(const T &sample) - { - uint8_t head_new = _head; - - if (!_first_write) { - head_new = (_head + 1) % SIZE; - } - - _buffer[head_new] = sample; - _head = head_new; - - // move tail if we overwrite it - if (_head == _tail && !_first_write) { - _tail = (_tail + 1) % SIZE; - - } else { - _first_write = false; - } - } - - uint8_t get_length() const { return SIZE; } - - T &operator[](const uint8_t index) { return _buffer[index]; } - - const T &get_newest() const { return _buffer[_head]; } - const T &get_oldest() const { return _buffer[_tail]; } - - uint8_t get_oldest_index() const { return _tail; } - - bool peak_first_older_than(const uint64_t ×tamp, T *sample) - { - // start looking from newest observation data - for (uint8_t i = 0; i < SIZE; i++) { - int index = (_head - i); - index = index < 0 ? SIZE + index : index; - - if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { - *sample = _buffer[index]; - return true; - } - - if (index == _tail) { - // we have reached the tail and haven't got a - // match - return false; - } - } - - return false; - } - - bool pop_first_older_than(const uint64_t ×tamp, T *sample) - { - // start looking from newest observation data - for (uint8_t i = 0; i < SIZE; i++) { - int index = (_head - i); - index = index < 0 ? SIZE + index : index; - - if (timestamp >= _buffer[index].time_us && timestamp < _buffer[index].time_us + (uint64_t)100'000) { - *sample = _buffer[index]; - - // Now we can set the tail to the item which - // comes after the one we removed since we don't - // want to have any older data in the buffer - if (index == _head) { - _tail = _head; - _first_write = true; - - } else { - _tail = (index + 1) % SIZE; - } - - _buffer[index].time_us = 0; - - return true; - } - - if (index == _tail) { - // we have reached the tail and haven't got a - // match - return false; - } - } - - return false; - } - - bool pop_oldest(const uint64_t ×tamp_oldest, const uint64_t ×tamp_newest, T *sample) - { - if (timestamp_oldest >= timestamp_newest) { - return false; - } - - for (uint8_t i = 0; i < SIZE; i++) { - - uint8_t index = (_tail + i) % SIZE; - - if (_buffer[index].time_us >= timestamp_oldest && _buffer[index].time_us <= timestamp_newest) { - *sample = _buffer[index]; - - // Now we can set the tail to the item which - // comes after the one we removed since we don't - // want to have any older data in the buffer - if (index == _head) { - _tail = _head; - _first_write = true; - - } else { - _tail = (index + 1) % SIZE; - } - - _buffer[index] = {}; - - return true; - } - } - - return false; - } - - bool pop_oldest(T *sample) - { - if (_tail == _head) { - return false; - } - - *sample = _buffer[_tail]; - _buffer[_tail].time_us = 0; - - _tail = (_tail + 1) % SIZE; - - return true; - } - - bool pop_newest(T *sample) - { - if (_tail == _head) { - return false; - } - - *sample = _buffer[_head]; - _buffer[_head].time_us = 0; - - _head = (_head - 1) % SIZE; - - return true; - } - - int get_total_size() const { return sizeof(*this) + sizeof(T) * SIZE; } - - uint8_t entries() const - { - int count = 0; - - for (uint8_t i = 0; i < SIZE; i++) { - if (_buffer[i].time_us != 0) { - count++; - } - } - - return count; - } - -private: - T _buffer[SIZE] {}; - - uint8_t _head{0}; - uint8_t _tail{0}; - - bool _first_write{true}; -}; diff --git a/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp deleted file mode 100644 index e629095be65a..000000000000 --- a/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ /dev/null @@ -1,491 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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 "VehicleOpticalFlow.hpp" - -#include - -namespace sensors -{ - -using namespace matrix; -using namespace time_literals; - -static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; - -VehicleOpticalFlow::VehicleOpticalFlow() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) -{ - _vehicle_optical_flow_pub.advertise(); - - _gyro_integrator.set_reset_samples(1); -} - -VehicleOpticalFlow::~VehicleOpticalFlow() -{ - Stop(); - perf_free(_cycle_perf); -} - -bool VehicleOpticalFlow::Start() -{ - _sensor_flow_sub.registerCallback(); - - _sensor_gyro_sub.registerCallback(); - _sensor_gyro_sub.set_required_updates(sensor_gyro_s::ORB_QUEUE_LENGTH / 2); - - _sensor_selection_sub.registerCallback(); - - ScheduleNow(); - return true; -} - -void VehicleOpticalFlow::Stop() -{ - Deinit(); - - // clear all registered callbacks - _sensor_flow_sub.unregisterCallback(); - _sensor_gyro_sub.unregisterCallback(); - _sensor_selection_sub.unregisterCallback(); -} - -void VehicleOpticalFlow::ParametersUpdate() -{ - // Check if parameters have changed - if (_params_sub.updated()) { - // clear update - parameter_update_s param_update; - _params_sub.copy(¶m_update); - - updateParams(); - - _flow_rotation = get_rot_matrix((enum Rotation)_param_sens_flow_rot.get()); - } -} - -void VehicleOpticalFlow::Run() -{ - perf_begin(_cycle_perf); - - ParametersUpdate(); - - UpdateDistanceSensor(); - - if (!_delta_angle_available) { - UpdateSensorGyro(); - } - - sensor_optical_flow_s sensor_optical_flow; - - if (_sensor_flow_sub.update(&sensor_optical_flow)) { - - // clear data accumulation if there's a gap in data - const uint64_t integration_gap_threshold_us = sensor_optical_flow.integration_timespan_us * 2; - - if ((sensor_optical_flow.timestamp_sample >= _flow_timestamp_sample_last + integration_gap_threshold_us) - || (_accumulated_count > 0 && (sensor_optical_flow.quality > 0) && _quality_sum == 0)) { - - ClearAccumulatedData(); - } - - - const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - sensor_optical_flow.integration_timespan_us; - const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; - - // delta angle - // - from sensor_optical_flow if available, otherwise use synchronized sensor_gyro if available - if (sensor_optical_flow.delta_angle_available && Vector2f(sensor_optical_flow.delta_angle).isAllFinite()) { - // passthrough integrated gyro if available - Vector3f delta_angle(sensor_optical_flow.delta_angle); - - if (!PX4_ISFINITE(delta_angle(2))) { - // Some sensors only provide X and Y angular rates, rotate them but place back the NAN on the Z axis - delta_angle(2) = 0.f; - _delta_angle += _flow_rotation * delta_angle; - _delta_angle(2) = NAN; - - } else { - _delta_angle += _flow_rotation * delta_angle; - } - - _delta_angle_available = true; - - } else { - _delta_angle_available = false; - - // integrate synchronized gyro - gyroSample gyro_sample; - - while (_gyro_buffer.pop_oldest(timestamp_oldest, timestamp_newest, &gyro_sample)) { - - _gyro_integrator.put(gyro_sample.data, gyro_sample.dt); - - float min_interval_s = (sensor_optical_flow.integration_timespan_us * 1e-6f) * 0.99f; - - if (_gyro_integrator.integral_dt() > min_interval_s) { - //PX4_INFO("integral dt: %.6f, min interval: %.6f", (double)_gyro_integrator.integral_dt(),(double) min_interval_s); - break; - } - } - - Vector3f delta_angle{NAN, NAN, NAN}; - uint16_t delta_angle_dt; - - if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) { - _delta_angle += delta_angle; - - } else { - // force integrator reset - _gyro_integrator.reset(); - } - } - - // distance - // - from sensor_optical_flow if available, otherwise use downward distance_sensor if available - if (sensor_optical_flow.distance_available && PX4_ISFINITE(sensor_optical_flow.distance_m)) { - if (!PX4_ISFINITE(_distance_sum)) { - _distance_sum = sensor_optical_flow.distance_m; - _distance_sum_count = 1; - - } else { - _distance_sum += sensor_optical_flow.distance_m; - _distance_sum_count += 1; - } - - } else { - // otherwise use buffered downward facing distance_sensor if available - rangeSample range_sample; - - if (_range_buffer.peak_first_older_than(sensor_optical_flow.timestamp_sample, &range_sample)) { - if (!PX4_ISFINITE(_distance_sum)) { - _distance_sum = range_sample.data; - _distance_sum_count = 1; - - } else { - _distance_sum += range_sample.data; - _distance_sum_count += 1; - } - } - } - - _flow_timestamp_sample_last = sensor_optical_flow.timestamp_sample; - _flow_integral(0) += sensor_optical_flow.pixel_flow[0]; - _flow_integral(1) += sensor_optical_flow.pixel_flow[1]; - - _integration_timespan_us += sensor_optical_flow.integration_timespan_us; - - _quality_sum += sensor_optical_flow.quality; - _accumulated_count++; - - bool publish = true; - - if (_param_sens_flow_rate.get() > 0) { - const float interval_us = 1e6f / _param_sens_flow_rate.get(); - - // don't allow publishing faster than SENS_FLOW_RATE - if (_integration_timespan_us < interval_us) { - publish = false; - } - } - - if (publish) { - vehicle_optical_flow_s vehicle_optical_flow{}; - - vehicle_optical_flow.timestamp_sample = sensor_optical_flow.timestamp_sample; - vehicle_optical_flow.device_id = sensor_optical_flow.device_id; - - _flow_integral.copyTo(vehicle_optical_flow.pixel_flow); - _delta_angle.copyTo(vehicle_optical_flow.delta_angle); - - vehicle_optical_flow.integration_timespan_us = _integration_timespan_us; - - vehicle_optical_flow.quality = _quality_sum / _accumulated_count; - - if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { - vehicle_optical_flow.distance_m = _distance_sum / _distance_sum_count; - - } else { - vehicle_optical_flow.distance_m = NAN; - } - - // SENS_FLOW_MAXR - if (PX4_ISFINITE(sensor_optical_flow.max_flow_rate) - && (sensor_optical_flow.max_flow_rate <= _param_sens_flow_maxr.get())) { - - vehicle_optical_flow.max_flow_rate = sensor_optical_flow.max_flow_rate; - - } else { - vehicle_optical_flow.max_flow_rate = _param_sens_flow_maxr.get(); - } - - // SENS_FLOW_MINHGT - if (PX4_ISFINITE(sensor_optical_flow.min_ground_distance) - && (sensor_optical_flow.min_ground_distance >= _param_sens_flow_minhgt.get())) { - - vehicle_optical_flow.min_ground_distance = sensor_optical_flow.min_ground_distance; - - } else { - vehicle_optical_flow.min_ground_distance = _param_sens_flow_minhgt.get(); - } - - // SENS_FLOW_MAXHGT - if (PX4_ISFINITE(sensor_optical_flow.max_ground_distance) - && (sensor_optical_flow.max_ground_distance <= _param_sens_flow_maxhgt.get())) { - - vehicle_optical_flow.max_ground_distance = sensor_optical_flow.max_ground_distance; - - } else { - vehicle_optical_flow.max_ground_distance = _param_sens_flow_maxhgt.get(); - } - - - // rotate (SENS_FLOW_ROT) - float zeroval = 0.f; - rotate_3f((enum Rotation)_param_sens_flow_rot.get(), vehicle_optical_flow.pixel_flow[0], - vehicle_optical_flow.pixel_flow[1], zeroval); - - vehicle_optical_flow.timestamp = hrt_absolute_time(); - _vehicle_optical_flow_pub.publish(vehicle_optical_flow); - - // vehicle_optical_flow_vel if distance is available (for logging) - if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { - const float range = _distance_sum / _distance_sum_count; - - vehicle_optical_flow_vel_s flow_vel{}; - - flow_vel.timestamp_sample = vehicle_optical_flow.timestamp_sample; - - // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate - // is produced by a RH rotation of the image about the sensor axis. - const Vector2f flow_xy_rad{-vehicle_optical_flow.pixel_flow[0], -vehicle_optical_flow.pixel_flow[1]}; - const Vector3f gyro_rate_integral{-vehicle_optical_flow.delta_angle[0], -vehicle_optical_flow.delta_angle[1], -vehicle_optical_flow.delta_angle[2]}; - - const float flow_dt = 1e-6f * vehicle_optical_flow.integration_timespan_us; - - // compensate for body motion to give a LOS rate - const Vector2f flow_compensated_XY_rad = flow_xy_rad - gyro_rate_integral.xy(); - - Vector3f vel_optflow_body; - vel_optflow_body(0) = - range * flow_compensated_XY_rad(1) / flow_dt; - vel_optflow_body(1) = range * flow_compensated_XY_rad(0) / flow_dt; - vel_optflow_body(2) = 0.f; - - // vel_body - flow_vel.vel_body[0] = vel_optflow_body(0); - flow_vel.vel_body[1] = vel_optflow_body(1); - - // vel_ne - flow_vel.vel_ne[0] = NAN; - flow_vel.vel_ne[1] = NAN; - - vehicle_attitude_s vehicle_attitude{}; - - if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - const matrix::Dcmf R_to_earth = matrix::Quatf(vehicle_attitude.q); - const Vector3f flow_vel_ne = R_to_earth * vel_optflow_body; - - flow_vel.vel_ne[0] = flow_vel_ne(0); - flow_vel.vel_ne[1] = flow_vel_ne(1); - } - - const Vector2f flow_rate(flow_xy_rad * (1.f / flow_dt)); - flow_rate.copyTo(flow_vel.flow_rate_uncompensated); - - const Vector2f flow_rate_compensated(flow_compensated_XY_rad * (1.f / flow_dt)); - flow_rate_compensated.copyTo(flow_vel.flow_rate_compensated); - - const Vector3f measured_body_rate(gyro_rate_integral * (1.f / flow_dt)); - - // gyro_rate - flow_vel.gyro_rate[0] = measured_body_rate(0); - flow_vel.gyro_rate[1] = measured_body_rate(1); - flow_vel.gyro_rate[2] = measured_body_rate(2); - - flow_vel.timestamp = hrt_absolute_time(); - - _vehicle_optical_flow_vel_pub.publish(flow_vel); - } - - ClearAccumulatedData(); - } - } - - // reschedule backup - ScheduleDelayed(10_ms); - - perf_end(_cycle_perf); -} - -void VehicleOpticalFlow::UpdateDistanceSensor() -{ - // update range finder buffer - distance_sensor_s distance_sensor; - - if ((_distance_sensor_selected < 0) && _distance_sensor_subs.advertised()) { - for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) { - - if (_distance_sensor_subs[i].update(&distance_sensor)) { - // only use the first instace which has the correct orientation - if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms) - && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) { - - int ndist = orb_group_count(ORB_ID(distance_sensor)); - - if (ndist > 1) { - PX4_INFO("selected distance_sensor:%d (%d advertised)", i, ndist); - } - - _distance_sensor_selected = i; - _last_range_sensor_update = distance_sensor.timestamp; - break; - } - } - } - } - - if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { - // range sample - if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { - - if ((distance_sensor.current_distance >= distance_sensor.min_distance) - && (distance_sensor.current_distance <= distance_sensor.max_distance)) { - - rangeSample sample; - sample.time_us = distance_sensor.timestamp; - sample.data = distance_sensor.current_distance; - - _range_buffer.push(sample); - - _last_range_sensor_update = distance_sensor.timestamp; - - return; - } - - } else { - _distance_sensor_selected = -1; - } - } - - if (hrt_elapsed_time(&_last_range_sensor_update) > 1_s) { - _distance_sensor_selected = -1; - } -} - -void VehicleOpticalFlow::UpdateSensorGyro() -{ - if (_sensor_selection_sub.updated()) { - sensor_selection_s sensor_selection{}; - _sensor_selection_sub.copy(&sensor_selection); - - for (uint8_t i = 0; i < MAX_SENSOR_COUNT; i++) { - uORB::SubscriptionData sensor_gyro_sub{ORB_ID(sensor_gyro), i}; - - if (sensor_gyro_sub.advertised() - && (sensor_gyro_sub.get().timestamp != 0) - && (sensor_gyro_sub.get().device_id != 0) - && (hrt_elapsed_time(&sensor_gyro_sub.get().timestamp) < 1_s)) { - - if (sensor_gyro_sub.get().device_id == sensor_selection.gyro_device_id) { - if (_sensor_gyro_sub.ChangeInstance(i) && _sensor_gyro_sub.registerCallback()) { - - _gyro_calibration.set_device_id(sensor_gyro_sub.get().device_id); - PX4_DEBUG("selecting sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); - break; - - } else { - PX4_ERR("unable to register callback for sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); - } - } - } - } - } - - // buffer - bool sensor_gyro_lost_printed = false; - int gyro_updates = 0; - - while (_sensor_gyro_sub.updated() && (gyro_updates < sensor_gyro_s::ORB_QUEUE_LENGTH)) { - gyro_updates++; - const unsigned last_generation = _sensor_gyro_sub.get_last_generation(); - sensor_gyro_s sensor_gyro; - - if (_sensor_gyro_sub.copy(&sensor_gyro)) { - - if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) { - if (!sensor_gyro_lost_printed) { - PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation()); - sensor_gyro_lost_printed = true; - } - } - - _gyro_calibration.set_device_id(sensor_gyro.device_id); - _gyro_calibration.SensorCorrectionsUpdate(); - - const float dt_s = (sensor_gyro.timestamp_sample - _gyro_timestamp_sample_last) * 1e-6f; - _gyro_timestamp_sample_last = sensor_gyro.timestamp_sample; - - gyroSample gyro_sample; - gyro_sample.time_us = sensor_gyro.timestamp_sample; - gyro_sample.data = _gyro_calibration.Correct(Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}); - gyro_sample.dt = dt_s; - - _gyro_buffer.push(gyro_sample); - } - } -} - -void VehicleOpticalFlow::ClearAccumulatedData() -{ - // clear accumulated data - _flow_integral.zero(); - _integration_timespan_us = 0; - - _delta_angle.zero(); - - _distance_sum = NAN; - _distance_sum_count = 0; - - _quality_sum = 0; - _accumulated_count = 0; - - _gyro_integrator.reset(); -} - -void VehicleOpticalFlow::PrintStatus() -{ - -} - -}; // namespace sensors diff --git a/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp deleted file mode 100644 index fb424ea5aff8..000000000000 --- a/src/modules/sensors/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ /dev/null @@ -1,147 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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 "data_validator/DataValidatorGroup.hpp" -#include "RingBuffer.hpp" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sensors -{ - -class VehicleOpticalFlow : public ModuleParams, public px4::ScheduledWorkItem -{ -public: - VehicleOpticalFlow(); - ~VehicleOpticalFlow() override; - - bool Start(); - void Stop(); - - void PrintStatus(); - -private: - void ClearAccumulatedData(); - void UpdateDistanceSensor(); - void UpdateSensorGyro(); - - void Run() override; - - void ParametersUpdate(); - void SensorCorrectionsUpdate(bool force = false); - - static constexpr int MAX_SENSOR_COUNT = 3; - - uORB::Publication _vehicle_optical_flow_pub{ORB_ID(vehicle_optical_flow)}; - uORB::Publication _vehicle_optical_flow_vel_pub{ORB_ID(vehicle_optical_flow_vel)}; - - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; - - uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; - - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - - uORB::SubscriptionCallbackWorkItem _sensor_flow_sub{this, ORB_ID(sensor_optical_flow)}; - uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub{this, ORB_ID(sensor_gyro)}; - uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; - - sensors::IntegratorConing _gyro_integrator{}; - - hrt_abstime _gyro_timestamp_sample_last{0}; - - calibration::Gyroscope _gyro_calibration{}; - - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; - - matrix::Dcmf _flow_rotation{matrix::eye()}; - - hrt_abstime _flow_timestamp_sample_last{0}; - matrix::Vector2f _flow_integral{}; - matrix::Vector3f _delta_angle{}; - uint32_t _integration_timespan_us{}; - float _distance_sum{NAN}; - uint8_t _distance_sum_count{0}; - uint16_t _quality_sum{0}; - uint8_t _accumulated_count{0}; - - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations - hrt_abstime _last_range_sensor_update{0}; - - bool _delta_angle_available{false}; - - struct gyroSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - matrix::Vector3f data{}; - float dt{0.f}; - }; - - struct rangeSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float data{}; - }; - - RingBuffer _gyro_buffer{}; - RingBuffer _range_buffer{}; - - DEFINE_PARAMETERS( - (ParamInt) _param_sens_flow_rot, - (ParamFloat) _param_sens_flow_minhgt, - (ParamFloat) _param_sens_flow_maxhgt, - (ParamFloat) _param_sens_flow_maxr, - (ParamFloat) _param_sens_flow_rate - ) -}; -}; // namespace sensors diff --git a/src/modules/sensors/sensors/voted_sensors_update.cpp b/src/modules/sensors/sensors/voted_sensors_update.cpp deleted file mode 100644 index 3e89e18644d2..000000000000 --- a/src/modules/sensors/sensors/voted_sensors_update.cpp +++ /dev/null @@ -1,497 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2022 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. - * - ****************************************************************************/ - -/** - * @file voted_sensors_update.cpp - * - * @author Beat Kueng - */ - -#include "voted_sensors_update.h" - -#include -#include -#include -#include - -using namespace sensors; -using namespace matrix; -using namespace time_literals; - -VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, - uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) : - ModuleParams(nullptr), - _vehicle_imu_sub(vehicle_imu_sub), - _hil_enabled(hil_enabled) -{ - _sensor_selection_pub.advertise(); - _sensors_status_imu_pub.advertise(); - - if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit - _gyro.voter.set_timeout(500000); - _accel.voter.set_timeout(500000); - } - - initializeSensors(); - - parametersUpdate(); -} - -void VotedSensorsUpdate::initializeSensors() -{ - initSensorClass(_gyro, MAX_SENSOR_COUNT); - initSensorClass(_accel, MAX_SENSOR_COUNT); -} - -void VotedSensorsUpdate::parametersUpdate() -{ - _parameter_update = true; - - updateParams(); - - // run through all IMUs - for (uint8_t uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - uORB::SubscriptionData imu{ORB_ID(vehicle_imu), uorb_index}; - imu.update(); - - if (imu.advertised() && (imu.get().timestamp != 0) - && (imu.get().accel_device_id != 0) && (imu.get().gyro_device_id != 0)) { - - // find corresponding configured accel priority - int8_t accel_cal_index = calibration::FindCurrentCalibrationIndex("ACC", imu.get().accel_device_id); - - if (accel_cal_index >= 0) { - // found matching CAL_ACCx_PRIO - int32_t accel_priority_old = _accel.priority_configured[uorb_index]; - - _accel.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("ACC", "PRIO", accel_cal_index); - - if (accel_priority_old != _accel.priority_configured[uorb_index]) { - if (_accel.priority_configured[uorb_index] == 0) { - // disabled - _accel.priority[uorb_index] = 0; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = _accel.priority_configured[uorb_index] - accel_priority_old; - _accel.priority[uorb_index] = math::constrain(_accel.priority[uorb_index] + priority_change, static_cast(1), - static_cast(100)); - } - } - } - - // find corresponding configured gyro priority - int8_t gyro_cal_index = calibration::FindCurrentCalibrationIndex("GYRO", imu.get().gyro_device_id); - - if (gyro_cal_index >= 0) { - // found matching CAL_GYROx_PRIO - int32_t gyro_priority_old = _gyro.priority_configured[uorb_index]; - - _gyro.priority_configured[uorb_index] = calibration::GetCalibrationParamInt32("GYRO", "PRIO", gyro_cal_index); - - if (gyro_priority_old != _gyro.priority_configured[uorb_index]) { - if (_gyro.priority_configured[uorb_index] == 0) { - // disabled - _gyro.priority[uorb_index] = 0; - - } else { - // change relative priority to incorporate any sensor faults - int priority_change = _gyro.priority_configured[uorb_index] - gyro_priority_old; - _gyro.priority[uorb_index] = math::constrain(_gyro.priority[uorb_index] + priority_change, static_cast(1), - static_cast(100)); - } - } - } - } - } -} - -void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) -{ - const hrt_abstime time_now_us = hrt_absolute_time(); - - for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { - vehicle_imu_s imu_report; - - if ((_accel.priority[uorb_index] > 0) && (_gyro.priority[uorb_index] > 0) - && _vehicle_imu_sub[uorb_index].update(&imu_report)) { - - // copy corresponding vehicle_imu_status for accel & gyro error counts - vehicle_imu_status_s imu_status{}; - _vehicle_imu_status_subs[uorb_index].copy(&imu_status); - - _accel_device_id[uorb_index] = imu_report.accel_device_id; - _gyro_device_id[uorb_index] = imu_report.gyro_device_id; - - // convert the delta velocities to an equivalent acceleration - const float accel_dt_inv = 1.e6f / (float)imu_report.delta_velocity_dt; - Vector3f accel_data = Vector3f{imu_report.delta_velocity} * accel_dt_inv; - - - // convert the delta angles to an equivalent angular rate - const float gyro_dt_inv = 1.e6f / (float)imu_report.delta_angle_dt; - Vector3f gyro_rate = Vector3f{imu_report.delta_angle} * gyro_dt_inv; - - _last_sensor_data[uorb_index].timestamp = imu_report.timestamp_sample; - _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); - _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); - _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); - _last_sensor_data[uorb_index].accelerometer_integral_dt = imu_report.delta_velocity_dt; - _last_sensor_data[uorb_index].accelerometer_clipping = imu_report.delta_velocity_clipping; - _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); - _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); - _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); - _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; - _last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping; - _last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count; - _last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count; - - _last_accel_timestamp[uorb_index] = imu_report.timestamp_sample; - - _accel.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, - imu_status.accel_error_count, _accel.priority[uorb_index]); - - _gyro.voter.put(uorb_index, imu_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, - imu_status.gyro_error_count, _gyro.priority[uorb_index]); - } - } - - // find the best sensor - int accel_best_index = _accel.last_best_vote; - int gyro_best_index = _gyro.last_best_vote; - - if (!_parameter_update) { - // update current accel/gyro selection, skipped on cycles where parameters update - _accel.voter.get_best(time_now_us, &accel_best_index); - _gyro.voter.get_best(time_now_us, &gyro_best_index); - - if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { - // use sensor_selection to find best - if (_sensor_selection_sub.update(&_selection)) { - // reset inconsistency checks against primary - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - _accel_diff[sensor_index].zero(); - _gyro_diff[sensor_index].zero(); - } - } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { - accel_best_index = i; - } - - if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { - gyro_best_index = i; - } - } - - } else { - // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published - checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); - checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); - } - } - - // write data for the best sensor to output variables - if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0) - && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) { - - raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; - memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, - sizeof(raw.accelerometer_m_s2)); - memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); - - raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; - raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; - - raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; - raw.gyro_clipping = _last_sensor_data[gyro_best_index].gyro_clipping; - raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; - - if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { - _accel.last_best_vote = (uint8_t)accel_best_index; - _selection.accel_device_id = _accel_device_id[accel_best_index]; - _selection_changed = true; - } - - if ((_gyro.last_best_vote != gyro_best_index) || (_selection.gyro_device_id != _gyro_device_id[gyro_best_index])) { - _gyro.last_best_vote = (uint8_t)gyro_best_index; - _selection.gyro_device_id = _gyro_device_id[gyro_best_index]; - _selection_changed = true; - - // clear all registered callbacks - for (auto &sub : _vehicle_imu_sub) { - sub.unregisterCallback(); - } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - vehicle_imu_s report{}; - - if (_vehicle_imu_sub[i].copy(&report)) { - if ((report.gyro_device_id != 0) && (report.gyro_device_id == _gyro_device_id[gyro_best_index])) { - _vehicle_imu_sub[i].registerCallback(); - } - } - } - } - } - - // publish sensor selection if changed - if (_param_sens_imu_mode.get() || (_selection.timestamp == 0)) { - if (_selection_changed) { - // don't publish until selected IDs are valid - if (_selection.accel_device_id > 0 && _selection.gyro_device_id > 0) { - _selection.timestamp = hrt_absolute_time(); - _sensor_selection_pub.publish(_selection); - _selection_changed = false; - } - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - _accel_diff[sensor_index].zero(); - _gyro_diff[sensor_index].zero(); - } - } - } -} - -bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, - events::px4::enums::sensor_type_t sensor_type) -{ - if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { - - uint32_t flags = sensor.voter.failover_state(); - int failover_index = sensor.voter.failover_index(); - - if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { - if (failover_index != -1) { - // we switched due to a non-critical reason. No need to panic. - PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index); - } - - } else { - if (failover_index != -1) { - - const hrt_abstime now = hrt_absolute_time(); - - if (now - _last_error_message > 3_s) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!\t", - sensor_name, - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); - - events::px4::enums::sensor_failover_reason_t failover_reason{}; - - if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } - - if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } - - if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } - - if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } - - /* EVENT - * @description - * Land immediately and check the system. - */ - events::send( - events::ID("sensor_failover"), events::Log::Emergency, "{1} sensor #{2} failure: {3}", sensor_type, failover_index, - failover_reason); - - _last_error_message = now; - } - - // reduce priority of failed sensor to the minimum - sensor.priority[failover_index] = 1; - } - } - - sensor.last_failover_count = sensor.voter.failover_count(); - return true; - } - - return false; -} - -void VotedSensorsUpdate::initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max) -{ - bool added = false; - int max_sensor_index = -1; - - for (unsigned i = 0; i < sensor_count_max; i++) { - - max_sensor_index = i; - - if (!sensor_data.advertised[i] && sensor_data.subscription[i].advertised()) { - sensor_data.advertised[i] = true; - sensor_data.priority[i] = DEFAULT_PRIORITY; - sensor_data.priority_configured[i] = DEFAULT_PRIORITY; - - if (i > 0) { - /* the first always exists, but for each further sensor, add a new validator */ - if (sensor_data.voter.add_new_validator()) { - added = true; - - } else { - PX4_ERR("failed to add validator for sensor %s %i", sensor_data.subscription[i].get_topic()->o_name, i); - } - } - } - } - - // never decrease the sensor count, as we could end up with mismatching validators - if (max_sensor_index + 1 > sensor_data.subscription_count) { - sensor_data.subscription_count = max_sensor_index + 1; - } - - if (added) { - // force parameter refresh if anything was added - parametersUpdate(); - } -} - -void VotedSensorsUpdate::printStatus() -{ - PX4_INFO_RAW("selected gyro: %" PRIu32 " (%" PRIu8 ")\n", _selection.gyro_device_id, _gyro.last_best_vote); - _gyro.voter.print(); - - PX4_INFO_RAW("\n"); - PX4_INFO_RAW("selected accel: %" PRIu32 " (%" PRIu8 ")\n", _selection.accel_device_id, _accel.last_best_vote); - _accel.voter.print(); -} - -void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) -{ - imuPoll(raw); - - calcAccelInconsistency(); - calcGyroInconsistency(); - - sensors_status_imu_s status{}; - status.accel_device_id_primary = _selection.accel_device_id; - status.gyro_device_id_primary = _selection.gyro_device_id; - - static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::accel_inconsistency_m_s_s) / sizeof( - sensors_status_imu_s::accel_inconsistency_m_s_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); - static_assert(MAX_SENSOR_COUNT == (sizeof(sensors_status_imu_s::gyro_inconsistency_rad_s) / sizeof( - sensors_status_imu_s::gyro_inconsistency_rad_s[0])), "check sensors_status_imu accel_inconsistency_m_s_s size"); - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_accel_device_id[i] != 0) && (_accel.priority[i] > 0)) { - status.accel_device_ids[i] = _accel_device_id[i]; - status.accel_inconsistency_m_s_s[i] = _accel_diff[i].norm(); - status.accel_healthy[i] = (_accel.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); - status.accel_priority[i] = _accel.voter.get_sensor_priority(i); - } - - if ((_gyro_device_id[i] != 0) && (_gyro.priority[i] > 0)) { - status.gyro_device_ids[i] = _gyro_device_id[i]; - status.gyro_inconsistency_rad_s[i] = _gyro_diff[i].norm(); - status.gyro_healthy[i] = (_gyro.voter.get_sensor_state(i) == DataValidator::ERROR_FLAG_NO_ERROR); - status.gyro_priority[i] = _gyro.voter.get_sensor_priority(i); - } - } - - - status.timestamp = hrt_absolute_time(); - _sensors_status_imu_pub.publish(status); - - if (_parameter_update) { - // reset - _parameter_update = false; - } -} - -void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) -{ - if (_last_accel_timestamp[_accel.last_best_vote]) { - raw.accelerometer_timestamp_relative = (int32_t)((int64_t)_last_accel_timestamp[_accel.last_best_vote] - - (int64_t)raw.timestamp); - } -} - -void VotedSensorsUpdate::calcAccelInconsistency() -{ - Vector3f accel_mean{}; - Vector3f accel_all[MAX_SENSOR_COUNT] {}; - uint8_t accel_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { - accel_count++; - accel_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].accelerometer_m_s2}; - accel_mean += accel_all[sensor_index]; - } - } - - if (accel_count > 0) { - accel_mean /= accel_count; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_accel_device_id[sensor_index] != 0) && (_accel.priority[sensor_index] > 0)) { - _accel_diff[sensor_index] = 0.95f * _accel_diff[sensor_index] + 0.05f * (accel_all[sensor_index] - accel_mean); - } - } - } -} - -void VotedSensorsUpdate::calcGyroInconsistency() -{ - Vector3f gyro_mean{}; - Vector3f gyro_all[MAX_SENSOR_COUNT] {}; - uint8_t gyro_count = 0; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { - gyro_count++; - gyro_all[sensor_index] = Vector3f{_last_sensor_data[sensor_index].gyro_rad}; - gyro_mean += gyro_all[sensor_index]; - } - } - - if (gyro_count > 0) { - gyro_mean /= gyro_count; - - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - if ((_gyro_device_id[sensor_index] != 0) && (_gyro.priority[sensor_index] > 0)) { - _gyro_diff[sensor_index] = 0.95f * _gyro_diff[sensor_index] + 0.05f * (gyro_all[sensor_index] - gyro_mean); - } - } - } -} diff --git a/src/modules/sensors/sensors/voted_sensors_update.h b/src/modules/sensors/sensors/voted_sensors_update.h deleted file mode 100644 index 9e8d40a6e934..000000000000 --- a/src/modules/sensors/sensors/voted_sensors_update.h +++ /dev/null @@ -1,186 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2022 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 - -/** - * @file voted_sensors_update.h - * - * @author Beat Kueng - */ - -#include "data_validator/DataValidator.hpp" -#include "data_validator/DataValidatorGroup.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace sensors -{ - -static constexpr uint8_t MAX_SENSOR_COUNT = 4; - -/** - ** class VotedSensorsUpdate - * - * Handling of sensor updates with voting - */ -class VotedSensorsUpdate : public ModuleParams -{ -public: - /** - * @param parameters parameter values. These do not have to be initialized when constructing this object. - * Only when calling init(), they have to be initialized. - */ - VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); - - /** - * This tries to find new sensor instances. This is called from init(), then it can be called periodically. - */ - void initializeSensors(); - - void printStatus(); - - /** - * call this whenever parameters got updated. Make sure to have initializeSensors() called at least - * once before calling this. - */ - void parametersUpdate(); - - /** - * read new sensor data - */ - void sensorsPoll(sensor_combined_s &raw); - - /** - * set the relative timestamps of each sensor timestamp, based on the last sensorsPoll, - * so that the data can be published. - */ - void setRelativeTimestamps(sensor_combined_s &raw); - -private: - - static constexpr uint8_t DEFAULT_PRIORITY = 50; - - struct SensorData { - SensorData() = delete; - explicit SensorData(ORB_ID meta) : subscription{{meta, 0}, {meta, 1}, {meta, 2}, {meta, 3}} {} - - uORB::Subscription subscription[MAX_SENSOR_COUNT]; /**< raw sensor data subscription */ - DataValidatorGroup voter{1}; - unsigned int last_failover_count{0}; - int32_t priority[MAX_SENSOR_COUNT] {}; - int32_t priority_configured[MAX_SENSOR_COUNT] {}; - uint8_t last_best_vote{0}; /**< index of the latest best vote */ - uint8_t subscription_count{0}; - bool advertised[MAX_SENSOR_COUNT] {false, false, false}; - }; - - void initSensorClass(SensorData &sensor_data, uint8_t sensor_count_max); - - /** - * Poll IMU for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void imuPoll(sensor_combined_s &raw); - - /** - * Check & handle failover of a sensor - * @return true if a switch occured (could be for a non-critical reason) - */ - bool checkFailover(SensorData &sensor, const char *sensor_name, events::px4::enums::sensor_type_t sensor_type); - - /** - * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors - */ - void calcAccelInconsistency(); - - /** - * Calculates the magnitude in rad/s of the largest difference between each gyro vector and the mean of all vectors - */ - void calcGyroInconsistency(); - - SensorData _accel{ORB_ID::sensor_accel}; - SensorData _gyro{ORB_ID::sensor_gyro}; - - hrt_abstime _last_error_message{0}; - orb_advert_t _mavlink_log_pub{nullptr}; - - uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */ - uORB::Publication _sensors_status_imu_pub{ORB_ID(sensors_status_imu)}; - - uORB::SubscriptionCallbackWorkItem(&_vehicle_imu_sub)[MAX_SENSOR_COUNT]; - uORB::SubscriptionMultiArray _vehicle_imu_status_subs{ORB_ID::vehicle_imu_status}; - - uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - - sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */ - - const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - - bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ - - matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ - matrix::Vector3f _gyro_diff[MAX_SENSOR_COUNT] {}; /**< filtered gyro differences between IMU uinits (rad/s) */ - - uint32_t _accel_device_id[MAX_SENSOR_COUNT] {}; /**< accel driver device id for each uorb instance */ - uint32_t _gyro_device_id[MAX_SENSOR_COUNT] {}; /**< gyro driver device id for each uorb instance */ - - uint64_t _last_accel_timestamp[MAX_SENSOR_COUNT] {}; /**< latest full timestamp */ - - sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ - - bool _parameter_update{false}; - - DEFINE_PARAMETERS( - (ParamBool) _param_sens_imu_mode - ) -}; - -} /* namespace sensors */