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
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Attach
BreakBeforeBraces: Linux
BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false

18
EKF/RingBuffer.h
View File

@ -42,9 +42,11 @@
#include <cstring>
template <typename data_type>
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 &timestamp, data_type *sample) {
bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
{
// start looking from newest observation data
for (uint8_t i = 0; i < _size; 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
// 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;

6
EKF/imu_down_sampler.hpp
View File

@ -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;
}

26
EKF/vel_pos_fusion.cpp
View File

@ -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;

18
mathlib/mathlib.h
View File

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