Moving Average Filter Implementation

AttitudeManager/Inc/IMUMovingAverageFilter.hpp

@@ -0,0 +1,25 @@
+#ifndef IMU_MOVING_AVERAGE_FILTER_HPP
+#define IMU_MOVING_AVERAGE_FILTER_HPP
+
+#include "MovingAverage.hpp"
+
+class IMUMovingAverageFilter {
+public:
+ IMUMovingAverageFilter();
+ MovingAverageError_t Update();
+ MovingAverageError_t GetAverage(uint8_t windowSize, IMUData_t* FilteredIMUData);
+
+private:
+ IMU* imuObj;
+ Window<float> magxVals;
+ Window<float> magyVals;
+ Window<float> magzVals;
+ Window<float> accxVals;
+ Window<float> accyVals;
+ Window<float> acczVals;
+ Window<float> gyrxVals;
+ Window<float> gyryVals;
+ Window<float> gyrzVals;
+};
+
+#endif // IMU_MOVING_AVERAGE_FILTER_HPP

AttitudeManager/Inc/MovingAverage.hpp

@@ -0,0 +1,27 @@
+#ifndef MOVING_AVERAGE_HPP
+#define MOVING_AVERAGE_HPP
+
+#include <stdint.h>
+
+#define MAX_WINDOW_SIZE 50
+#define MAX_POS_LIMIT MAX_WINDOW_SIZE * 1000000
+
+enum MovingAverageError_t {
+ SUCCESS,
+ FAILURE,
+ OLD_VALUES,
+};
+
+template<typename T>
+class Window {
+private:
+ T window[MAX_WINDOW_SIZE];
+ uint8_t size;
+ uint32_t pos;
+public:
+ Window();
+ void update(T newValue);
+ MovingAverageError_t getAverage(uint8_t n, T* result);
+};
+
+#endif // MOVING_AVERAGE_HPP

AttitudeManager/Inc/imu.hpp

@@ -0,0 +1,112 @@
+/**
+ * IMU Sensor Functions and Part Number Selection.
+ * Available IMU driver(s): BMX160
+ * Author(s): Lucy Gong, Dhruv Rawat, Anthony Berbari
+ */
+
+#ifndef IMU_HPP
+#define IMU_HPP
+
+#include <cstdint>
+#include "CommonDataTypes.hpp"
+/***********************************************************************************************************************
+ * Definitions
+ **********************************************************************************************************************/
+
+struct IMUData_t {
+
+ float magx, magy, magz; // TODO although the BMX 160 has a magnetometer, it seems to produce bizarre results.More investigation needs to be done. Figuring out what the RHall register does is likely a part of that.
+ float accx, accy, accz;
+ float gyrx, gyry, gyrz; 
+ float temp; 
+
+ bool isDataNew;
+ SensorErrorCodes sensorStatus; // 0 = SUCCESS, -1 = FAIL, 1 = BUSY
+};
+
+/***********************************************************************************************************************
+ * Prototypes
+ *********************************************************************************************************************/
+
+class IMU {
+ public:
+ /**
+ * Begins a transaction with the IMU.
+ * This function is non blocking and returns right away, data will be stored inside the module as it arrives.
+ * To achieve synchronous data, this function must be called synchronously.
+ * */
+ virtual void Begin_Measuring() = 0; 
+
+ /**
+ * Retrieves any data already received by the imu.
+ * If no new data is available, the appropriate flag will be set in the return struct.
+ * All contents of the return struct, apart from the isDataNew flag, are undefined unless isDataNew is set to 1.
+ * This function is non blocking and returns right away.
+ * @param[in] Data reference to the results struct.
+ * */
+ virtual void GetResult(IMUData_t& Data) = 0;
+};
+
+class BMX160: public IMU{
+ public:
+
+ /**
+ * This module is built as a singleton. Thus to access a BMX160 object, this function must be called.
+ * Only a single BMX160 object will ever be created and will be shared by all callers of this function.
+ * @return IMU reference to the singleton object.
+ * */
+ static IMU& getInstance();
+
+ /**
+ * Deletes the constructor to disallow users to instantiate objects.
+ * */
+ BMX160(const BMX160*) = delete;
+
+ void Begin_Measuring();
+ void GetResult(IMUData_t& Data);
+
+ private:
+
+ BMX160();
+
+ void SetAllPowerModesToNormal(void);
+ void ConfigAcc(void);
+ void ConfigGyro(void);
+ void ConfigMag(void);
+ void SetMagConfig(uint8_t regAddr, uint8_t data);
+ void PrepareMagForDataMode(void);
+
+ void Bmx160WriteReg(uint8_t reg, uint8_t val);
+ void Bmx160ReadReg(uint8_t const regAddr, uint8_t *pData, uint8_t len);
+
+ void Calibrate(void);
+
+ //Variables
+ uint8_t rawImuData[21];
+ IMUData_t ImuCalibration;
+ IMUData_t ImuCalibrationFinal;
+
+};
+
+/**
+* Callback for exclusive use by the SPI ISR.
+* Users of this module should never call this function.
+* Any manipulation of this function by the higher level voids all guarantees made by this module.
+* */
+void ImuTxRxCallback();
+
+#ifdef UNIT_TESTING
+
+class TestIMU : public IMU {
+ public:
+ static TestIMU* GetInstance();
+
+ void Begin_Measuring();
+
+ void GetResult(IMUData_t& Data);
+
+};
+
+#endif
+
+#endif

