From e79737a38da1b1293a6fcc4da6193f72874b5e66 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Oct 2023 14:19:48 -0400 Subject: [PATCH 01/14] ekf2: create simple estimator aid source base class and extract zero velocity update --- src/modules/ekf2/CMakeLists.txt | 3 +- src/modules/ekf2/EKF/CMakeLists.txt | 3 +- .../EKF/aid_sources/EstimatorAidSource.hpp | 64 +++++++++++++++++++ .../ZeroVelocityUpdate.cpp} | 47 +++++++++----- .../EKF/aid_sources/ZeroVelocityUpdate.hpp | 54 ++++++++++++++++ src/modules/ekf2/EKF/control.cpp | 3 +- src/modules/ekf2/EKF/ekf.cpp | 3 +- src/modules/ekf2/EKF/ekf.h | 14 ++-- 8 files changed, 165 insertions(+), 26 deletions(-) create mode 100644 src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp rename src/modules/ekf2/EKF/{zero_velocity_update.cpp => aid_sources/ZeroVelocityUpdate.cpp} (63%) create mode 100644 src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 37fc6c005b..30118b3672 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -131,7 +131,7 @@ list(APPEND EKF_SRCS EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp EKF/zero_gyro_update.cpp - EKF/zero_velocity_update.cpp + EKF/aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) @@ -220,6 +220,7 @@ px4_add_module( #-O0 INCLUDES EKF + EKF/aid_sources ${EKF_GENERATED_DERIVATION_INCLUDE_PATH} PRIORITY "SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 60f9d10c5d..6af5b891c7 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -48,7 +48,8 @@ list(APPEND EKF_SRCS yaw_fusion.cpp zero_innovation_heading_update.cpp zero_gyro_update.cpp - zero_velocity_update.cpp + + aid_sources/ZeroVelocityUpdate.cpp ) if(CONFIG_EKF2_AIRSPEED) diff --git a/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp new file mode 100644 index 0000000000..6d6c9e8ec2 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/EstimatorAidSource.hpp @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ESTIMATOR_AID_SOURCE_HPP +#define EKF_ESTIMATOR_AID_SOURCE_HPP + +#include + +#include + +// forward declarations +class Ekf; + +namespace estimator +{ +struct imuSample; +}; + +class EstimatorAidSource +{ +public: + EstimatorAidSource() = default; + virtual ~EstimatorAidSource() = default; + + virtual bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) = 0; + + virtual void reset() = 0; + +private: + + +}; + +#endif // !EKF_ESTIMATOR_AID_SOURCE_HPP diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp similarity index 63% rename from src/modules/ekf2/EKF/zero_velocity_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index f78dc28cc2..d091e5aadb 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 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 @@ -31,37 +31,50 @@ * ****************************************************************************/ -/** - * @file zero_velocity_update.cpp - * Control function for ekf zero velocity update - */ +#include "ZeroVelocityUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroVelocityUpdate() +ZeroVelocityUpdate::ZeroVelocityUpdate() +{ + reset(); +} + +void ZeroVelocityUpdate::reset() +{ + _time_last_zero_velocity_fuse = 0; +} + +bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { // Fuse zero velocity at a limited rate (every 200 milliseconds) - const bool zero_velocity_update_data_ready = isTimedOut(_time_last_zero_velocity_fuse, (uint64_t)2e5); + const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us); if (zero_velocity_update_data_ready) { - const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest - && _control_status_prev.flags.vehicle_at_rest - && (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift + const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest + && ekf.control_status_prev_flags().vehicle_at_rest + && (!ekf.isVerticalVelocityAidingActive() + || !ekf.control_status_flags().tilt_align); // otherwise the filter is "too rigid" to follow a position drift if (continuing_conditions_passing) { - Vector3f vel_obs{0, 0, 0}; - Vector3f innovation = _state.vel - vel_obs; + Vector3f vel_obs{0.f, 0.f, 0.f}; + Vector3f innovation = ekf.state().vel - vel_obs; // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements - const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var = getVelocityVariance() + obs_var; + + const float obs_var = ekf.control_status_flags().tilt_align ? sq(0.2f) : sq(0.001f); + Vector3f innov_var = ekf.getVelocityVariance() + obs_var; for (unsigned i = 0; i < 3; i++) { - fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + ekf.fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); } - _time_last_zero_velocity_fuse = _time_delayed_us; + _time_last_zero_velocity_fuse = imu_delayed.time_us; + + return true; } } + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp new file mode 100644 index 0000000000..0016c584c6 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2022-2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_VELOCITY_UPDATE_HPP +#define EKF_ZERO_VELOCITY_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroVelocityUpdate : public EstimatorAidSource +{ +public: + ZeroVelocityUpdate(); + virtual ~ZeroVelocityUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + +}; + +#endif // !EKF_ZERO_VELOCITY_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 91acf6660b..d55ef16c2e 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -145,7 +145,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlZeroInnovationHeadingUpdate(); - controlZeroVelocityUpdate(); + _zero_velocity_update.update(*this, imu_delayed); + controlZeroGyroUpdate(imu_delayed); // Fake position measurement for constraining drift when no other velocity or position measurements diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f1f5204e98..454b29eb70 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -110,7 +110,6 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; - _time_last_zero_velocity_fuse = 0; _last_known_pos.setZero(); @@ -175,6 +174,8 @@ void Ekf::reset() #if defined(CONFIG_EKF2_RANGE_FINDER) resetEstimatorAidStatus(_aid_src_rng_hgt); #endif // CONFIG_EKF2_RANGE_FINDER + + _zero_velocity_update.reset(); } bool Ekf::update() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fbe782f22e..9f6e3828ea 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,8 @@ #include #include +#include "aid_sources/ZeroVelocityUpdate.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface @@ -83,6 +85,8 @@ public: static uint8_t getNumberOfStates() { return State::size; } + const StateSample &state() const { return _state; } + #if defined(CONFIG_EKF2_BAROMETER) const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } @@ -317,6 +321,9 @@ public: #endif } + // fuse single velocity and position measurement + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); + // gyro bias const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s @@ -514,7 +521,6 @@ private: uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; - uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -774,9 +780,6 @@ private: void fuseDrag(const dragSample &drag_sample); #endif // CONFIG_EKF2_DRAG_FUSION - // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); - void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var); void resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var); @@ -1070,7 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroVelocityUpdate(); void controlZeroGyroUpdate(const imuSample &imu_delayed); void fuseDeltaAngBias(float innov, float innov_var, int obs_index); @@ -1239,6 +1241,8 @@ private: // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; } + + ZeroVelocityUpdate _zero_velocity_update{}; }; #endif // !EKF_EKF_H From 27f9b1b65a3acd876a091dd0169ac10cd67c067a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Oct 2023 14:47:27 -0400 Subject: [PATCH 02/14] ekf2: move zero gyro update to aid source class --- src/modules/ekf2/CMakeLists.txt | 3 +- src/modules/ekf2/EKF/CMakeLists.txt | 2 +- .../ZeroGyroUpdate.cpp} | 60 ++++++++-------- .../ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp | 54 ++++++++++++++ src/modules/ekf2/EKF/control.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 72 +++++++++---------- 6 files changed, 127 insertions(+), 68 deletions(-) rename src/modules/ekf2/EKF/{zero_gyro_update.cpp => aid_sources/ZeroGyroUpdate.cpp} (70%) create mode 100644 src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 30118b3672..2854fc3b0d 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -130,7 +130,8 @@ list(APPEND EKF_SRCS EKF/vel_pos_fusion.cpp EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp - EKF/zero_gyro_update.cpp + + EKF/aid_sources/ZeroGyroUpdate.cpp EKF/aid_sources/ZeroVelocityUpdate.cpp ) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 6af5b891c7..cdb0a56ba5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -47,8 +47,8 @@ list(APPEND EKF_SRCS vel_pos_fusion.cpp yaw_fusion.cpp zero_innovation_heading_update.cpp - zero_gyro_update.cpp + aid_sources/ZeroGyroUpdate.cpp aid_sources/ZeroVelocityUpdate.cpp ) diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp similarity index 70% rename from src/modules/ekf2/EKF/zero_gyro_update.cpp rename to src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp index 571bde209a..ea9d3f11a8 100644 --- a/src/modules/ekf2/EKF/zero_gyro_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.cpp @@ -31,21 +31,25 @@ * ****************************************************************************/ -/** - * @file zero_gyro_update.cpp - * Control function for ekf zero gyro update - */ +#include "ZeroGyroUpdate.hpp" -#include "ekf.h" +#include "../ekf.h" -void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) +ZeroGyroUpdate::ZeroGyroUpdate() { - if (!(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias))) { - return; - } + reset(); +} +void ZeroGyroUpdate::reset() +{ + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; +} + +bool ZeroGyroUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) +{ // When at rest, fuse the gyro data as a direct observation of the gyro bias - if (_control_status.flags.vehicle_at_rest) { + if (ekf.control_status_flags().vehicle_at_rest) { // Downsample gyro data to run the fusion at a lower rate _zgup_delta_ang += imu_delayed.delta_ang; _zgup_delta_ang_dt += imu_delayed.delta_ang_dt; @@ -54,38 +58,38 @@ void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt; if (zero_gyro_update_data_ready) { + Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; - Vector3f innovation = _state.gyro_bias - gyro_bias; + Vector3f innovation = ekf.state().gyro_bias - gyro_bias; - const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); + const float obs_var = sq(math::constrain(ekf.getGyroNoise(), 0.f, 1.f)); - const Vector3f innov_var = getGyroBiasVariance() + obs_var; + const Vector3f innov_var = ekf.getGyroBiasVariance() + obs_var; for (int i = 0; i < 3; i++) { - fuseDeltaAngBias(innovation(i), innov_var(i), i); + Ekf::VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. + const unsigned state_index = State::gyro_bias.idx + i; + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < State::size; row++) { + K(row) = ekf.stateCovariance(row, state_index) / innov_var(i); + } + + ekf.measurementUpdate(K, innov_var(i), innovation(i)); } // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; + + return true; } - } else if (_control_status_prev.flags.vehicle_at_rest) { + } else if (ekf.control_status_prev_flags().vehicle_at_rest) { // Reset the integrators _zgup_delta_ang.setZero(); _zgup_delta_ang_dt = 0.f; } -} - -void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) -{ - VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + State::gyro_bias.idx; - - // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < State::size; row++) { - K(row) = P(row, state_index) / innov_var; - } - - measurementUpdate(K, innov_var, innov); + + return false; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp new file mode 100644 index 0000000000..1ad488e539 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/ZeroGyroUpdate.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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. + * + ****************************************************************************/ + +#ifndef EKF_ZERO_GYRO_UPDATE_HPP +#define EKF_ZERO_GYRO_UPDATE_HPP + +#include "EstimatorAidSource.hpp" + +class ZeroGyroUpdate : public EstimatorAidSource +{ +public: + ZeroGyroUpdate(); + virtual ~ZeroGyroUpdate() = default; + + void reset() override; + bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + +private: + + matrix::Vector3f _zgup_delta_ang{}; + float _zgup_delta_ang_dt{0.f}; +}; + +#endif // !EKF_ZERO_GYRO_UPDATE_HPP diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index d55ef16c2e..5c7aa5790b 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -147,7 +147,9 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) _zero_velocity_update.update(*this, imu_delayed); - controlZeroGyroUpdate(imu_delayed); + if (_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)) { + _zero_gyro_update.update(*this, imu_delayed); + } // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 9f6e3828ea..4311417d36 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -59,6 +59,7 @@ #include #include +#include "aid_sources/ZeroGyroUpdate.hpp" #include "aid_sources/ZeroVelocityUpdate.hpp" enum class Likelihood { LOW, MEDIUM, HIGH }; @@ -249,6 +250,7 @@ public: // get the full covariance matrix const matrix::SquareMatrix &covariances() const { return P; } + float stateCovariance(unsigned r, unsigned c) const { return P(r, c); } // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } @@ -328,6 +330,7 @@ public: const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s float getGyroBiasLimit() const { return _params.gyro_bias_lim; } + float getGyroNoise() const { return _params.gyro_noise; } // accel bias const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 @@ -473,6 +476,37 @@ public: const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL + + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) + { + clearInhibitedStateKalmanGains(K); + + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; + + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { + // Instad of literally computing KHP, use an equvalent + // equation involving less mathematical operations + KHP(row, col) = KS(row) * K(col); + } + } + + const bool is_healthy = checkAndFixCovarianceUpdate(KHP); + + if (is_healthy) { + // apply the covariance corrections + P -= KHP; + + fixCovarianceErrors(true); + + // apply the state corrections + fuse(K, innovation); + } + + return is_healthy; + } + private: // set the internal states and status to their default value @@ -530,10 +564,6 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - // zero gyro update - Vector3f _zgup_delta_ang{}; - float _zgup_delta_ang_dt{0.f}; - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _height_rate_lpf{0.0f}; float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) @@ -917,36 +947,6 @@ private: #endif // CONFIG_EKF2_WIND } - bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) - { - clearInhibitedStateKalmanGains(K); - - const VectorState KS = K * innovation_variance; - SquareMatrixState KHP; - - for (unsigned row = 0; row < State::size; row++) { - for (unsigned col = 0; col < State::size; col++) { - // Instad of literally computing KHP, use an equvalent - // equation involving less mathematical operations - KHP(row, col) = KS(row) * K(col); - } - } - - const bool is_healthy = checkAndFixCovarianceUpdate(KHP); - - if (is_healthy) { - // apply the covariance corrections - P -= KHP; - - fixCovarianceErrors(true); - - // apply the state corrections - fuse(K, innovation); - } - - return is_healthy; - } - // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); @@ -1073,9 +1073,6 @@ private: void resetHeightToLastKnown(); void stopFakeHgtFusion(); - void controlZeroGyroUpdate(const imuSample &imu_delayed); - void fuseDeltaAngBias(float innov, float innov_var, int obs_index); - void controlZeroInnovationHeadingUpdate(); #if defined(CONFIG_EKF2_AUXVEL) @@ -1242,6 +1239,7 @@ private: status.innovation_rejected = innovation_rejected; } + ZeroGyroUpdate _zero_gyro_update{}; ZeroVelocityUpdate _zero_velocity_update{}; }; From 71b9e31005a3131f548604806db14f617e5d2437 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 18 Oct 2023 16:14:33 -0400 Subject: [PATCH 03/14] drivers/osd/msp_osd: use proper EKF status flags instead of solution status bits --- src/drivers/osd/msp_osd/msp_osd.cpp | 12 +----------- src/drivers/osd/msp_osd/msp_osd.hpp | 2 -- src/drivers/osd/msp_osd/uorb_to_msp.cpp | 7 +++---- src/drivers/osd/msp_osd/uorb_to_msp.hpp | 3 --- 4 files changed, 4 insertions(+), 20 deletions(-) diff --git a/src/drivers/osd/msp_osd/msp_osd.cpp b/src/drivers/osd/msp_osd/msp_osd.cpp index 087ffce6c8..e6e7f236f7 100644 --- a/src/drivers/osd/msp_osd/msp_osd.cpp +++ b/src/drivers/osd/msp_osd/msp_osd.cpp @@ -350,16 +350,12 @@ void MspOsd::Run() home_position_s home_position{}; _home_position_sub.copy(&home_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_global_position_s vehicle_global_position{}; _vehicle_global_position_sub.copy(&vehicle_global_position); // construct and send message const auto msg = msp_osd::construct_COMP_GPS( home_position, - estimator_status, vehicle_global_position, _heartbeat); this->Send(MSP_COMP_GPS, &msg); @@ -379,17 +375,11 @@ void MspOsd::Run() sensor_gps_s vehicle_gps_position{}; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - estimator_status_s estimator_status{}; - _estimator_status_sub.copy(&estimator_status); - vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); // construct and send message - const auto msg = msp_osd::construct_ALTITUDE( - vehicle_gps_position, - estimator_status, - vehicle_local_position); + const auto msg = msp_osd::construct_ALTITUDE(vehicle_gps_position, vehicle_local_position); this->Send(MSP_ALTITUDE, &msg); } diff --git a/src/drivers/osd/msp_osd/msp_osd.hpp b/src/drivers/osd/msp_osd/msp_osd.hpp index ad233e2041..87918c7864 100644 --- a/src/drivers/osd/msp_osd/msp_osd.hpp +++ b/src/drivers/osd/msp_osd/msp_osd.hpp @@ -43,7 +43,6 @@ #include #include #include -#include #include #include #include @@ -145,7 +144,6 @@ private: // subscriptions to desired vehicle display information uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; - uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _home_position_sub{ORB_ID(home_position)}; uORB::Subscription _input_rc_sub{ORB_ID(input_rc)}; uORB::Subscription _log_message_sub{ORB_ID(log_message)}; diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index 1a78179855..e2cf9daf5e 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -365,7 +365,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, } msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat) { @@ -375,7 +374,8 @@ msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, // Calculate distance and direction to home if (home_position.valid_hpos && home_position.valid_lpos - && estimator_status.solution_status_flags & (1 << 4)) { + && (hrt_elapsed_time(&vehicle_global_position.timestamp) < 1_s)) { + float bearing_to_home = math::degrees(get_bearing_to_next_waypoint(vehicle_global_position.lat, vehicle_global_position.lon, home_position.lat, home_position.lon)); @@ -425,7 +425,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude) } msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position) { // initialize result @@ -438,7 +437,7 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, altitude.estimatedActualPosition = 0; } - if (estimator_status.solution_status_flags & (1 << 5)) { + if (vehicle_local_position.v_z_valid) { altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s } else { diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.hpp b/src/drivers/osd/msp_osd/uorb_to_msp.hpp index d841faf522..1f5cdb3b6d 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.hpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.hpp @@ -54,7 +54,6 @@ #include #include #include -#include #include #include @@ -94,7 +93,6 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, // construct an MSP_COMP_GPS struct msp_comp_gps_t construct_COMP_GPS(const home_position_s &home_position, - const estimator_status_s &estimator_status, const vehicle_global_position_s &vehicle_global_position, const bool heartbeat); @@ -103,7 +101,6 @@ msp_attitude_t construct_ATTITUDE(const vehicle_attitude_s &vehicle_attitude); // construct an MSP_ALTITUDE struct msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, - const estimator_status_s &estimator_status, const vehicle_local_position_s &vehicle_local_position); // construct an MSP_ESC_SENSOR_DATA struct From 96ee73f2952d8235ab32536ba6649490a2be7911 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 18 Oct 2023 03:05:54 -0700 Subject: [PATCH 04/14] px4_fmu-v6x:Rev 6 Sensors omit starting icm42688p, icm42670p, icm20649, icm20602 --- boards/px4/fmu-v6x/init/rc.board_sensors | 44 ++++++++++++------------ 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 5155fcf96c..d3ae62d8a4 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -81,32 +81,32 @@ else fi fi fi -fi -# Internal SPI bus ICM42688p -if ver hwtypecmp V6X009010 V6X010010 -then - icm42688p -R 12 -s start -else - if ver hwtypecmp V6X000010 - then - icm42688p -R 14 -s start - else - icm42688p -R 6 -s start - fi -fi - -if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 -then - # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 10 -s start -else + # Internal SPI bus ICM42688p if ver hwtypecmp V6X009010 V6X010010 then - icm20602 -R 6 -s start + icm42688p -R 12 -s start else - # Internal SPI bus ICM-20649 (hard-mounted) - icm20649 -R 14 -s start + if ver hwtypecmp V6X000010 + then + icm42688p -R 14 -s start + else + icm42688p -R 6 -s start + fi + fi + + if ver hwtypecmp V6X000003 V6X001003 V6X003003 V6X000004 V6X001004 V6X004003 V6X004004 V6X005003 V6X005004 + then + # Internal SPI bus ICM-42670-P (hard-mounted) + icm42670p -R 10 -s start + else + if ver hwtypecmp V6X009010 V6X010010 + then + icm20602 -R 6 -s start + else + # Internal SPI bus ICM-20649 (hard-mounted) + icm20649 -R 14 -s start + fi fi fi From 68bc90bab5b7a31d1b39de3381a446d5138c2172 Mon Sep 17 00:00:00 2001 From: Robbie Drage Date: Thu, 19 Oct 2023 08:52:43 +1300 Subject: [PATCH 05/14] uorb: fix Subscription::ChangeInstance() bug --- platforms/common/uORB/Subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9c17cab378..473c554866 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -76,7 +76,7 @@ void Subscription::unsubscribe() bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { + if (uORB::Manager::orb_device_node_exists(_orb_id, instance)) { // if desired new instance exists, unsubscribe from current unsubscribe(); _instance = instance; From e31e170438d1026c5ddbba624f04a0272cd0581d Mon Sep 17 00:00:00 2001 From: Titus Date: Thu, 19 Oct 2023 03:01:07 +0200 Subject: [PATCH 06/14] Tools/setup/ubuntu.sh: fix GCC_VER_STR failure handling (#22007) * Fixed an issue where if the GCC_VER_STR would not contain the right NUTTX_GCC_VERSION, the grep -c command would throw a failure, silently exiting the entire ubuntu.sh setup script --- Tools/setup/ubuntu.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index af7ec2fbff..a6bdf9b780 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -165,7 +165,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then source $HOME/.profile # load changed path for the case the script is reran before relogin if [ $(which arm-none-eabi-gcc) ]; then GCC_VER_STR=$(arm-none-eabi-gcc --version) - GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}") + GCC_FOUND_VER=$(echo $GCC_VER_STR | grep -c "${NUTTX_GCC_VERSION}" || true) fi if [[ "$GCC_FOUND_VER" == "1" ]]; then From 3ad2c641da9fa787e26afec74373bdb58712cea6 Mon Sep 17 00:00:00 2001 From: Engin Oksuz <38870269+enginksz@users.noreply.github.com> Date: Thu, 19 Oct 2023 17:01:47 +0300 Subject: [PATCH 07/14] README.md spelling mistake corrected --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 0665aef3fa..c2909b1a9e 100644 --- a/README.md +++ b/README.md @@ -114,7 +114,7 @@ These boards are maintained to be compatible with PX4-Autopilot by the Manufactu ### Community supported -These boards don't fully comply industry standards, and thus is solely maintained by the PX4 publc community members. +These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members. ### Experimental From f120ebcdc0f2110ccfaed1bb5552365f8e679be6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 20 Oct 2023 10:29:26 +1300 Subject: [PATCH 08/14] mavlink: properly set mission_type This was defaulted to 0 before which messed with transmitting geofence and rally items. Signed-off-by: Julian Oes --- src/modules/mavlink/mavlink_mission.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index c4bf9c42f8..95ee75817d 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -1561,6 +1561,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s * mavlink_mission_item->frame = mission_item->frame; mavlink_mission_item->command = mission_item->nav_cmd; mavlink_mission_item->autocontinue = mission_item->autocontinue; + mavlink_mission_item->mission_type = _mission_type; /* default mappings for generic commands */ if (mission_item->frame == MAV_FRAME_MISSION) { From ecb78ca207daa42820e5e259e7d36dc451a2029a Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Sat, 21 Oct 2023 19:25:45 +0200 Subject: [PATCH 09/14] new library for atmosphere calculations Signed-off-by: RomanBapst --- src/lib/CMakeLists.txt | 1 + src/lib/airspeed/airspeed.cpp | 30 +---- src/lib/airspeed/airspeed.h | 1 - src/lib/atmosphere/CMakeLists.txt | 37 +++++ src/lib/atmosphere/atmosphere.cpp | 81 +++++++++++ src/lib/atmosphere/atmosphere.h | 80 +++++++++++ src/lib/atmosphere/test_atmosphere.cpp | 126 ++++++++++++++++++ .../sensors/vehicle_air_data/CMakeLists.txt | 2 + .../vehicle_air_data/VehicleAirData.cpp | 38 ++---- .../vehicle_air_data/VehicleAirData.hpp | 2 - 10 files changed, 341 insertions(+), 57 deletions(-) create mode 100644 src/lib/atmosphere/CMakeLists.txt create mode 100644 src/lib/atmosphere/atmosphere.cpp create mode 100644 src/lib/atmosphere/atmosphere.h create mode 100644 src/lib/atmosphere/test_atmosphere.cpp diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 8210be8280..1094ff33c1 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) +add_subdirectory(atmosphere EXCLUDE_FROM_ALL) add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp index 24f7f01850..4d29be4a00 100644 --- a/src/lib/airspeed/airspeed.cpp +++ b/src/lib/airspeed/airspeed.cpp @@ -44,6 +44,9 @@ #include #include +#include + +using atmosphere::getDensityFromPressureAndTemp; float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_SENSOR_MODEL smodel, float tube_len, float tube_dia_mm, float differential_pressure, float pressure_ambient, float temperature_celsius) @@ -53,7 +56,7 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ } // air density in kg/m3 - const float rho_air = get_air_density(pressure_ambient, temperature_celsius); + const float rho_air = getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius); const float dp = fabsf(differential_pressure); float dp_tot = dp; @@ -153,18 +156,6 @@ float calc_IAS_corrected(enum AIRSPEED_COMPENSATION_MODEL pmodel, enum AIRSPEED_ break; } - // if (!PX4_ISFINITE(dp_tube)) { - // dp_tube = 0.0f; - // } - - // if (!PX4_ISFINITE(dp_pitot)) { - // dp_pitot = 0.0f; - // } - - // if (!PX4_ISFINITE(dv)) { - // dv = 0.0f; - // } - // computed airspeed without correction for inflow-speed at tip of pitot-tube const float airspeed_uncorrected = sqrtf(2.0f * dp_tot / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); @@ -192,7 +183,7 @@ float calc_TAS_from_CAS(float speed_calibrated, float pressure_ambient, float te temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius } - return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, + return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / getDensityFromPressureAndTemp(pressure_ambient, temperature_celsius)); } @@ -203,7 +194,7 @@ float calc_CAS_from_IAS(float speed_indicated, float scale) float calc_TAS(float total_pressure, float static_pressure, float temperature_celsius) { - float density = get_air_density(static_pressure, temperature_celsius); + float density = getDensityFromPressureAndTemp(static_pressure, temperature_celsius); if (density < 0.0001f || !PX4_ISFINITE(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; @@ -219,15 +210,6 @@ float calc_TAS(float total_pressure, float static_pressure, float temperature_ce } } -float get_air_density(float static_pressure, float temperature_celsius) -{ - if (!PX4_ISFINITE(temperature_celsius)) { - temperature_celsius = 15.f; // ICAO Standard Atmosphere 15 degrees Celsius - } - - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} - float calc_calibrated_from_true_airspeed(float speed_true, float air_density) { return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h index dba000a4e5..6b5161c5dd 100644 --- a/src/lib/airspeed/airspeed.h +++ b/src/lib/airspeed/airspeed.h @@ -127,7 +127,6 @@ __EXPORT float calc_TAS(float total_pressure, float static_pressure, float tempe * @param static_pressure ambient pressure in millibar * @param temperature_celcius air / ambient temperature in Celsius */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); /** * @brief Calculates calibrated airspeed from true airspeed and air density diff --git a/src/lib/atmosphere/CMakeLists.txt b/src/lib/atmosphere/CMakeLists.txt new file mode 100644 index 0000000000..eb25c8df4b --- /dev/null +++ b/src/lib/atmosphere/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2023 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(atmosphere + atmosphere.cpp + ) +px4_add_unit_gtest(SRC test_atmosphere.cpp LINKLIBS atmosphere) diff --git a/src/lib/atmosphere/atmosphere.cpp b/src/lib/atmosphere/atmosphere.cpp new file mode 100644 index 0000000000..66b98626f7 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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 atmosphere.cpp + * + */ + +#include +#include "atmosphere.h" +namespace atmosphere +{ + +static constexpr float kTempRefKelvin = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin +static constexpr float kTempGradient = -6.5f / 1000.f; // temperature gradient in degrees per meter +static constexpr float kPressRefSeaLevelPa = 101325.f; // pressure at sea level in Pa + +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius) +{ + return (pressure_pa / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS))); +} +float getPressureFromAltitude(const float altitude_m) +{ + + return kPressRefSeaLevelPa * powf((altitude_m * kTempGradient + kTempRefKelvin) / kTempRefKelvin, + -CONSTANTS_ONE_G / (kTempGradient * CONSTANTS_AIR_GAS_CONST)); +} +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa) +{ + // calculate altitude using the hypsometric equation + + const float pressure_ratio = pressure_pa / pressure_sealevel_pa; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + return (((powf(pressure_ratio, (-(kTempGradient * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * kTempRefKelvin) - + kTempRefKelvin) / kTempGradient; + +} +float getStandardTemperatureAtAltitude(float altitude_m) +{ + return 15.0f + kTempGradient * altitude_m; +} +} \ No newline at end of file diff --git a/src/lib/atmosphere/atmosphere.h b/src/lib/atmosphere/atmosphere.h new file mode 100644 index 0000000000..849a430347 --- /dev/null +++ b/src/lib/atmosphere/atmosphere.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2023 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 atmosphere.h + * + */ + +#ifndef PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ +#define PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ + +namespace atmosphere +{ + +// NOTE: We are currently only modelling the first layer of the US Standard Atmosphere 1976. +// This means that the functions are only valid up to an altitude of 11km. + +/** +* Calculate air density given air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param temperature_celsius ambient temperature in degrees Celsius +*/ +float getDensityFromPressureAndTemp(const float pressure_pa, const float temperature_celsius); + +/** +* Calculate standard airpressure given altitude. +* +* @param altitude_m altitude above MSL in meters in the standard atmosphere +*/ +float getPressureFromAltitude(const float altitude_m); + +/** +* Calculate altitude from air pressure and temperature. +* +* @param pressure_pa ambient pressure in Pa +* @param pressure_sealevel_pa sea level pressure in Pa +*/ +float getAltitudeFromPressure(float pressure_pa, float pressure_sealevel_pa); + +/** +* Get standard temperature at altitude. +* +* @param altitude_m Altitude msl in meters +* @return Standard temperature in degrees Celsius +*/ +float getStandardTemperatureAtAltitude(float altitude_m); +} + +#endif //PX4_SRC_LIB_ATMOSPHERE_ATMOSPHERE_H_ diff --git a/src/lib/atmosphere/test_atmosphere.cpp b/src/lib/atmosphere/test_atmosphere.cpp new file mode 100644 index 0000000000..0a79c89ca1 --- /dev/null +++ b/src/lib/atmosphere/test_atmosphere.cpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (C) 2023 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. + * + ****************************************************************************/ + +/** + * To run this test only use: make tests TESTFILTER=atmosphere + */ + +#include +#include +using namespace atmosphere; + +TEST(TestAtmosphere, pressureFromAltitude) +{ + // GIVEN pressure at seal level in standard atmosphere and sea level altitude + const float pressureAtSeaLevel = 101325.f; // Pa + float altitude = 0.0f; + + // WHEN we calculate pressure based on altitude + float pressure = getPressureFromAltitude(altitude); + + // THEN expect seal level pressure for standard atmosphere + EXPECT_FLOAT_EQ(pressure, pressureAtSeaLevel); + + // WHEN we are at 3000m altitude + altitude = 3000.0f; + pressure = getPressureFromAltitude(altitude); + + // THEN expect standard atmosphere pressure at 3000m (error of 10Pa corresponds to 1m altitude error in standard atmosphere when altitude is between 1000 and 2000m) + EXPECT_NEAR(pressure, 70120.f, 10.0f); +} + +TEST(TestAtmosphere, altitudeFromPressure) +{ + // GIVEN pressure at seal level in standard atmosphere + const float pressureAtSealevel = 101325.f; + float pressure = pressureAtSealevel; + + // WHEN we calculate altitude from pressure + float altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect resulting altitude to be near sea level + EXPECT_FLOAT_EQ(altitude, 0.0f); + + // GIVEN pressure is standard atmosphere pressure at 3000m + pressure = 70109.f; + + // WHEN we calculate altitude from pressure + altitude = getAltitudeFromPressure(pressure, pressureAtSealevel); + + // THEN expect altitude to be near 3000m + EXPECT_NEAR(altitude, 3000.0f, 0.5f); +} + +TEST(TestAtmosphere, DensityFromPressure) +{ +// GIVEN standard atmosphere at sea level + const float pressureAtSealevel = 101325.f; + const float tempSeaLevel = 15.f; + + // WHEN we calculate density from pressure and temperature + float density = getDensityFromPressureAndTemp(pressureAtSealevel, tempSeaLevel); + + // THEN expect density at sea level in standard atmosphere + EXPECT_NEAR(density, 1.225f, 0.001f); + + // GIVEN standard atmosphere at 3000m + const float pressure = 70109.f; + const float tempAt3000m = -4.5f; + + // WHEN we calculate density from pressure and temperature + density = getDensityFromPressureAndTemp(pressure, tempAt3000m); + + // THEN expect density at 3000m in standard atmosphere + EXPECT_NEAR(density, 0.9091f, 0.001f); +} + +TEST(TestAtmosphere, StandardTemperature) +{ + // GIVEN standard atmosphere at sea level + float altitude = 0.f; + + // WHEN we calculate standard temperature from altitude + float temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at sea level + EXPECT_NEAR(temperature, 15.f, 0.001f); + + // GIVEN standard atmosphere at 3000m + altitude = 3000.f; + + // WHEN we calculate standard temperature from altitude + temperature = getStandardTemperatureAtAltitude(altitude); + + // THEN expect standard temperature at 3000m + EXPECT_NEAR(temperature, -4.5f, 0.001f); +} \ No newline at end of file diff --git a/src/modules/sensors/vehicle_air_data/CMakeLists.txt b/src/modules/sensors/vehicle_air_data/CMakeLists.txt index c8457cc2df..909f249a7c 100644 --- a/src/modules/sensors/vehicle_air_data/CMakeLists.txt +++ b/src/modules/sensors/vehicle_air_data/CMakeLists.txt @@ -40,4 +40,6 @@ target_link_libraries(vehicle_air_data data_validator px4_work_queue sensor_calibration + PUBLIC + atmosphere ) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 472d228c58..4c15c74a2a 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -36,11 +36,14 @@ #include #include #include +#include + namespace sensors { using namespace matrix; +using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; @@ -189,8 +192,9 @@ void VehicleAirData::Run() // 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, PressureToAltitude(pressure_corrected)}; + 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; @@ -251,11 +255,11 @@ void VehicleAirData::Run() const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; - float altitude = PressureToAltitude(pressure_pa, temperature); + const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; + const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - float air_density = pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; @@ -295,32 +299,6 @@ void VehicleAirData::Run() perf_end(_cycle_perf); } -float VehicleAirData::PressureToAltitude(float pressure_pa, float temperature) const -{ - // calculate altitude using the hypsometric equation - static constexpr float T1 = 15.f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin - static constexpr float a = -6.5f / 1000.f; // temperature gradient in degrees per metre - - // current pressure at MSL in kPa (QNH in hPa) - const float p1 = _param_sens_baro_qnh.get() * 0.1f; - - // measured pressure in kPa - const float p = pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - float altitude = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; - - return altitude; -} - void VehicleAirData::CheckFailover(const hrt_abstime &time_now_us) { // check failover and report (save failover report for a cycle where parameters didn't update) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 390f25767f..fd28de34f8 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -79,8 +79,6 @@ private: bool ParametersUpdate(bool force = false); void UpdateStatus(); - float PressureToAltitude(float pressure_pa, float temperature = 15.f) const; - static constexpr int MAX_SENSOR_COUNT = 4; uORB::Publication _sensors_status_baro_pub{ORB_ID(sensors_status_baro)}; From 63b5c790b79895f66475bc098774ade079367557 Mon Sep 17 00:00:00 2001 From: Niklas Hauser Date: Wed, 31 May 2023 17:07:06 +0200 Subject: [PATCH 10/14] iotimer: Enable timer when configuring input capture We provide a latency measurement in the input capture handler. However, since the timer was not enabled, none of the counter were running therefore all counters were zero, thus latency was also zero. Since the HRT is used to provide a timestamp, the lack of the running timer was never noticed. After enabling the timer, latency now correctly shows 9-10 counts. --- .../nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c index 5a72cc9f5e..4246ea3585 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/io_pins/io_timer.c @@ -898,11 +898,13 @@ int io_timer_set_enable(bool state, io_timer_channel_mode_t mode, io_timer_chann case IOTimerChanMode_Dshot: dier_bit = 0; - cr1_bit = state ? GTIM_CR1_CEN : 0; - break; - case IOTimerChanMode_PWMIn: + /* fallthrough */ case IOTimerChanMode_Capture: + cr1_bit = state ? GTIM_CR1_CEN : 0; + + /* fallthrough */ + case IOTimerChanMode_PWMIn: break; default: From f68f88b97c77d38054d54696fca5ef1a3e91822b Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Sun, 22 Oct 2023 01:39:45 +0800 Subject: [PATCH 11/14] driver/pca9685_pwm_output: bugfixs & support outputting in duty-cycle mode (#21528) - make it work again - also supports Kconfig based clk source selection - adapt to recent changes of default PWM limits - support outputting in duty-cycle mode - i2c addr use use hex representation - revert back to common min/max value & move duty-cycle mode to advanced --- boards/scumaker/pilotpi/default.px4board | 2 + src/drivers/pca9685_pwm_out/Kconfig | 18 +- src/drivers/pca9685_pwm_out/PCA9685.cpp | 289 ++++++++++------------- src/drivers/pca9685_pwm_out/PCA9685.h | 122 +++++----- src/drivers/pca9685_pwm_out/main.cpp | 255 +++++++++++--------- src/drivers/pca9685_pwm_out/module.yaml | 86 +++++-- 6 files changed, 413 insertions(+), 359 deletions(-) diff --git a/boards/scumaker/pilotpi/default.px4board b/boards/scumaker/pilotpi/default.px4board index 0de2a4311a..ccb3bb3ce9 100644 --- a/boards/scumaker/pilotpi/default.px4board +++ b/boards/scumaker/pilotpi/default.px4board @@ -16,6 +16,8 @@ CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL=y +CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ=25000000 CONFIG_COMMON_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y diff --git a/src/drivers/pca9685_pwm_out/Kconfig b/src/drivers/pca9685_pwm_out/Kconfig index 84cc71cda7..ad7c25b8d7 100644 --- a/src/drivers/pca9685_pwm_out/Kconfig +++ b/src/drivers/pca9685_pwm_out/Kconfig @@ -2,4 +2,20 @@ menuconfig DRIVERS_PCA9685_PWM_OUT bool "pca9685_pwm_out" default n ---help--- - Enable support for pca9685_pwm_out \ No newline at end of file + Enable support for pca9685_pwm_out + +if DRIVERS_PCA9685_PWM_OUT + config PCA9685_USE_EXTERNAL_CRYSTAL + bool "Use external crystal for clock reference" + default n + + config PCA9685_EXTERNAL_CRYSTAL_FREQ + int "External crystal frequency" + depends on PCA9685_USE_EXTERNAL_CRYSTAL + default 25000000 + + config PCA9685_INTERNAL_CRYSTAL_FREQ + int "Corrected frequency of internal oscillator" + depends on !PCA9685_USE_EXTERNAL_CRYSTAL + default 26075000 +endif \ No newline at end of file diff --git a/src/drivers/pca9685_pwm_out/PCA9685.cpp b/src/drivers/pca9685_pwm_out/PCA9685.cpp index c2a1af4b16..7eccd6c04b 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.cpp +++ b/src/drivers/pca9685_pwm_out/PCA9685.cpp @@ -32,23 +32,66 @@ ****************************************************************************/ #include +#include #include #include "PCA9685.h" -#include + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_EXTERNAL_CRYSTAL_FREQ +#else +#define PCA9685_CLOCK_REFERENCE CONFIG_PCA9685_INTERNAL_CRYSTAL_FREQ +#endif + +#define PCA9685_DEFAULT_MODE1_CFG PCA9685_MODE1_AI_MASK +#define PCA9685_DEFAULT_MODE2_CFG PCA9685_MODE2_OUTDRV_MASK using namespace drv_pca9685_pwm; PCA9685::PCA9685(int bus, int addr): - I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000) + I2C(DRV_PWM_DEVTYPE_PCA9685, MODULE_NAME, bus, addr, 400000), + currentFreq(0.0) { } -int PCA9685::Stop() +int PCA9685::init() { - disableAllOutput(); - stopOscillator(); + int ret = I2C::init(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {}; + + buf[0] = PCA9685_REG_MODE1; + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK; // put into sleep mode + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#ifdef CONFIG_PCA9685_USE_EXTERNAL_CRYSTAL + buf[1] = PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK | PCA9685_MODE1_EXTCLK_MASK; + ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + +#endif + + buf[0] = PCA9685_REG_MODE2; + buf[1] = PCA9685_DEFAULT_MODE2_CFG; + ret = transfer(buf, 2, nullptr, 0); + + if (OK != ret) { + PX4_ERR("init: i2c::transfer returned %d", ret); + return ret; + } + return PX4_OK; } @@ -63,198 +106,118 @@ int PCA9685::updatePWM(const uint16_t *outputs, unsigned num_outputs) memcpy(out, outputs, sizeof(uint16_t) * num_outputs); for (unsigned i = 0; i < num_outputs; ++i) { - out[i] = (uint16_t)roundl((out[i] * _Freq * PCA9685_PWM_RES / (float)1e6)); // convert us to 12 bit resolution + out[i] = calcRawFromPulse(out[i]); } - setPWM(num_outputs, out); - - return 0; + return writePWM(0, out, num_outputs); } -int PCA9685::setFreq(float freq) +int PCA9685::updateFreq(float freq) { - uint16_t realResolution = floorl((float)PCA9685_CLOCK_FREQ / freq); + uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_REFERENCE / freq / PCA9685_PWM_RES) - 1; - if (realResolution < PCA9685_PWM_RES) { // unable to provide enough resolution - PX4_DEBUG("frequency too high"); - return -EINVAL; + if (divider > 0x00FF) { + PX4_ERR("frequency too low"); + return PX4_ERROR; } - uint16_t divider = (uint16_t)round((float)PCA9685_CLOCK_FREQ / freq / PCA9685_PWM_RES) - 1; - - if (divider > 0x00FF) { // out of divider - PX4_DEBUG("frequency too low"); - return -EINVAL; + if (divider < 0x0003) { + PX4_ERR("frequency too high"); + return PX4_ERROR; } - float freq_err = ((float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) - - (float)(freq * PCA9685_PWM_RES)) - / (float)(freq * PCA9685_PWM_RES); // actually asked for (freq * PCA9685_PWM_RES) - - if (fabsf(freq_err) > 0.01f) { // TODO decide threshold - PX4_WARN("frequency error too large: %.4f", (double)freq_err); - // should we return an error? - } - - _Freq = (float)PCA9685_CLOCK_FREQ / (float)(divider + (uint16_t)1) / PCA9685_PWM_RES; // use actual pwm freq instead. - - setDivider(divider); - - return PX4_OK; + currentFreq = (float)PCA9685_CLOCK_REFERENCE / (float)((divider + 1) * 4096); + PX4_INFO("PCA9685 PWM frequency: target=%.2f real=%.2f", (double)freq, (double)currentFreq); + return setDivider(divider); } -int PCA9685::initReg() +int PCA9685::updateRAW(const uint16_t *outputs, unsigned int num_outputs) { - uint8_t buf[2] = {}; + return writePWM(0, outputs, num_outputs); +} - buf[0] = PCA9685_REG_MODE1; - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); // make sure oscillator is disabled +int PCA9685::setAllPWM(uint16_t output) +{ + uint16_t val = (uint16_t)roundl((output * currentFreq * PCA9685_PWM_RES / (float)1e6)); + uint8_t buf[] = { + PCA9685_REG_ALLLED_ON_L, + 0x00, 0x00, + (uint8_t)(val & (uint8_t)0xFF), + val != 0 ? (uint8_t)(val >> 8) : (uint8_t)PCA9685_LED_ON_FULL_ON_OFF_MASK + }; + return transfer(buf, sizeof(buf), nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } +int PCA9685::sleep() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_SLEEP_MASK + }; + return transfer(buf, 2, nullptr, 0); +} - ret = transfer(buf, 2, nullptr, 0); // enable EXTCLK if possible +int PCA9685::wake() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG + }; + return transfer(buf, 2, nullptr, 0); +} - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - buf[0] = PCA9685_REG_MODE2; - buf[1] = DEFAULT_MODE2_CFG; - ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_ERR("init: i2c::transfer returned %d", ret); - return ret; - } - - return PX4_OK; +int PCA9685::doRestart() +{ + uint8_t buf[2] = { + PCA9685_REG_MODE1, + PCA9685_DEFAULT_MODE1_CFG | PCA9685_MODE1_RESTART_MASK + }; + return transfer(buf, 2, nullptr, 0); } int PCA9685::probe() { - return I2C::probe(); + int ret = I2C::probe(); + + if (ret != PX4_OK) { return ret; } + + uint8_t buf[2] = {0x00}; + return transfer(buf, 2, buf, 1); } -void PCA9685::setPWM(uint8_t channel, const uint16_t &value) -{ - if (value >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } - - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_LED0 + channel * PCA9685_REG_LED_INCREMENT; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = (uint8_t)(value & (uint8_t)0xFF); - buf[4] = value != 0 ? ((uint8_t)(value >> (uint8_t)8)) : PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } -} - -void PCA9685::setPWM(uint8_t channel_count, const uint16_t *value) +int PCA9685::writePWM(uint8_t idx, const uint16_t *value, uint8_t num) { uint8_t buf[PCA9685_PWM_CHANNEL_COUNT * PCA9685_REG_LED_INCREMENT + 1] = {}; - buf[0] = PCA9685_REG_LED0; - - for (int i = 0; i < channel_count; ++i) { - if (value[i] >= 4096) { - PX4_DEBUG("invalid pwm value"); - return; - } + buf[0] = PCA9685_REG_LED0 + PCA9685_REG_LED_INCREMENT * idx; + for (int i = 0; i < num; ++i) { buf[1 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; - buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & (uint8_t)0xFF); - buf[4 + i * PCA9685_REG_LED_INCREMENT] = value[i] != 0 ? ((uint8_t)(value[i] >> (uint8_t)8)) : - PCA9685_LED_ON_FULL_ON_OFF_MASK; + + if (value[i] == 0) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + + } else if (value[i] == 4096) { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = PCA9685_LED_ON_FULL_ON_OFF_MASK; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[4 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + + } else { + buf[2 + i * PCA9685_REG_LED_INCREMENT] = 0x00; + buf[3 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] & 0xFF); + buf[4 + i * PCA9685_REG_LED_INCREMENT] = (uint8_t)(value[i] >> 8); + } } - int ret = transfer(buf, channel_count * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("setPWM: i2c::transfer returned %d", ret); - } + return transfer(buf, num * PCA9685_REG_LED_INCREMENT + 1, nullptr, 0); } -void PCA9685::disableAllOutput() -{ - uint8_t buf[5] = {}; - buf[0] = PCA9685_REG_ALLLED_ON_L; - buf[1] = 0x00; - buf[2] = 0x00; - buf[3] = 0x00; - buf[4] = PCA9685_LED_ON_FULL_ON_OFF_MASK; - - int ret = transfer(buf, 5, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - } -} - -void PCA9685::setDivider(uint8_t value) +int PCA9685::setDivider(uint8_t value) { uint8_t buf[2] = {}; buf[0] = PCA9685_REG_PRE_SCALE; buf[1] = value; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::stopOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // set to sleep - buf[1] = DEFAULT_MODE1_CFG; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::startOscillator() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("startOscillator: i2c::transfer returned %d", ret); - return; - } -} - -void PCA9685::triggerRestart() -{ - uint8_t buf[2] = {PCA9685_REG_MODE1}; - - // clear sleep bit, with restart bit = 0 - buf[1] = DEFAULT_MODE1_CFG & (~PCA9685_MODE1_SLEEP_MASK); - buf[1] |= PCA9685_MODE1_RESTART_MASK; - int ret = transfer(buf, 2, nullptr, 0); - - if (OK != ret) { - PX4_DEBUG("triggerRestart: i2c::transfer returned %d", ret); - return; - } + return transfer(buf, 2, nullptr, 0); } diff --git a/src/drivers/pca9685_pwm_out/PCA9685.h b/src/drivers/pca9685_pwm_out/PCA9685.h index 5c7740164c..e8bd5eb4d8 100644 --- a/src/drivers/pca9685_pwm_out/PCA9685.h +++ b/src/drivers/pca9685_pwm_out/PCA9685.h @@ -34,10 +34,7 @@ #pragma once #include #include -#include - -namespace drv_pca9685_pwm -{ +#include #define PCA9685_REG_MODE1 0x00 // Mode register 1 #define PCA9685_REG_MODE2 0x01 // Mode register 2 @@ -82,93 +79,98 @@ namespace drv_pca9685_pwm // PRE_SCALE register #define PCA9685_PRE_SCALE_MASK 0xFF +// common sense #define PCA9685_PWM_CHANNEL_COUNT 16 -#define PCA9685_PWM_RES 4096 //Resolution 4096=12bit -/* This should be 25000000 ideally, - * but it seems most chips have its oscillator working at a higher frequency - * Reference: https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/blob/6664ce936210eea53259b814062009d9569a4213/Adafruit_PWMServoDriver.h#L66 */ -#define PCA9685_CLOCK_INT 26075000.0 //25MHz internal clock -#ifndef PCA9685_CLOCK_EXT -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_INT // use int clk -#else -#define PCA9685_CLOCK_FREQ PCA9685_CLOCK_EXT // use ext clk -#endif +#define PCA9685_PWM_RES 4096 -#define PCA9685_DEVICE_BASE_PATH "/dev/pca9685" -#define PWM_DEFAULT_FREQUENCY 50 // default pwm frequency +namespace drv_pca9685_pwm +{ //! Main class that exports features for PCA9685 chip class PCA9685 : public device::I2C { public: PCA9685(int bus, int addr); + ~PCA9685() override = default; - int Stop(); + int init() override; /* - * outputs formatted to us. + * Write new PWM value to device + * + * *output: pulse width, us */ int updatePWM(const uint16_t *outputs, unsigned num_outputs); - int setFreq(float freq); - - ~PCA9685() override = default; - - int initReg(); - - inline float getFrequency() {return _Freq;} - /* - * disable all of the output + * Set PWM frequency to new value. + * + * Only a few of precious frequency can be set, while others will be rounded to the nearest possible value. + * + * Only allowed when PCA9685 is put into sleep mode + * + * freq: Hz */ - void disableAllOutput(); + int updateFreq(float freq); /* - * turn off oscillator - */ - void stopOscillator(); - - /* - * turn on oscillator + * Write new PWM value to device, in raw counter value + * + * *output: 0~4095 */ - void startOscillator(); + int updateRAW(const uint16_t *outputs, unsigned num_outputs); /* - * turn on output + * Get the real frequency */ - void triggerRestart(); + float inline getFreq() {return currentFreq;} + + uint16_t inline calcRawFromPulse(uint16_t pulse_width) + { + return (uint16_t)roundl((pulse_width * currentFreq * PCA9685_PWM_RES / (float)1e6)); + } + + /* + * Set PWM value on all channels at once + */ + int setAllPWM(uint16_t output); + + /* + * Put PCA9685 into sleep mode + * + * This will disable the clock reference inside PCA9685 + */ + int sleep(); + + /* + * Put PCA9685 out of sleep mode. + * + * Must wait 500 us for oscillator stabilization before outputting anything + */ + int wake(); + + /* + * If PCA9685 is put into sleep without clearing all the outputs, + * then the restart command will be available, and it can bring back PWM output without the + * need of updatePWM() call. + */ + int doRestart(); protected: int probe() override; -#ifdef PCA9685_CLOCL_EXT - static const uint8_t DEFAULT_MODE1_CFG = 0x70; // Auto-Increment, Sleep, EXTCLK -#else - static const uint8_t DEFAULT_MODE1_CFG = 0x30; // Auto-Increment, Sleep -#endif - static const uint8_t DEFAULT_MODE2_CFG = 0x04; // totem pole - - float _Freq = PWM_DEFAULT_FREQUENCY; - - /** - * set PWM value for a channel[0,15]. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel, const uint16_t &value); - - /** - * set all PWMs in a single I2C transmission. - * value should be range of 0-4095 - */ - void setPWM(uint8_t channel_count, const uint16_t *value); - /* * set clock divider */ - void setDivider(uint8_t value); + int setDivider(uint8_t value); + + /* + * Write PWM value to PCA9685 + */ + int writePWM(uint8_t idx, const uint16_t *value, uint8_t num); private: - + float currentFreq; }; } diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 4c3ee60c54..0e9bfa63df 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -33,9 +33,10 @@ /** * @file pca9685/main.cpp - * A cross-platform driver and wrapper for pca9685. - * Designed to support all control-groups by binding to correct mixer files - * @author SalimTerryLi + * + * This file serves as the wrapper layer for PCA9685 driver, working with parameters + * and scheduling stuffs on PX4 side. + * */ #include @@ -45,11 +46,10 @@ #include #include #include +#include #include "PCA9685.h" -#include - #define PCA9685_DEFAULT_IICBUS 1 #define PCA9685_DEFAULT_ADDRESS (0x40) @@ -59,26 +59,25 @@ using namespace time_literals; class PCA9685Wrapper : public ModuleBase, public OutputModuleInterface { public: - PCA9685Wrapper(int schd_rate_limit = 400); - ~PCA9685Wrapper() override ; + PCA9685Wrapper(); + ~PCA9685Wrapper() override; + PCA9685Wrapper(const PCA9685Wrapper &) = delete; + PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; int init(); - /** @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); bool updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) override; - PCA9685Wrapper(const PCA9685Wrapper &) = delete; - PCA9685Wrapper operator=(const PCA9685Wrapper &) = delete; - int print_status() override; +protected: + void updateParams() override; + private: perf_counter_t _cycle_perf; @@ -86,40 +85,37 @@ private: INIT, WAIT_FOR_OSC, RUNNING - }; - STATE _state{STATE::INIT}; - // used to compare and cancel unecessary scheduling changes caused by parameter update - int32_t _last_fetched_Freq = -1; - // If this value is above zero, then change freq and scheduling in running state. - float _targetFreq = -1.0f; + } state{STATE::INIT}; + PCA9685 *pca9685 = nullptr; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + MixingOutput _mixing_output { + "PCA9685", + PCA9685_PWM_CHANNEL_COUNT, + *this, + MixingOutput::SchedulingPolicy::Disabled, + true + }; + + float param_pwm_freq, previous_pwm_freq; + float param_schd_rate, previous_schd_rate; + uint32_t param_duty_mode; void Run() override; - -protected: - int _schd_rate_limit = 400; - - PCA9685 *pca9685 = nullptr; // driver handle. - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - - MixingOutput _mixing_output{"PCA9685", PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true}; }; -PCA9685Wrapper::PCA9685Wrapper(int schd_rate_limit) : +PCA9685Wrapper::PCA9685Wrapper() : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), - _schd_rate_limit(schd_rate_limit) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { } PCA9685Wrapper::~PCA9685Wrapper() { - if (pca9685 != nullptr) { // normally this should not be called. - PX4_DEBUG("Destruction of PCA9685Wrapper without pwmDevice unloaded!"); - pca9685->Stop(); // force stop + if (pca9685 != nullptr) { + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; - pca9685 = nullptr; } perf_free(_cycle_perf); @@ -129,18 +125,7 @@ int PCA9685Wrapper::init() { int ret = pca9685->init(); - if (ret != PX4_OK) { - return ret; - } - - param_t param_h = param_find("PCA9685_RATE"); - - if (param_h != PARAM_INVALID) { - param_get(param_h, &_targetFreq); - - } else { - PX4_DEBUG("PARAM_INVALID: PCA9685_RATE"); - } + if (ret != PX4_OK) { return ret; } this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); @@ -154,7 +139,26 @@ int PCA9685Wrapper::init() bool PCA9685Wrapper::updateOutputs(bool stop_motors, uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - return pca9685->updatePWM(outputs, num_outputs) == 0 ? true : false; + if (state != STATE::RUNNING) { return false; } + + uint16_t low_level_outputs[PCA9685_PWM_CHANNEL_COUNT] = {}; + num_outputs = num_outputs > PCA9685_PWM_CHANNEL_COUNT ? PCA9685_PWM_CHANNEL_COUNT : num_outputs; + + for (uint8_t i = 0; i < num_outputs; ++i) { + if (param_duty_mode & (1 << i)) { + low_level_outputs[i] = outputs[i]; + + } else { + low_level_outputs[i] = pca9685->calcRawFromPulse(outputs[i]); + } + } + + if (pca9685->updateRAW(low_level_outputs, num_outputs) != PX4_OK) { + PX4_ERR("Failed to write PWM to PCA9685"); + return false; + } + + return true; } void PCA9685Wrapper::Run() @@ -163,7 +167,8 @@ void PCA9685Wrapper::Run() ScheduleClear(); _mixing_output.unregister(); - pca9685->Stop(); + pca9685->setAllPWM(0); + pca9685->sleep(); delete pca9685; pca9685 = nullptr; @@ -171,44 +176,27 @@ void PCA9685Wrapper::Run() return; } - perf_begin(_cycle_perf); - - switch (_state) { + switch (state) { case STATE::INIT: - pca9685->initReg(); + updateParams(); + pca9685->updateFreq(param_pwm_freq); + previous_pwm_freq = param_pwm_freq; + previous_schd_rate = param_schd_rate; - if (_targetFreq > 0.0f) { - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency to %.2f, fall back to 50Hz", (double)_targetFreq); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - - } else { - // should not happen - PX4_ERR("INIT failed: invalid initial frequency settings"); - } - - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; + pca9685->wake(); + state = STATE::WAIT_FOR_OSC; ScheduleDelayed(500); break; case STATE::WAIT_FOR_OSC: { - pca9685->triggerRestart(); // start actual outputting - _state = STATE::RUNNING; - float schedule_rate = pca9685->getFrequency(); - - if (_schd_rate_limit < pca9685->getFrequency()) { - schedule_rate = _schd_rate_limit; - } - - ScheduleOnInterval(1000000 / schedule_rate, 1000000 / schedule_rate); + state = STATE::RUNNING; + ScheduleOnInterval(1000000 / param_schd_rate, 0); } break; case STATE::RUNNING: + perf_begin(_cycle_perf); + _mixing_output.update(); // check for parameter updates @@ -219,30 +207,36 @@ void PCA9685Wrapper::Run() // update parameters from storage updateParams(); + + // apply param updates + if ((float)fabs(previous_pwm_freq - param_pwm_freq) > 0.01f) { + previous_pwm_freq = param_pwm_freq; + + ScheduleClear(); + + pca9685->sleep(); + pca9685->updateFreq(param_pwm_freq); + pca9685->wake(); + + // update of PWM freq will always trigger scheduling change + previous_schd_rate = param_schd_rate; + + state = STATE::WAIT_FOR_OSC; + ScheduleDelayed(500); + + } else if ((float)fabs(previous_schd_rate - param_schd_rate) > 0.01f) { + // case when PWM freq not changed but scheduling rate does + previous_schd_rate = param_schd_rate; + ScheduleClear(); + ScheduleOnInterval(1000000 / param_schd_rate, 1000000 / param_schd_rate); + } } _mixing_output.updateSubscriptions(false); - if (_targetFreq > 0.0f) { // check if frequency should be changed - ScheduleClear(); - pca9685->disableAllOutput(); - pca9685->stopOscillator(); - - if (pca9685->setFreq(_targetFreq) != PX4_OK) { - PX4_ERR("failed to set pwm frequency, fall back to 50Hz"); - pca9685->setFreq(50.0f); // this should not fail - } - - _targetFreq = -1.0f; - pca9685->startOscillator(); - _state = STATE::WAIT_FOR_OSC; - ScheduleDelayed(500); - } - + perf_end(_cycle_perf); break; } - - perf_end(_cycle_perf); } int PCA9685Wrapper::print_usage(const char *reason) @@ -254,27 +248,24 @@ int PCA9685Wrapper::print_usage(const char *reason) PRINT_MODULE_DESCRIPTION( R"DESCR_STR( ### Description -This module is responsible for generate pwm pulse with PCA9685 chip. +This is a PCA9685 PWM output driver. -It listens on the actuator_controls topics, does the mixing and writes the PWM outputs. +It runs on I2C workqueue which is asynchronous with FC control loop, +fetching the latest mixing result and write them to PCA9685 at its scheduling ticks. -### Implementation -This module depends on ModuleBase and OutputModuleInterface. -IIC communication is based on CDev::I2C +It can do full 12bits output as duty-cycle mode, while also able to output precious pulse width +that can be accepted by most ESCs and servos. ### Examples It is typically started with: -$ pca9685_pwm_out start -a 64 -b 1 +$ pca9685_pwm_out start -a 0x40 -b 1 -The number X can be acquired by executing -`pca9685_pwm_out status` when this driver is running. )DESCR_STR"); PRINT_MODULE_USAGE_NAME("pca9685_pwm_out", "driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the task"); - PRINT_MODULE_USAGE_PARAM_INT('a',64,0,255,"device address on this bus",true); + PRINT_MODULE_USAGE_PARAM_STRING('a',"0x40","","7-bits I2C address of PCA9685",true); PRINT_MODULE_USAGE_PARAM_INT('b',1,0,255,"bus that pca9685 is connected to",true); - PRINT_MODULE_USAGE_PARAM_INT('r',400,50,400,"schedule rate limit",true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; @@ -282,39 +273,42 @@ The number X can be acquired by executing int PCA9685Wrapper::print_status() { int ret = ModuleBase::print_status(); - PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, true frequency %.5f", + PX4_INFO("PCA9685 @I2C Bus %d, address 0x%.2x, real frequency %.2f", pca9685->get_device_bus(), pca9685->get_device_address(), - (double)(pca9685->getFrequency())); + (double)(pca9685->getFreq())); return ret; } -int PCA9685Wrapper::custom_command(int argc, char **argv) { // only for test use +int PCA9685Wrapper::custom_command(int argc, char **argv) { return PX4_OK; } int PCA9685Wrapper::task_spawn(int argc, char **argv) { - int ch; int address=PCA9685_DEFAULT_ADDRESS; int iicbus=PCA9685_DEFAULT_IICBUS; - int schd_rate_limit=400; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "a:b:r:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': - address = atoi(myoptarg); + errno = 0; + address = strtol(myoptarg, nullptr, 16); + if (errno != 0) { + PX4_WARN("Invalid address"); + return PX4_ERROR; + } break; case 'b': - iicbus = atoi(myoptarg); - break; - - case 'r': - schd_rate_limit = atoi(myoptarg); + iicbus = strtol(myoptarg, nullptr, 10); + if (errno != 0) { + PX4_WARN("Invalid bus"); + return PX4_ERROR; + } break; case '?': @@ -326,7 +320,7 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { } } - auto *instance = new PCA9685Wrapper(schd_rate_limit); + auto *instance = new PCA9685Wrapper(); if (instance) { _object.store(instance); @@ -358,6 +352,31 @@ int PCA9685Wrapper::task_spawn(int argc, char **argv) { return PX4_ERROR; } +void PCA9685Wrapper::updateParams() { + ModuleParams::updateParams(); + + param_t param = param_find("PCA9685_SCHD_HZ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_schd_rate); + } else { + PX4_ERR("param PCA9685_SCHD_HZ not found"); + } + + param = param_find("PCA9685_PWM_FREQ"); + if (param != PARAM_INVALID) { + param_get(param, ¶m_pwm_freq); + } else { + PX4_ERR("param PCA9685_PWM_FREQ not found"); + } + + param = param_find("PCA9685_DUTY_EN"); + if (param != PARAM_INVALID) { + param_get(param, (int32_t*)¶m_duty_mode); + } else { + PX4_ERR("param PCA9685_DUTY_EN not found"); + } +} + extern "C" __EXPORT int pca9685_pwm_out_main(int argc, char *argv[]){ return PCA9685Wrapper::main(argc, argv); } diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 78801fbdb6..198810f8cb 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -5,23 +5,75 @@ actuator_output: channel_label: 'Channel' standard_params: disarmed: { min: 800, max: 2200, default: 1000 } - min: { min: 800, max: 1400, default: 1000 } - max: { min: 1600, max: 2200, default: 2000 } + min: { min: 800, max: 1400, default: 1100 } + max: { min: 1600, max: 2200, default: 1900 } failsafe: { min: 800, max: 2200 } + custom_params: + - name: 'DUTY_EN' + label: "Duty-Cycle\n Mode" + index_offset: -1 + show_as: bitset + advanced: true num_channels: 16 + parameters: - - group: PCA9685 - definitions: - PCA9685_RATE: - description: - short: PWM frequency for PCA9685 - long: | - Update rate at which the PWM signal is sent to the ESC. - type: float - decimal: 1 - increment: 0.1 - default: 50 - min: 50 - max: 450 - unit: Hz - reboot_required: true + - group: Actuator Outputs + definitions: + PCA9685_SCHD_HZ: + description: + short: PWM update rate + long: | + Controls the update rate of PWM output. + Flight Controller will inform those numbers of update events in a second, to PCA9685. + Higher update rate will consume more I2C bandwidth, which may even lead to worse + output latency, or completely block I2C bus. + type: float + decimal: 2 + min: 50.0 + max: 400.0 + default: 50.0 + PCA9685_PWM_FREQ: + description: + short: PWM cycle frequency + long: | + Controls the PWM frequency at timing perspective. + This is independent from PWM update frequency, as PCA9685 is capable to output + without being continuously commanded by FC. + Higher frequency leads to more accurate pulse width, but some ESCs and servos may not support it. + This parameter should be set to the same value as PWM update rate in most case. + This parameter MUST NOT exceed upper limit of 400.0, if any outputs as generic 1000~2000us + pulse width is desired. Frequency higher than 400 only makes sense in duty-cycle mode. + type: float + decimal: 2 + min: 23.8 + max: 1525.87 + default: 50.0 + PCA9685_DUTY_EN: + description: + short: Put the selected channels into Duty-Cycle output mode + long: | + The driver will output standard pulse-width encoded signal without this bit set. + To make PCA9685 output in duty-cycle fashion, please enable the corresponding + channel bit here and adjusting standard params to suit your need. + The driver will have 12bits resolution for duty-cycle output. That means to achieve 0% to 100% + output range on one channel, the corresponding params MIN and MAX for the channel should be set + to 0 and 4096. Other standard params follows the same rule. + type: bitmask + bit: + 0: Put CH1 to Duty-Cycle mode + 1: Put CH2 to Duty-Cycle mode + 2: Put CH3 to Duty-Cycle mode + 3: Put CH4 to Duty-Cycle mode + 4: Put CH5 to Duty-Cycle mode + 5: Put CH6 to Duty-Cycle mode + 6: Put CH7 to Duty-Cycle mode + 7: Put CH8 to Duty-Cycle mode + 8: Put CH9 to Duty-Cycle mode + 9: Put CH10 to Duty-Cycle mode + 10: Put CH11 to Duty-Cycle mode + 11: Put CH12 to Duty-Cycle mode + 12: Put CH13 to Duty-Cycle mode + 13: Put CH14 to Duty-Cycle mode + 14: Put CH15 to Duty-Cycle mode + 15: Put CH16 to Duty-Cycle mode + default: 0 From eed2870fd885bb28a97bc3d8e6d456b76b2c37f0 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Wed, 25 Oct 2023 15:04:56 +0200 Subject: [PATCH 12/14] ekf2: fix optical_flow_vel publication We should otherwise call this publication before the aid_src publisher that sets the timestamp. Having it separate avoids this ordering constraint. --- src/modules/ekf2/EKF2.cpp | 4 +++- src/modules/ekf2/EKF2.hpp | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 72d8244148..18b5e5b3cb 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2184,7 +2184,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) { const hrt_abstime timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; - if ((timestamp_sample != 0) && (timestamp_sample > _status_optical_flow_pub_last)) { + if ((timestamp_sample != 0) && (timestamp_sample > _optical_flow_vel_pub_last)) { vehicle_optical_flow_vel_s flow_vel{}; flow_vel.timestamp_sample = _ekf.aid_src_optical_flow().timestamp_sample; @@ -2205,6 +2205,8 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_optical_flow_vel_pub.publish(flow_vel); + + _optical_flow_vel_pub_last = timestamp_sample; } } #endif // CONFIG_EKF2_OPTICAL_FLOW diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 4e48b4a85a..88d1e59cbf 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -377,6 +377,7 @@ private: uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; + hrt_abstime _optical_flow_vel_pub_last{0}; perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW From 2ef807eaa05eded07294333332066727fd79d230 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 25 Oct 2023 15:28:40 +0200 Subject: [PATCH 13/14] sdcardCheck: shorten hardfault log message To make sure it's showing correctly in the output. --- .../commander/HealthAndArmingChecks/checks/sdcardCheck.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp index 1136c3f282..89ba247ed2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp @@ -122,7 +122,7 @@ void SdCardChecks::checkAndReport(const Context &context, Report &reporter) events::Log::Error, "Crash dumps present on SD card"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD"); } } From 12b291b82f4dcc2a8b1c59b86fddeee0b675abe2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 25 Oct 2023 15:42:26 +0200 Subject: [PATCH 14/14] px4_log: comment typo alway{s} --- platforms/common/px4_log.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 03845239ef..a3d9b02338 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -136,7 +136,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char #if defined(PX4_LOG_COLORIZED_OUTPUT) if (use_color) { - // alway reset color + // always reset color const ssize_t sz = math::min(pos, max_length - (ssize_t)strlen(PX4_ANSI_COLOR_RESET) - (ssize_t)1); pos += snprintf(buf + sz, math::max(max_length - sz, (ssize_t)0), "%s\n", PX4_ANSI_COLOR_RESET);