From 2927132954afd3533a580a7ede327e502bfc4efc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 25 Jun 2020 12:50:14 -0400 Subject: [PATCH] clang-format set BreakBeforeBraces to Linux style - this keeps things consistent with PX4/Firmware astyle --- .clang-format | 2 +- EKF/RingBuffer.h | 18 ++++++++++++------ EKF/imu_down_sampler.cpp | 6 ++++-- EKF/imu_down_sampler.hpp | 6 ++++-- EKF/vel_pos_fusion.cpp | 26 ++++++++++++++++++++------ mathlib/mathlib.h | 18 ++++++++++++------ 6 files changed, 53 insertions(+), 23 deletions(-) diff --git a/.clang-format b/.clang-format index 6edbfee64f..c8ed09f5cc 100644 --- a/.clang-format +++ b/.clang-format @@ -37,7 +37,7 @@ BraceWrapping: SplitEmptyRecord: true SplitEmptyNamespace: true BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach +BreakBeforeBraces: Linux BreakBeforeInheritanceComma: false BreakBeforeTernaryOperators: true BreakConstructorInitializersBeforeComma: false diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index f9f60d1598..2061f080c6 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -42,9 +42,11 @@ #include template -class RingBuffer { +class RingBuffer +{ public: - RingBuffer() { + RingBuffer() + { if (allocate(1)) { // initialize with one empty sample data_type d = {}; @@ -59,7 +61,8 @@ public: RingBuffer(RingBuffer &&) = delete; RingBuffer &operator=(RingBuffer &&) = delete; - bool allocate(uint8_t size) { + bool allocate(uint8_t size) + { if (_buffer != nullptr) { delete[] _buffer; @@ -87,12 +90,14 @@ public: return true; } - void unallocate() { + void unallocate() + { delete[] _buffer; _buffer = nullptr; } - void push(const data_type &sample) { + void push(const data_type &sample) + { uint8_t head_new = _head; @@ -121,7 +126,8 @@ public: uint8_t get_oldest_index() const { return _tail; } - bool pop_first_older_than(const uint64_t ×tamp, data_type *sample) { + bool pop_first_older_than(const uint64_t ×tamp, data_type *sample) + { // start looking from newest observation data for (uint8_t i = 0; i < _size; i++) { int index = (_head - i); diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp index 92cb11980d..7d9c4965ac 100644 --- a/EKF/imu_down_sampler.cpp +++ b/EKF/imu_down_sampler.cpp @@ -5,7 +5,8 @@ ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec} // integrate imu samples until target dt reached // assumes that dt of the gyroscope is close to the dt of the accelerometer // returns true if target dt is reached -bool ImuDownSampler::update(const imuSample &imu_sample_new) { +bool ImuDownSampler::update(const imuSample &imu_sample_new) +{ if (_do_reset) { reset(); } @@ -50,7 +51,8 @@ bool ImuDownSampler::update(const imuSample &imu_sample_new) { } } -void ImuDownSampler::reset() { +void ImuDownSampler::reset() +{ _imu_down_sampled.delta_ang.setZero(); _imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_ang_dt = 0.0f; diff --git a/EKF/imu_down_sampler.hpp b/EKF/imu_down_sampler.hpp index d61e7fe9b3..17cdaf27d4 100644 --- a/EKF/imu_down_sampler.hpp +++ b/EKF/imu_down_sampler.hpp @@ -44,14 +44,16 @@ using namespace estimator; -class ImuDownSampler { +class ImuDownSampler +{ public: explicit ImuDownSampler(float target_dt_sec); ~ImuDownSampler() = default; bool update(const imuSample &imu_sample_new); - imuSample getDownSampledImuAndTriggerReset() { + imuSample getDownSampledImuAndTriggerReset() + { _do_reset = true; return _imu_down_sampled; } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 4aa4b05a04..7b0ea7b75b 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -46,7 +46,8 @@ #include "ekf.h" bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) { + Vector3f &innov_var, Vector2f &test_ratio) +{ innov_var(0) = P(4, 4) + obs_var(0); innov_var(1) = P(5, 5) + obs_var(1); @@ -54,6 +55,7 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); const bool innov_check_pass = (test_ratio(0) <= 1.0f); + if (innov_check_pass) { _time_last_hor_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_hor_vel = false; @@ -62,6 +64,7 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga fuseVelPosHeight(innov(1), innov_var(1), 1); return true; + } else { _innov_check_fail_status.flags.reject_hor_vel = true; return false; @@ -69,12 +72,14 @@ bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_ga } bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) { + Vector3f &innov_var, Vector2f &test_ratio) +{ innov_var(2) = P(6, 6) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); const bool innov_check_pass = (test_ratio(1) <= 1.0f); + if (innov_check_pass) { _time_last_ver_vel_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_vel = false; @@ -82,6 +87,7 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate fuseVelPosHeight(innov(2), innov_var(2), 2); return true; + } else { _innov_check_fail_status.flags.reject_ver_vel = true; return false; @@ -89,7 +95,8 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate } bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) { + Vector3f &innov_var, Vector2f &test_ratio) +{ innov_var(0) = P(7, 7) + obs_var(0); innov_var(1) = P(8, 8) + obs_var(1); @@ -97,6 +104,7 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1))); const bool innov_check_pass = test_ratio(0) <= 1.0f; + if (innov_check_pass) { if (!_fuse_hpos_as_odom) { _time_last_hor_pos_fuse = _time_last_imu; @@ -104,12 +112,14 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga } else { _time_last_delpos_fuse = _time_last_imu; } + _innov_check_fail_status.flags.reject_hor_pos = false; fuseVelPosHeight(innov(0), innov_var(0), 3); fuseVelPosHeight(innov(1), innov_var(1), 4); return true; + } else { _innov_check_fail_status.flags.reject_hor_pos = true; return false; @@ -117,12 +127,14 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga } bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, - Vector3f &innov_var, Vector2f &test_ratio) { + Vector3f &innov_var, Vector2f &test_ratio) +{ innov_var(2) = P(9, 9) + obs_var(2); test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2)); const bool innov_check_pass = test_ratio(1) <= 1.0f; + if (innov_check_pass) { _time_last_hgt_fuse = _time_last_imu; _innov_check_fail_status.flags.reject_ver_pos = false; @@ -138,7 +150,8 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate } // Helper function that fuses a single velocity or position measurement -void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) { +void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +{ float Kfusion[24]; // Kalman gain vector for any single observation - sequential fusion is used. const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state @@ -191,7 +204,8 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } } -void Ekf::setVelPosFaultStatus(const int index, const bool status) { +void Ekf::setVelPosFaultStatus(const int index, const bool status) +{ if (index == 0) { _fault_status.flags.bad_vel_N = status; diff --git a/mathlib/mathlib.h b/mathlib/mathlib.h index a01001138e..8b24c94aea 100644 --- a/mathlib/mathlib.h +++ b/mathlib/mathlib.h @@ -55,29 +55,35 @@ #define M_PI 3.141592653589793238462643383280 #endif -namespace math { +namespace math +{ template -static constexpr Type min(Type val1, Type val2) { +static constexpr Type min(Type val1, Type val2) +{ return (val1 < val2) ? val1 : val2; } template -static constexpr Type max(Type val1, Type val2) { +static constexpr Type max(Type val1, Type val2) +{ return (val1 > val2) ? val1 : val2; } template -static constexpr Type constrain(Type val, Type min, Type max) { +static constexpr Type constrain(Type val, Type min, Type max) +{ return (val < min) ? min : ((val > max) ? max : val); } template -static constexpr Type radians(Type degrees) { +static constexpr Type radians(Type degrees) +{ return (degrees / Type(180)) * Type(M_PI); } template -static constexpr Type degrees(Type radians) { +static constexpr Type degrees(Type radians) +{ return (radians * Type(180)) / Type(M_PI); }