AttitudeManager/Src/IMUMovingAverageFilter.cpp

@@ -0,0 +1,57 @@
+#include "IMUMovingAverageFilter.hpp"
+#include "imu.hpp"
+#include "MovingAverage.hpp"
+
+IMUMovingAverageFilter::IMUMovingAverageFilter() {
+ imuObj = &BMX160::getInstance();
+ magxVals = Window<float>();
+ magyVals = Window<float>();
+ magzVals = Window<float>();
+ accxVals = Window<float>();
+ accyVals = Window<float>();
+ acczVals = Window<float>();
+ gyrxVals = Window<float>();
+ gyryVals = Window<float>();
+ gyrzVals = Window<float>();
+}
+
+MovingAverageError_t IMUMovingAverageFilter::Update() {
+ IMUData_t currentIMUData;
+ imuObj->GetResult(currentIMUData);
+
+ if (!currentIMUData.isDataNew) {
+ // We could log here, ideally this case shouldn't occur and we should slow 
+ // down the refresh rate if we encounter this often.
+ return OLD_VALUES;
+ }
+
+ magxVals.update(currentIMUData.magx);
+ magyVals.update(currentIMUData.magy);
+ magzVals.update(currentIMUData.magz);
+ accxVals.update(currentIMUData.accx);
+ accyVals.update(currentIMUData.accy);
+ acczVals.update(currentIMUData.accz);
+ gyrxVals.update(currentIMUData.gyrx);
+ gyryVals.update(currentIMUData.gyry);
+ gyrzVals.update(currentIMUData.gyrz);
+
+ return SUCCESS;
+}
+
+// Populates FilteredIMUData.
+MovingAverageError_t IMUMovingAverageFilter::GetAverage(uint8_t windowSize, IMUData_t* FilteredIMUData) {
+
+ int checksum;
+
+ checksum += magxVals.getAverage(windowSize, &FilteredIMUData->magx);
+ checksum += magyVals.getAverage(windowSize, &FilteredIMUData->magy);
+ checksum += magzVals.getAverage(windowSize, &FilteredIMUData->magz);
+ checksum += accyVals.getAverage(windowSize, &FilteredIMUData->accx);
+ checksum += acczVals.getAverage(windowSize, &FilteredIMUData->accy);
+ checksum += accxVals.getAverage(windowSize, &FilteredIMUData->accz);
+ checksum += gyrxVals.getAverage(windowSize, &FilteredIMUData->gyrx);
+ checksum += gyryVals.getAverage(windowSize, &FilteredIMUData->gyry);
+ checksum += gyrzVals.getAverage(windowSize, &FilteredIMUData->gyrz);
+
+ return checksum == 0 ? FAILURE : SUCCESS;
+}

AttitudeManager/Src/MovingAverage.cpp

@@ -0,0 +1,41 @@
+#include "MovingAverage.hpp"
+#include <string.h>
+
+template<typename T>
+Window<T>::Window() {
+ memset(window, 0, MAX_WINDOW_SIZE);
+ size = 0;
+ pos = 0;
+}
+
+template<typename T>
+void Window<T>::update(T newValue) {
+ window[pos] = newValue;
+ pos++;
+
+ // If the window is not full yet, increase size.
+ if (size < MAX_WINDOW_SIZE) {
+ size++;
+ }
+
+ // Reset pos once in a while to ensure it doesn't get too big.
+ if (pos == MAX_POS_LIMIT) {
+ pos = 0;
+ }
+}
+
+template<typename T>
+MovingAverageError_t Window<T>::getAverage(uint8_t n, T* result) {
+ if (n <= 0 || n > size) {
+ return FAILURE;
+ }
+
+ int sum = 0;
+ for (int i = pos; i > pos - n; i--) {
+ uint8_t realIdx = pos % MAX_WINDOW_SIZE;
+ sum += window[realIdx];
+ }
+
+ *result = static_cast<T>(sum) / n;
+ return SUCCESS;
+}