Skip to content

Commit

Permalink
Updated Circular Buffer class and implemented changes for filters.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
@zacharydiringer
zacharydiringer committed Apr 4, 2024
1 parent 7a9d17e commit 78b36a5
Show file tree
Hide file tree
Showing 3 changed files with 153 additions and 166 deletions.
36 changes: 13 additions & 23 deletions common/system/filtering/filters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,18 +3,18 @@
*/

#include "filters.h"
#include <iostream>

#include <cmath>
#include <cstring>

#include "altitude_kalman.h"
#include "board_config_common.h"
#include "cli.h"
#include "cpp_circular_buffer.h"
#include "data_log.h"
#include "hardware_manager.h"
#include "math_utils.h"
#include "orientation_estimator.h"
#include "cpp_circular_buffer.h"
#define RAD_TO_DEG 57.29578 // 180 / PI
#define MATH_PI 3.141592653589 // PI
#define R_DRY_AIR 287.0474909 // J/K/kg
Expand All @@ -29,23 +29,23 @@
#define kGyroRefCount 10

static double runningPresMedians[kPrevPresMedianCount + 1];
static CircularBuffer <float,kPrevPresMedianCount> runningPresMediansBuffer;
static CircularBuffer<float, kPrevPresMedianCount> runningPresMediansBuffer;
static uint8_t runningPresMedianCount;
static double runningPres[kPrevPresCount + 1];
CircularBuffer <float,kPrevPresMedianCount> runningPresBuffer;
CircularBuffer<float, kPrevPresMedianCount> runningPresBuffer;
static uint8_t runningPresCount;

static double gravityRefBuffer[kGravityRefCount];
static double runningGravityRef[kGravityRefCount + 1];
static CircularBuffer <int32_t,kGravityRefCount + 1> runningGravityRefBuffer;
static CircularBuffer<int32_t, kGravityRefCount + 1> runningGravityRefBuffer;
static float gyroXRefBack[kGyroRefCount + 1];
static CircularBuffer <float,kGyroRefCount> gyroXRefBuffer;
static CircularBuffer<float, kGyroRefCount> gyroXRefBuffer;
static float gyroXOffset;
static float gyroYRefBack[kGyroRefCount + 1];
static CircularBuffer<float,kGyroRefCount> gyroYRefBuffer;
static CircularBuffer<float, kGyroRefCount> gyroYRefBuffer;
static float gyroYOffset;
static float gyroZRefBack[kGyroRefCount + 1];
static CircularBuffer<float,kGravityRefCount> gyroZRefBuffer;
static CircularBuffer<float, kGravityRefCount> gyroZRefBuffer;
static float gyroZOffset;

static FilterData_s filterData;
Expand Down Expand Up @@ -327,21 +327,20 @@ static void filterGyros(SensorData_s* curSensorVals) {
filterData.yaw = atan2(siny_cosp, cosy_cosp) * RAD_TO_DEG;
}

