From 818cb314e669041d19b83a445da2c1d557202ef4 Mon Sep 17 00:00:00 2001 From: twilkhoo Date: Tue, 5 Mar 2024 15:11:30 -0500 Subject: [PATCH 1/2] Commit moving average filter content, with old IMU interface. --- AttitudeManager/Inc/FilteringGeneral.hpp | 21 ++ AttitudeManager/Inc/FilteringInterface.hpp | 12 + AttitudeManager/Inc/MovingAverageFilter.hpp | 27 ++ AttitudeManager/Inc/imu.hpp | 112 +++++++ AttitudeManager/Src/FilteringGeneral.cpp | 29 ++ AttitudeManager/Src/FilteringInterface.cpp | 25 ++ AttitudeManager/Src/MovingAverageFilter.cpp | 52 ++++ AttitudeManager/Src/imu.cpp | 328 ++++++++++++++++++++ 8 files changed, 606 insertions(+) create mode 100644 AttitudeManager/Inc/FilteringGeneral.hpp create mode 100644 AttitudeManager/Inc/FilteringInterface.hpp create mode 100644 AttitudeManager/Inc/MovingAverageFilter.hpp create mode 100644 AttitudeManager/Inc/imu.hpp create mode 100644 AttitudeManager/Src/FilteringGeneral.cpp create mode 100644 AttitudeManager/Src/FilteringInterface.cpp create mode 100644 AttitudeManager/Src/MovingAverageFilter.cpp create mode 100644 AttitudeManager/Src/imu.cpp diff --git a/AttitudeManager/Inc/FilteringGeneral.hpp b/AttitudeManager/Inc/FilteringGeneral.hpp new file mode 100644 index 00000000..2a556d20 --- /dev/null +++ b/AttitudeManager/Inc/FilteringGeneral.hpp @@ -0,0 +1,21 @@ +#ifndef FILTERINGGENERAL_HPP +#define FILTERINGGENERAL_HPP + +#include "imu.hpp" + +// -1 = FAILED +// 0 = SUCCESS +// 1 = Old Data +struct FilteringError_t { + int errorCode; +}; + +class FilteringGeneral { +public: + static IMU *imuObj; + static FilteringError_t Init(); + static IMUData_t GetRawIMU(); +}; + + +#endif diff --git a/AttitudeManager/Inc/FilteringInterface.hpp b/AttitudeManager/Inc/FilteringInterface.hpp new file mode 100644 index 00000000..e1a9f461 --- /dev/null +++ b/AttitudeManager/Inc/FilteringInterface.hpp @@ -0,0 +1,12 @@ +// FilteringInterface is the only source that will be used directly by the client. +// We define the exact filters we want to use in FilteringInterface.cpp. + +#ifndef FILTERING_INTERFACE_HPP +#define FILTERING_INTERFACE_HPP + +#include "FilteringGeneral.hpp" + +FilteringError_t FilteringInterfaceInit(void); +FilteringError_t FilteringInterfaceExecute(void); + +#endif \ No newline at end of file diff --git a/AttitudeManager/Inc/MovingAverageFilter.hpp b/AttitudeManager/Inc/MovingAverageFilter.hpp new file mode 100644 index 00000000..2a2d3997 --- /dev/null +++ b/AttitudeManager/Inc/MovingAverageFilter.hpp @@ -0,0 +1,27 @@ +#ifndef MOVING_AVERAGE_FILTER_HPP +#define MOVING_AVERAGE_FILTER_HPP + +#define WINDOW_SIZE 50 + +#include "FilteringGeneral.hpp" +#include "circular_buffer.hpp" + +struct MovingAverageOutput_t { + float magx; +}; + +struct MovingAverageWindow { + float magxVals[WINDOW_SIZE]; +}; + +class MovingAverageFiltering { +public: + static FilteringError_t Init(); + static FilteringError_t Update(MovingAverageOutput_t* output); + +private: + static MovingAverageWindow Window; + static int WindowCount; +}; + +#endif diff --git a/AttitudeManager/Inc/imu.hpp b/AttitudeManager/Inc/imu.hpp new file mode 100644 index 00000000..198bffd8 --- /dev/null +++ b/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 +#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 diff --git a/AttitudeManager/Src/FilteringGeneral.cpp b/AttitudeManager/Src/FilteringGeneral.cpp new file mode 100644 index 00000000..6caaedf4 --- /dev/null +++ b/AttitudeManager/Src/FilteringGeneral.cpp @@ -0,0 +1,29 @@ +#include "FilteringGeneral.hpp" +#include "imu.hpp" +#include + +namespace FilteringGeneral { + +IMU* FilteringGeneral::imuObj = nullptr; + +FilteringError_t FilteringGeneral::Init() { + FilteringError_t ret; + ret.errorCode = 0; + + imuObj = &BMX160::getInstance(); + + return ret; +} + +IMUData_t FilteringGeneral::GetRawIMU() { + IMUData_t imuData; + imuObj->GetResult(imuData); + + IMUData_t imuOutput; + + std:memcpy(&imuOutput, &imuData, sizeof(IMUData_t)); + + return imuOutput; +} + +} diff --git a/AttitudeManager/Src/FilteringInterface.cpp b/AttitudeManager/Src/FilteringInterface.cpp new file mode 100644 index 00000000..b08453ad --- /dev/null +++ b/AttitudeManager/Src/FilteringInterface.cpp @@ -0,0 +1,25 @@ +#include "FilteringInterface.hpp" +#include "FilteringGeneral.hpp" +#include "MovingAverageFilter.hpp" + +FilteringError_t FilteringInterfaceInit(void) { + FilteringError_t ret; + + ret = FilteringGeneral::Init(); + if (ret.errorCode != 0) return ret; + + MovingAverageFiltering::Init(); + if (ret.errorCode != 0) return ret; + + return ret; +} + +FilteringError_t FilteringInterfaceExecute(void) { + FilteringError_t ret; + + MovingAverageOutput_t output; + ret = MovingAverageFiltering::Update(&output); + if (ret.errorCode != 0) return ret; + + return ret; +} diff --git a/AttitudeManager/Src/MovingAverageFilter.cpp b/AttitudeManager/Src/MovingAverageFilter.cpp new file mode 100644 index 00000000..b44acadf --- /dev/null +++ b/AttitudeManager/Src/MovingAverageFilter.cpp @@ -0,0 +1,52 @@ +#include "MovingAverageFilter.hpp" +#include "FilteringGeneral.hpp" + +#include +#include + +float sumBuf(float* buf, int count) { + float sum = 0; + for (int i = 0; i < count; i++) { + sum += buf[i]; + } + return sum; +} + +int MovingAverageFiltering::WindowCount = 0; + +FilteringError_t MovingAverageFiltering::Init() { + FilteringError_t ret; + ret.errorCode = 0; + + memset(Window.magxVals, 0, sizeof(WINDOW_SIZE)); + + return ret; +} + +FilteringError_t MovingAverageFiltering::Update(MovingAverageOutput_t* output) { + FilteringError_t ret; + ret.errorCode = 0; + + // Get the updated IMU data. + IMUData_t imuData; + FilteringGeneral::imuObj->GetResult(imuData); + if (!imuData.isDataNew) { + ret.errorCode = 1; + return ret; + } + + // Update the window (acts as a circular buffer, we can later use circular_buffer.hpp if we make + // the types generic for that). + int overwritePos = WindowCount % WINDOW_SIZE; + Window.magxVals[overwritePos] = imuData.magx; + WindowCount++; + + // Recompute the average. + unsigned int count = std::min(WindowCount, WINDOW_SIZE); + if (count == 0) count = 1; // Avoid division by 0. + + float magxSum = sumBuf(Window.magxVals, count); + output->magx = magxSum / count; + + return ret; +} \ No newline at end of file diff --git a/AttitudeManager/Src/imu.cpp b/AttitudeManager/Src/imu.cpp new file mode 100644 index 00000000..46c79101 --- /dev/null +++ b/AttitudeManager/Src/imu.cpp @@ -0,0 +1,328 @@ +#include "imu.hpp" + +#include "stm32f7xx_hal.h" +#include "stm32f7xx_hal_spi.h" +#include "stm32f7xx_hal_gpio.h" + +/*********************************************************************************************************************** + * Definitions + **********************************************************************************************************************/ + +#define BMX160_READ_BIT 0x80 +#define BMX160_WRITE_BIT 0x00 +#define BMX160_DUMMY_BYTE 0x00 + +#define CHIP_ID_REG 0x00 +#define DATA_REG 0x04 +#define STATUS_REG 0x1B +#define CMD_REG 0x7E +#define PMU_STATUS_REG 0x03 +#define ACC_CONF_REG 0x40 +#define ACC_RANGE_REG 0x41 +#define GYR_CONF_REG 0x42 +#define GYR_RANGE_REG 0x43 +#define MAG_CONF_REG 0x44 +#define MAG_IF_0_REG 0x4C +#define MAG_IF_1_REG 0x4D +#define MAG_IF_2_REG 0x4E +#define MAG_IF_3_REG 0x4F +#define MAG_REPZ_REG 0x52 +#define MAG_REPXY_REG 0x51 +#define MAG_MODE_REG 0x4B + +#define ACC_NORMAL_MODE_CMD 0x11 +#define GYRO_NORMAL_MODE_CMD 0x15 +#define MAG_NORMAL_MODE_CMD 0x19 + +#define ACC_RANGE_8G 0x08 +#define ACC_ODR_800_OSR4 0x0A // samples are taken at 800 Hz but every 4 are averaged. +#define GYRO_RANGE_1000 0x01 +#define GYRO_ODR_800_OSR4 0x0A +#define MAG_SETUP_EN 0x80 +#define MAG_SETUP_DIS 0x00 +#define REP_XY_REGULAR_PRESET 0x04 // TODO there is also a high accuracy preset. Not too sure what that's about. +#define REP_Z_REGULAR_PRESET 0x0E +#define MAG_IF_3_DATA_MODE 0x02 +#define MAG_IF_2_DATA_MODE 0x4C +#define MAG_IF_1_DATA_MODE 0x42 +#define MAG_REFRESH_50_HZ 0x07 // TODO mag should refresh at 200 Hz as well, but I cant seem to get that to work. +#define MAG_SLEEP_MODE 0x01 + +#define MAX_BUFF_SIZE 21 + +constexpr float GYRO_RANGE_1000_FACTOR = 1879.3f; // LSB/rad/s +constexpr float ACC_RANGE_8_FACTOR = 4.09f; // LSB/mg + +/*********************************************************************************************************************** + * HAL SPI handle + **********************************************************************************************************************/ + +extern SPI_HandleTypeDef hspi1; + +/*********************************************************************************************************************** + * Static variables and static helper function prototypes + **********************************************************************************************************************/ + +static bool dataIsNew; +static void AssertSlaveSelect(void); +static void DeassertSlaveSelect(void); + +#ifdef TARGET_BUILD +/********************TEMPORARY FOR DATA COLLECTION*****************************************/ +static int16_t accXLog[2000]; +static int16_t accYLog[2000]; +static int16_t accZLog[2000]; + +static int16_t gyrXLog[2000]; +static int16_t gyrYLog[2000]; +static int16_t gyrZLog[2000]; + +static uint16_t index; +static uint8_t cnter; + +/********************TEMPORARY FOR DATA COLLECTION*****************************************/ +#endif + +/*********************************************************************************************************************** + * Public methods + ***********************************************************************:D**********************************************/ + +IMU& BMX160::getInstance() +{ + static BMX160 singleton; + return singleton; +} + +void BMX160::Begin_Measuring(void) +{ + Bmx160ReadReg(DATA_REG, rawImuData, 20); +} + +void BMX160::GetResult(IMUData_t& Data) +{ + + int16_t *intImuDataPtr = (int16_t *) &(rawImuData[1]); // first byte is garbage. It's just what was on the line when we asked the IMU for data, which it started sending as of the second byte. + + int16_t magx = intImuDataPtr[0]; + int16_t magy = intImuDataPtr[1]; + int16_t magz = intImuDataPtr[2]; + int16_t gyrx = intImuDataPtr[4]; // missing 3 is not a mistake, there is a field here named RHall which has something to do with the magnetometer. But I can't quite figure that out yet. + int16_t gyry = intImuDataPtr[5]; + int16_t gyrz = intImuDataPtr[6]; + int16_t accx = intImuDataPtr[7]; + int16_t accy = intImuDataPtr[8]; + int16_t accz = intImuDataPtr[9]; + + Data.magx = static_cast (magx); + Data.magy = static_cast (magy); + Data.magz = static_cast (magy); + Data.accx = (static_cast (accx) / ACC_RANGE_8_FACTOR) - ImuCalibrationFinal.accx; + Data.accy = (static_cast (accy) / ACC_RANGE_8_FACTOR) - ImuCalibrationFinal.accy; + Data.accz = (static_cast (accz) / ACC_RANGE_8_FACTOR) - ImuCalibrationFinal.accz; + Data.gyrx = (static_cast (gyrx) / GYRO_RANGE_1000_FACTOR) - ImuCalibrationFinal.gyrx; + Data.gyry = (static_cast (gyry) / GYRO_RANGE_1000_FACTOR) - ImuCalibrationFinal.gyry; + Data.gyrz = (static_cast (gyrz) / GYRO_RANGE_1000_FACTOR) - ImuCalibrationFinal.gyrz; + +#ifdef TARGET_BUILD + +/********************TEMPORARY FOR DATA COLLECTION*****************************************/ + + cnter ++; + + if( (cnter == 50) && (index < 2000) ) + { + cnter = 0; + + accXLog[index] = accx; + accYLog[index] = accy; + accZLog[index] = accz; + + gyrXLog[index] = gyrx; + gyrYLog[index] = gyry; + gyrZLog[index] = gyrz; + + index ++; + } + +/********************TEMPORARY FOR DATA COLLECTION*****************************************/ +#endif + + if (! dataIsNew) + { + Data.isDataNew = false; + return; + } + + Data.isDataNew = true; + Data.sensorStatus = SENSOR_SUCCESS; + + dataIsNew = false; +} + +/*********************************************************************************************************************** + * Private methods + **********************************************************************************************************************/ + +BMX160::BMX160() +{ + HAL_GPIO_WritePin(GPIOG, GPIO_PIN_10, GPIO_PIN_SET); + + SetAllPowerModesToNormal(); + ConfigAcc(); + ConfigGyro(); + ConfigMag(); + + ImuCalibrationFinal.accx = 64.5476685f; + ImuCalibrationFinal.accy = 28.5623455f; + ImuCalibrationFinal.accz = -31.5992432f; + ImuCalibrationFinal.gyrx = 0.00373543333f; + ImuCalibrationFinal.gyry = -0.00117596972f; + ImuCalibrationFinal.gyrz = -0.00466663251f; + ImuCalibrationFinal.magx = 0.0f; + ImuCalibrationFinal.magy = 0.0f; + ImuCalibrationFinal.magz = 0.0f; + + //this->Calibrate(); + + dataIsNew = false; +} + +void BMX160::Calibrate() +{ + const int nSamplesForReliableAverage = 100; + IMUData_t TempImuData; + + ImuCalibration.accx = 0.0f; + ImuCalibration.accy = 0.0f; + ImuCalibration.accz = 0.0f; + ImuCalibration.gyrx = 0.0f; + ImuCalibration.gyry = 0.0f; + ImuCalibration.gyrz = 0.0f; + ImuCalibration.magx = 0.0f; + ImuCalibration.magy = 0.0f; + ImuCalibration.magz = 0.0f; + + for (int i = 0; i < nSamplesForReliableAverage; i++) + { + this->Begin_Measuring(); + + HAL_Delay(7); + + this->GetResult(TempImuData); + + ImuCalibration.gyrx += TempImuData.gyrx; + ImuCalibration.gyry += TempImuData.gyry; + ImuCalibration.gyrz += TempImuData.gyrz; + ImuCalibration.accx += TempImuData.accx; + ImuCalibration.accy += TempImuData.accy; + ImuCalibration.accz += TempImuData.accz; + + } + + ImuCalibration.gyrx /= (float) nSamplesForReliableAverage; + ImuCalibration.gyry /= (float) nSamplesForReliableAverage; + ImuCalibration.gyrz /= (float) nSamplesForReliableAverage; + ImuCalibration.accx /= (float) nSamplesForReliableAverage; + ImuCalibration.accy /= (float) nSamplesForReliableAverage; + ImuCalibration.accz /= (float) nSamplesForReliableAverage; + + ImuCalibration.accz -= 1000.0f; // at calibration, Z needs to read 1g. + + ImuCalibrationFinal = ImuCalibration; +} + +void BMX160::SetAllPowerModesToNormal() +{ + + Bmx160WriteReg(CMD_REG, ACC_NORMAL_MODE_CMD); + Bmx160WriteReg(CMD_REG, GYRO_NORMAL_MODE_CMD); + Bmx160WriteReg(CMD_REG, MAG_NORMAL_MODE_CMD); +} + +void BMX160::ConfigAcc() +{ + + Bmx160WriteReg(ACC_RANGE_REG, ACC_RANGE_8G); + Bmx160WriteReg(ACC_CONF_REG, ACC_ODR_800_OSR4); +} + + +void BMX160::ConfigGyro() +{ + + Bmx160WriteReg(GYR_RANGE_REG, GYRO_RANGE_1000); + Bmx160WriteReg(GYR_CONF_REG, GYRO_ODR_800_OSR4); +} + +void BMX160::ConfigMag() +{ + Bmx160WriteReg(MAG_IF_0_REG, MAG_SETUP_EN); + + + SetMagConfig(MAG_MODE_REG, MAG_SLEEP_MODE); + SetMagConfig(MAG_REPXY_REG, REP_XY_REGULAR_PRESET); + SetMagConfig(MAG_REPZ_REG, REP_Z_REGULAR_PRESET); + + PrepareMagForDataMode(); + + Bmx160WriteReg(MAG_CONF_REG, MAG_REFRESH_50_HZ); + Bmx160WriteReg(MAG_IF_0_REG, MAG_SETUP_DIS); + +} + +void BMX160::SetMagConfig(uint8_t regAddr, uint8_t data) +{ + Bmx160WriteReg(MAG_IF_3_REG, data); + Bmx160WriteReg(MAG_IF_2_REG, regAddr); +} + +void BMX160::PrepareMagForDataMode(void) +{ + + Bmx160WriteReg(MAG_IF_3_REG, MAG_IF_3_DATA_MODE); + Bmx160WriteReg(MAG_IF_2_REG, MAG_IF_2_DATA_MODE); + Bmx160WriteReg(MAG_IF_1_REG, MAG_IF_1_DATA_MODE); +} + +void BMX160::Bmx160WriteReg(uint8_t reg, uint8_t val) +{ + uint8_t rx[2] = { 0, 0 }; // we don't care about what gets sent back to us in write mode + uint8_t tx[2] = { static_cast (reg | BMX160_WRITE_BIT), val }; // Set first bit to 0 + + AssertSlaveSelect(); + + HAL_SPI_TransmitReceive_IT(&hspi1, tx, rx, 2); + + HAL_Delay(20); // writes only happen in config. Allow some time for the writes to complete before moving on to the next write +} + +void BMX160::Bmx160ReadReg(uint8_t const regAddr, uint8_t *pData, uint8_t len) +{ + uint8_t tx[MAX_BUFF_SIZE] = {0}; + + tx[0] = static_cast (regAddr | BMX160_READ_BIT); + + AssertSlaveSelect(); + + HAL_SPI_TransmitReceive_IT(&hspi1, tx, pData, (len + 1)); +} + +/*********************************************************************************************************************** + * Interrupt Callbacks and static helpers functions + **********************************************************************************************************************/ + +void ImuTxRxCallback() { + DeassertSlaveSelect(); + + dataIsNew = true; +} + +static void AssertSlaveSelect(void) +{ + HAL_GPIO_WritePin(GPIOG, GPIO_PIN_10, GPIO_PIN_RESET); +} + +static void DeassertSlaveSelect(void) +{ + HAL_GPIO_WritePin(GPIOG, GPIO_PIN_10, GPIO_PIN_SET); +} \ No newline at end of file From bddd94e60eda093077dd6f0164bbedf6213e1fd1 Mon Sep 17 00:00:00 2001 From: twilkhoo Date: Mon, 11 Mar 2024 23:33:41 -0400 Subject: [PATCH 2/2] Make MovingAverage generic and create a specific IMU implementation for it. --- AttitudeManager/Inc/FilteringGeneral.hpp | 21 ------- AttitudeManager/Inc/FilteringInterface.hpp | 12 ---- .../Inc/IMUMovingAverageFilter.hpp | 25 ++++++++ AttitudeManager/Inc/MovingAverage.hpp | 27 +++++++++ AttitudeManager/Inc/MovingAverageFilter.hpp | 27 --------- AttitudeManager/Src/FilteringGeneral.cpp | 29 ---------- AttitudeManager/Src/FilteringInterface.cpp | 25 -------- .../Src/IMUMovingAverageFilter.cpp | 57 +++++++++++++++++++ AttitudeManager/Src/MovingAverage.cpp | 41 +++++++++++++ AttitudeManager/Src/MovingAverageFilter.cpp | 52 ----------------- 10 files changed, 150 insertions(+), 166 deletions(-) delete mode 100644 AttitudeManager/Inc/FilteringGeneral.hpp delete mode 100644 AttitudeManager/Inc/FilteringInterface.hpp create mode 100644 AttitudeManager/Inc/IMUMovingAverageFilter.hpp create mode 100644 AttitudeManager/Inc/MovingAverage.hpp delete mode 100644 AttitudeManager/Inc/MovingAverageFilter.hpp delete mode 100644 AttitudeManager/Src/FilteringGeneral.cpp delete mode 100644 AttitudeManager/Src/FilteringInterface.cpp create mode 100644 AttitudeManager/Src/IMUMovingAverageFilter.cpp create mode 100644 AttitudeManager/Src/MovingAverage.cpp delete mode 100644 AttitudeManager/Src/MovingAverageFilter.cpp diff --git a/AttitudeManager/Inc/FilteringGeneral.hpp b/AttitudeManager/Inc/FilteringGeneral.hpp deleted file mode 100644 index 2a556d20..00000000 --- a/AttitudeManager/Inc/FilteringGeneral.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef FILTERINGGENERAL_HPP -#define FILTERINGGENERAL_HPP - -#include "imu.hpp" - -// -1 = FAILED -// 0 = SUCCESS -// 1 = Old Data -struct FilteringError_t { - int errorCode; -}; - -class FilteringGeneral { -public: - static IMU *imuObj; - static FilteringError_t Init(); - static IMUData_t GetRawIMU(); -}; - - -#endif diff --git a/AttitudeManager/Inc/FilteringInterface.hpp b/AttitudeManager/Inc/FilteringInterface.hpp deleted file mode 100644 index e1a9f461..00000000 --- a/AttitudeManager/Inc/FilteringInterface.hpp +++ /dev/null @@ -1,12 +0,0 @@ -// FilteringInterface is the only source that will be used directly by the client. -// We define the exact filters we want to use in FilteringInterface.cpp. - -#ifndef FILTERING_INTERFACE_HPP -#define FILTERING_INTERFACE_HPP - -#include "FilteringGeneral.hpp" - -FilteringError_t FilteringInterfaceInit(void); -FilteringError_t FilteringInterfaceExecute(void); - -#endif \ No newline at end of file diff --git a/AttitudeManager/Inc/IMUMovingAverageFilter.hpp b/AttitudeManager/Inc/IMUMovingAverageFilter.hpp new file mode 100644 index 00000000..5be56531 --- /dev/null +++ b/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 magxVals; + Window magyVals; + Window magzVals; + Window accxVals; + Window accyVals; + Window acczVals; + Window gyrxVals; + Window gyryVals; + Window gyrzVals; +}; + +#endif // IMU_MOVING_AVERAGE_FILTER_HPP diff --git a/AttitudeManager/Inc/MovingAverage.hpp b/AttitudeManager/Inc/MovingAverage.hpp new file mode 100644 index 00000000..38c5808f --- /dev/null +++ b/AttitudeManager/Inc/MovingAverage.hpp @@ -0,0 +1,27 @@ +#ifndef MOVING_AVERAGE_HPP +#define MOVING_AVERAGE_HPP + +#include + +#define MAX_WINDOW_SIZE 50 +#define MAX_POS_LIMIT MAX_WINDOW_SIZE * 1000000 + +enum MovingAverageError_t { + SUCCESS, + FAILURE, + OLD_VALUES, +}; + +template +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 diff --git a/AttitudeManager/Inc/MovingAverageFilter.hpp b/AttitudeManager/Inc/MovingAverageFilter.hpp deleted file mode 100644 index 2a2d3997..00000000 --- a/AttitudeManager/Inc/MovingAverageFilter.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef MOVING_AVERAGE_FILTER_HPP -#define MOVING_AVERAGE_FILTER_HPP - -#define WINDOW_SIZE 50 - -#include "FilteringGeneral.hpp" -#include "circular_buffer.hpp" - -struct MovingAverageOutput_t { - float magx; -}; - -struct MovingAverageWindow { - float magxVals[WINDOW_SIZE]; -}; - -class MovingAverageFiltering { -public: - static FilteringError_t Init(); - static FilteringError_t Update(MovingAverageOutput_t* output); - -private: - static MovingAverageWindow Window; - static int WindowCount; -}; - -#endif diff --git a/AttitudeManager/Src/FilteringGeneral.cpp b/AttitudeManager/Src/FilteringGeneral.cpp deleted file mode 100644 index 6caaedf4..00000000 --- a/AttitudeManager/Src/FilteringGeneral.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "FilteringGeneral.hpp" -#include "imu.hpp" -#include - -namespace FilteringGeneral { - -IMU* FilteringGeneral::imuObj = nullptr; - -FilteringError_t FilteringGeneral::Init() { - FilteringError_t ret; - ret.errorCode = 0; - - imuObj = &BMX160::getInstance(); - - return ret; -} - -IMUData_t FilteringGeneral::GetRawIMU() { - IMUData_t imuData; - imuObj->GetResult(imuData); - - IMUData_t imuOutput; - - std:memcpy(&imuOutput, &imuData, sizeof(IMUData_t)); - - return imuOutput; -} - -} diff --git a/AttitudeManager/Src/FilteringInterface.cpp b/AttitudeManager/Src/FilteringInterface.cpp deleted file mode 100644 index b08453ad..00000000 --- a/AttitudeManager/Src/FilteringInterface.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "FilteringInterface.hpp" -#include "FilteringGeneral.hpp" -#include "MovingAverageFilter.hpp" - -FilteringError_t FilteringInterfaceInit(void) { - FilteringError_t ret; - - ret = FilteringGeneral::Init(); - if (ret.errorCode != 0) return ret; - - MovingAverageFiltering::Init(); - if (ret.errorCode != 0) return ret; - - return ret; -} - -FilteringError_t FilteringInterfaceExecute(void) { - FilteringError_t ret; - - MovingAverageOutput_t output; - ret = MovingAverageFiltering::Update(&output); - if (ret.errorCode != 0) return ret; - - return ret; -} diff --git a/AttitudeManager/Src/IMUMovingAverageFilter.cpp b/AttitudeManager/Src/IMUMovingAverageFilter.cpp new file mode 100644 index 00000000..34bd067e --- /dev/null +++ b/AttitudeManager/Src/IMUMovingAverageFilter.cpp @@ -0,0 +1,57 @@ +#include "IMUMovingAverageFilter.hpp" +#include "imu.hpp" +#include "MovingAverage.hpp" + +IMUMovingAverageFilter::IMUMovingAverageFilter() { + imuObj = &BMX160::getInstance(); + magxVals = Window(); + magyVals = Window(); + magzVals = Window(); + accxVals = Window(); + accyVals = Window(); + acczVals = Window(); + gyrxVals = Window(); + gyryVals = Window(); + gyrzVals = Window(); +} + +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; +} diff --git a/AttitudeManager/Src/MovingAverage.cpp b/AttitudeManager/Src/MovingAverage.cpp new file mode 100644 index 00000000..bbe21651 --- /dev/null +++ b/AttitudeManager/Src/MovingAverage.cpp @@ -0,0 +1,41 @@ +#include "MovingAverage.hpp" +#include + +template +Window::Window() { + memset(window, 0, MAX_WINDOW_SIZE); + size = 0; + pos = 0; +} + +template +void Window::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 +MovingAverageError_t Window::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(sum) / n; + return SUCCESS; +} diff --git a/AttitudeManager/Src/MovingAverageFilter.cpp b/AttitudeManager/Src/MovingAverageFilter.cpp deleted file mode 100644 index b44acadf..00000000 --- a/AttitudeManager/Src/MovingAverageFilter.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "MovingAverageFilter.hpp" -#include "FilteringGeneral.hpp" - -#include -#include - -float sumBuf(float* buf, int count) { - float sum = 0; - for (int i = 0; i < count; i++) { - sum += buf[i]; - } - return sum; -} - -int MovingAverageFiltering::WindowCount = 0; - -FilteringError_t MovingAverageFiltering::Init() { - FilteringError_t ret; - ret.errorCode = 0; - - memset(Window.magxVals, 0, sizeof(WINDOW_SIZE)); - - return ret; -} - -FilteringError_t MovingAverageFiltering::Update(MovingAverageOutput_t* output) { - FilteringError_t ret; - ret.errorCode = 0; - - // Get the updated IMU data. - IMUData_t imuData; - FilteringGeneral::imuObj->GetResult(imuData); - if (!imuData.isDataNew) { - ret.errorCode = 1; - return ret; - } - - // Update the window (acts as a circular buffer, we can later use circular_buffer.hpp if we make - // the types generic for that). - int overwritePos = WindowCount % WINDOW_SIZE; - Window.magxVals[overwritePos] = imuData.magx; - WindowCount++; - - // Recompute the average. - unsigned int count = std::min(WindowCount, WINDOW_SIZE); - if (count == 0) count = 1; // Avoid division by 0. - - float magxSum = sumBuf(Window.magxVals, count); - output->magx = magxSum / count; - - return ret; -} \ No newline at end of file