Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Commit

Permalink
PX4 style fixes (excluding EKF generated code)
Browse files Browse the repository at this point in the history
  • Loading branch information
dagar committed May 4, 2018
1 parent e5952fa commit c5b2f59
Show file tree
Hide file tree
Showing 22 changed files with 384 additions and 187 deletions.
15 changes: 9 additions & 6 deletions EKF/RingBuffer.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ template <typename data_type>
class RingBuffer
{
public:
RingBuffer() {
RingBuffer()
{
if (allocate(1)) {
// initialize with one empty sample
data_type d = {};
Expand Down Expand Up @@ -81,17 +82,19 @@ class RingBuffer
for (uint8_t index = 0; index < _size; index++) {
_buffer[index] = {};
}

_first_write = true;

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;

Expand All @@ -115,12 +118,12 @@ class RingBuffer

data_type &operator[](const uint8_t index) { return _buffer[index]; }

const data_type& get_newest() { return _buffer[_head]; }
const data_type& get_oldest() { return _buffer[_tail]; }
const data_type &get_newest() { return _buffer[_head]; }
const data_type &get_oldest() { return _buffer[_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
for (uint8_t i = 0; i < _size; i++) {
Expand Down
8 changes: 6 additions & 2 deletions EKF/airspeed_fusion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,7 @@ void Ekf::fuseAirspeed()
for (unsigned row = 0; row <= 21; row++) {
Kfusion[row] = 0.0f;
}

} else {
// we have no other source of aiding, so use airspeed measurements to correct states
Kfusion[0] = SK_TAS[0]*(P[0][4]*SH_TAS[2] - P[0][22]*SH_TAS[2] + P[0][5]*SK_TAS[1] - P[0][23]*SK_TAS[1] + P[0][6]*vd*SH_TAS[0]);
Expand Down Expand Up @@ -164,6 +165,7 @@ void Ekf::fuseAirspeed()
if (_tas_test_ratio > 1.0f) {
_innov_check_fail_status.flags.reject_airspeed = true;
return;

} else {
_innov_check_fail_status.flags.reject_airspeed = false;
}
Expand All @@ -176,6 +178,7 @@ void Ekf::fuseAirspeed()
// then calculate P - KHP
float KHP[_k_num_states][_k_num_states];
float KH[5];

for (unsigned row = 0; row < _k_num_states; row++) {

KH[0] = Kfusion[row] * H_TAS[4];
Expand All @@ -198,11 +201,12 @@ void Ekf::fuseAirspeed()
// the covariance marix is unhealthy and must be corrected
bool healthy = true;
_fault_status.flags.bad_airspeed = false;

for (int i = 0; i < _k_num_states; i++) {
if (P[i][i] < KHP[i][i]) {
// zero rows and columns
zeroRows(P,i,i);
zeroCols(P,i,i);
zeroRows(P, i, i);
zeroCols(P, i, i);

//flag as unhealthy
healthy = false;
Expand Down
Loading

0 comments on commit c5b2f59

Please sign in to comment.