void updateGyroOffsetOneAxis(CircularBuffer<float,kPrevPresCount>* refBuffer, const float& newValue,
float* offset) {
void updateGyroOffsetOneAxis(CircularBuffer<float, kPrevPresCount>* refBuffer,
const float& newValue, float* offset) {
uint8_t i;
float gyroSum = 0;
static float referenceBuffer[kGyroRefCount];
if (refBuffer->full()) {
//cb_dequeue(refBuffer, 1);
// cb_dequeue(refBuffer, 1);
refBuffer->dequeue(1);
}

refBuffer->enqueue(newValue);

gyroSum = refBuffer->get_sum();
*offset = gyroSum / (float)refBuffer->count();
std::cout << "Gyro Offeset: " <<*offset << std::endl;
}

void filter_addGyroRef() {
Expand Down Expand Up @@ -438,8 +437,6 @@ void filter_addGravityRef() {
size_t gravCount = runningGravityRefBuffer.count();
double accelSum = 0;
accelSum = runningGravityRefBuffer.get_sum();
std::cout << "accelSum " << (int) accelSum << std::endl;


// Check that we are mostly in the direction of gravity in some direction.
// If the sum of our accelerations averages out to less than half G per
Expand All @@ -466,7 +463,6 @@ void filter_addGravityRef() {
void filter_addPressureRef(SensorData_s* curSensorVals) {
// Average current pressures
double currentPres = filter_getAveragePressure(curSensorVals);
std::cout << "runningPresMedianCount " << (int) runningPresMedianCount << std::endl;
// For the first 10 seconds (before we have any current medians
// just set the current pressure ref so we don't depend on
// a single initial value
Expand All @@ -481,14 +477,12 @@ void filter_addPressureRef(SensorData_s* curSensorVals) {
// Add current pressure
runningPresBuffer.enqueue(currentPres);
++runningPresCount;
std::cout << "runningPresCount " << (int) runningPresCount << " kPrevPresCount " << (int) kPrevPresCount << std::endl;
// Add median to the running medians every n values
if (runningPresCount == kPrevPresCount) {
runningPresCount = 0;
if (runningPresMedianCount < kPrevPresMedianCount) ++runningPresMedianCount;
// Make room for new value, discarding oldest median stored if full
if (
runningPresMediansBuffer.full()) {
if (runningPresMediansBuffer.full()) {
runningPresMediansBuffer.dequeue(1);
}

Expand All @@ -497,11 +491,7 @@ void filter_addPressureRef(SensorData_s* curSensorVals) {
double currentMedian = runningPresBuffer.get_med();
runningPresMediansBuffer.enqueue(currentMedian);




presRef = runningPresMediansBuffer.get_med();
std::cout << "Pressure reference: " <<presRef << std::endl;
/*
// Only set pressure ref if we have enough values recorded
if (runningPresMedianCount == kPrevPresMedianCount) {
Expand All @@ -512,7 +502,7 @@ void filter_addPressureRef(SensorData_s* curSensorVals) {
//&numMedElements);
//runningPresMediansBuffer.peek(medianArray,numMedElements);
//presRef = median(medianArray, kPrevPresMedianCount);
} else {
// Otherwise (for the first 100 seconds) set current pressure ref to the
// median of the last 10 seconds so that we have at least a somewhat
Expand Down
128 changes: 64 additions & 64 deletions common/utils/cpp_circular_buffer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,88 +150,88 @@ class CircularBuffer {
}
}

float get_sum(){
float get_sum() {
Type* tailTracker = this->head;
float sum = 0;
while (tailTracker != this->tail){
sum += *tailTracker;
this->incrementPointer(&tailTracker,1);
}
float sum = 0;
while (tailTracker != this->tail) {
sum += *tailTracker;
this->incrementPointer(&tailTracker, 1);
}
return sum;
}


float get_avg(){
Type* tailTracker = this->head;
float sum = 0;
while (tailTracker != this->tail){
sum += *tailTracker;
this->incrementPointer(&tailTracker,1);
}
float avg = sum / count();
return avg;
float get_avg() {
Type* tailTracker = this->head;
float sum = 0;
while (tailTracker != this->tail) {
sum += *tailTracker;
this->incrementPointer(&tailTracker, 1);
}
float avg = sum / count();
return avg;
}

float get_min(){
Type* tailTracker = this->head;
float min = *tailTracker;
while (tailTracker != this->tail){
float new_val = *tailTracker;
if (new_val < min){
min = new_val;
}
this->incrementPointer(&tailTracker,1);
}
return min;
float get_min() {
Type* tailTracker = this->head;
float min = *tailTracker;
while (tailTracker != this->tail) {
float new_val = *tailTracker;
if (new_val < min) {
min = new_val;
}
this->incrementPointer(&tailTracker, 1);
}
return min;
}
float get_max(){
Type* tailTracker = this->head;
float max = *tailTracker;
while (tailTracker != this->tail){
float new_val = *tailTracker;
if (new_val > max){
max = new_val;
}
this->incrementPointer(&tailTracker,1);
}
return max;
float get_max() {
Type* tailTracker = this->head;
float max = *tailTracker;
while (tailTracker != this->tail) {
float new_val = *tailTracker;
if (new_val > max) {
max = new_val;
}
this->incrementPointer(&tailTracker, 1);
}
return max;
}

float get_med(){
Type* tailTracker = this->head;
int length = count();
float arr[length];
int i = 0;
float med = 0;
while (tailTracker != this->tail){
float cur_val = *tailTracker;
arr[i] = cur_val;
arr[i] = *tailTracker;
this->incrementPointer(&tailTracker,1);
i += 1;
float get_med() {
Type* tailTracker = this->head;
int length = count();
float arr[Capacity + 1];
int i = 0;
float med = 0;
while (tailTracker != this->tail) {
float cur_val = *tailTracker;
arr[i] = cur_val;
arr[i] = *tailTracker;
this->incrementPointer(&tailTracker, 1);
i += 1;
}
for (int i = 0; i < length; ++i) {
for (int j = 0; j < length - 1 - i; ++j) {
if (arr[j] > arr[j + 1]) {
// Swap arr[j] and arr[j + 1]
float temp = arr[j];
arr[j] = arr[j + 1];
arr[j + 1] = temp;
}
for (int i = 0; i < length; ++i) {
for (int j = 0; j < length - 1 - i; ++j) {
if (arr[j] > arr[j + 1]) {
// Swap arr[j] and arr[j + 1]
float temp = arr[j];
arr[j] = arr[j + 1];
arr[j + 1] = temp;
}
}
}
if (length % 2 == 1) med = arr[length / 2];
else med = (arr[length / 2 - 1] + arr[length / 2]) / 2;
return med;
}
if (length % 2 == 1)
med = arr[length / 2];
else
med = (arr[length / 2 - 1] + arr[length / 2]) / 2;
return med;
}
/**
* Check if the buffer is currently full or not.
*
* @return bool: True if buffer is full, false otherwise.
*/


bool full() const { return this->count() == Capacity; }
};

#endif /* COMMON_UTILS_CPP_CIRCULAR_BUFFER_H_ */
#endif // COMMON_UTILS_CPP_CIRCULAR_BUFFER_H_
Loading

0 comments on commit 78b36a5

Please sign in to comment.