dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

clang-format set BreakBeforeBraces to Linux style 

- this keeps things consistent with PX4/Firmware astyle
This commit is contained in:
Daniel Agar 2020-06-25 12:50:14 -04:00
parent b96c62ed8b
commit 2927132954
6 changed files with 53 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.clang-format
View File

@ -37,7 +37,7 @@ BraceWrapping:
SplitEmptyRecord: true SplitEmptyRecord: true
SplitEmptyNamespace: true SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None BreakBeforeBinaryOperators: None
BreakBeforeBraces: Attach BreakBeforeBraces: Linux
BreakBeforeInheritanceComma: false BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: true BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false BreakConstructorInitializersBeforeComma: false

18
EKF/RingBuffer.h
View File

@ -42,9 +42,11 @@
#include <cstring> #include <cstring>
template <typename data_type> template <typename data_type>
class RingBuffer { class RingBuffer
{
public: public:
RingBuffer() { RingBuffer()
{
if (allocate(1)) { if (allocate(1)) {
// initialize with one empty sample // initialize with one empty sample
data_type d = {}; data_type d = {};
@ -59,7 +61,8 @@ public:
RingBuffer(RingBuffer &&) = delete; RingBuffer(RingBuffer &&) = delete;
RingBuffer &operator=(RingBuffer &&) = delete; RingBuffer &operator=(RingBuffer &&) = delete;
bool allocate(uint8_t size) { bool allocate(uint8_t size)
{
if (_buffer != nullptr) { if (_buffer != nullptr) {
delete[] _buffer; delete[] _buffer;
@ -87,12 +90,14 @@ public:
return true; return true;
} }
void unallocate() { void unallocate()
{
delete[] _buffer; delete[] _buffer;
_buffer = nullptr; _buffer = nullptr;
} }
void push(const data_type &sample) { void push(const data_type &sample)
{
uint8_t head_new = _head; uint8_t head_new = _head;
@ -121,7 +126,8 @@ public:
uint8_t get_oldest_index() const { return _tail; } uint8_t get_oldest_index() const { return _tail; }
bool pop_first_older_than(const uint64_t &timestamp, data_type *sample) { bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
{
// start looking from newest observation data // start looking from newest observation data
for (uint8_t i = 0; i < _size; i++) { for (uint8_t i = 0; i < _size; i++) {
int index = (_head - i); int index = (_head - i);

6
EKF/imu_down_sampler.cpp
View File

@ -5,7 +5,8 @@ ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec}
// integrate imu samples until target dt reached // integrate imu samples until target dt reached
// assumes that dt of the gyroscope is close to the dt of the accelerometer // assumes that dt of the gyroscope is close to the dt of the accelerometer
// returns true if target dt is reached // 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) { if (_do_reset) {
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_ang.setZero();
_imu_down_sampled.delta_vel.setZero(); _imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f; _imu_down_sampled.delta_ang_dt = 0.0f;

6
EKF/imu_down_sampler.hpp
View File

@ -44,14 +44,16 @@
using namespace estimator; using namespace estimator;
class ImuDownSampler { class ImuDownSampler
{
public: public:
explicit ImuDownSampler(float target_dt_sec); explicit ImuDownSampler(float target_dt_sec);
~ImuDownSampler() = default; ~ImuDownSampler() = default;
bool update(const imuSample &imu_sample_new); bool update(const imuSample &imu_sample_new);
imuSample getDownSampledImuAndTriggerReset() { imuSample getDownSampledImuAndTriggerReset()
{
_do_reset = true; _do_reset = true;
return _imu_down_sampled; return _imu_down_sampled;
} }

26
EKF/vel_pos_fusion.cpp
View File

@ -46,7 +46,8 @@
#include "ekf.h" #include "ekf.h"
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var, 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(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1); 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))); sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f); const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass) { if (innov_check_pass) {
_time_last_hor_vel_fuse = _time_last_imu; _time_last_hor_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hor_vel = false; _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); fuseVelPosHeight(innov(1), innov_var(1), 1);
return true; return true;
} else { } else {
_innov_check_fail_status.flags.reject_hor_vel = true; _innov_check_fail_status.flags.reject_hor_vel = true;
return false; 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, 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); innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_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); const bool innov_check_pass = (test_ratio(1) <= 1.0f);
if (innov_check_pass) { if (innov_check_pass) {
_time_last_ver_vel_fuse = _time_last_imu; _time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false; _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); fuseVelPosHeight(innov(2), innov_var(2), 2);
return true; return true;
} else { } else {
_innov_check_fail_status.flags.reject_ver_vel = true; _innov_check_fail_status.flags.reject_ver_vel = true;
return false; 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, 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(0) = P(7, 7) + obs_var(0);
innov_var(1) = P(8, 8) + obs_var(1); 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))); sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = test_ratio(0) <= 1.0f; const bool innov_check_pass = test_ratio(0) <= 1.0f;
if (innov_check_pass) { if (innov_check_pass) {
if (!_fuse_hpos_as_odom) { if (!_fuse_hpos_as_odom) {
_time_last_hor_pos_fuse = _time_last_imu; _time_last_hor_pos_fuse = _time_last_imu;
@ -104,12 +112,14 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
} else { } else {
_time_last_delpos_fuse = _time_last_imu; _time_last_delpos_fuse = _time_last_imu;
} }
_innov_check_fail_status.flags.reject_hor_pos = false; _innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0), innov_var(0), 3); fuseVelPosHeight(innov(0), innov_var(0), 3);
fuseVelPosHeight(innov(1), innov_var(1), 4); fuseVelPosHeight(innov(1), innov_var(1), 4);
return true; return true;
} else { } else {
_innov_check_fail_status.flags.reject_hor_pos = true; _innov_check_fail_status.flags.reject_hor_pos = true;
return false; 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, 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); innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_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; const bool innov_check_pass = test_ratio(1) <= 1.0f;
if (innov_check_pass) { if (innov_check_pass) {
_time_last_hgt_fuse = _time_last_imu; _time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false; _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 // 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. 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 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) { if (index == 0) {
_fault_status.flags.bad_vel_N = status; _fault_status.flags.bad_vel_N = status;

18
mathlib/mathlib.h
View File

@ -55,29 +55,35 @@
#define M_PI 3.141592653589793238462643383280 #define M_PI 3.141592653589793238462643383280
#endif #endif
namespace math { namespace math
{
template <typename Type> template <typename Type>
static constexpr Type min(Type val1, Type val2) { static constexpr Type min(Type val1, Type val2)
{
return (val1 < val2) ? val1 : val2; return (val1 < val2) ? val1 : val2;
} }
template <typename Type> template <typename Type>
static constexpr Type max(Type val1, Type val2) { static constexpr Type max(Type val1, Type val2)
{
return (val1 > val2) ? val1 : val2; return (val1 > val2) ? val1 : val2;
} }
template <typename Type> template <typename Type>
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); return (val < min) ? min : ((val > max) ? max : val);
} }
template <typename Type> template <typename Type>
static constexpr Type radians(Type degrees) { static constexpr Type radians(Type degrees)
{
return (degrees / Type(180)) * Type(M_PI); return (degrees / Type(180)) * Type(M_PI);
} }
template <typename Type> template <typename Type>
static constexpr Type degrees(Type radians) { static constexpr Type degrees(Type radians)
{
return (radians * Type(180)) / Type(M_PI); return (radians * Type(180)) / Type(M_PI);
} }