From 215fe8e9e84b8d5af4955d77f372957b68771331 Mon Sep 17 00:00:00 2001 From: Alessandro Gatti Date: Thu, 10 May 2018 02:02:29 +0200 Subject: [PATCH] Reduce code size for accelerometer support (#288) * Reduce the LIS2DH12 driver's code size. * Reduce the MMA8652FC driver's code size. * Make orientation detection smaller. * Inlined C++ class constructor. * De-unroll I2C register writes. * Removed unused setSensitivity method. --- workspace/TS100/inc/LIS2DH12.hpp | 8 ++-- workspace/TS100/inc/MMA8652FC.hpp | 7 ++-- workspace/TS100/inc/hardware.h | 6 +++ workspace/TS100/src/LIS2DH12.cpp | 58 +++++++++++---------------- workspace/TS100/src/MMA8652FC.cpp | 66 ++++++++++++++++--------------- workspace/TS100/src/main.cpp | 6 +-- 6 files changed, 76 insertions(+), 75 deletions(-) diff --git a/workspace/TS100/inc/LIS2DH12.hpp b/workspace/TS100/inc/LIS2DH12.hpp index 243803d5..2721e7c4 100644 --- a/workspace/TS100/inc/LIS2DH12.hpp +++ b/workspace/TS100/inc/LIS2DH12.hpp @@ -10,16 +10,16 @@ #include "stm32f1xx_hal.h" #include "FRToSI2C.hpp" #include "LIS2DH12_defines.hpp" +#include "hardware.h" + class LIS2DH12 { public: - LIS2DH12(FRToSI2C* i2cHandle); + LIS2DH12(FRToSI2C* i2cHandle) : i2c(i2cHandle) {} void initalize(); - uint8_t getOrientation(); + Orientation getOrientation() { return static_cast((I2C_RegisterRead(LIS_INT2_SRC) >> 2) - 1); } void getAxisReadings(int16_t *x, int16_t *y, int16_t *z); private: - void setSensitivity(uint8_t threshold, uint8_t filterTime); // Sets the sensitivity of the unit - void I2C_RegisterWrite(uint8_t reg, uint8_t data); uint8_t I2C_RegisterRead(uint8_t reg); FRToSI2C* i2c; diff --git a/workspace/TS100/inc/MMA8652FC.hpp b/workspace/TS100/inc/MMA8652FC.hpp index bbd778c2..84f9fb0e 100644 --- a/workspace/TS100/inc/MMA8652FC.hpp +++ b/workspace/TS100/inc/MMA8652FC.hpp @@ -10,17 +10,18 @@ #include "stm32f1xx_hal.h" #include "MMA8652FC_defines.h" #include "FRToSI2C.hpp" +#include "hardware.h" + class MMA8652FC { public: - MMA8652FC(FRToSI2C* i2cHandle); + MMA8652FC(FRToSI2C* i2cHandle) : i2c(i2cHandle) {} void initalize(); // Initalize the system - uint8_t getOrientation();// Reads the I2C register and returns the orientation (true == left) + Orientation getOrientation();// Reads the I2C register and returns the orientation (true == left) void getAxisReadings(int16_t *x, int16_t *y, int16_t *z); private: - void setSensitivity(uint8_t threshold, uint8_t filterTime); // Sets the sensitivity of the unit void I2C_RegisterWrite(uint8_t reg, uint8_t data); uint8_t I2C_RegisterRead(uint8_t reg); diff --git a/workspace/TS100/inc/hardware.h b/workspace/TS100/inc/hardware.h index 49e1f73b..ca149c43 100644 --- a/workspace/TS100/inc/hardware.h +++ b/workspace/TS100/inc/hardware.h @@ -13,6 +13,12 @@ extern "C" { #endif +enum Orientation { + ORIENTATION_LEFT_HAND = 0, + ORIENTATION_RIGHT_HAND = 1, + ORIENTATION_FLAT = 3 +}; + #define KEY_B_Pin GPIO_PIN_6 #define KEY_B_GPIO_Port GPIOA #define TMP36_INPUT_Pin GPIO_PIN_7 diff --git a/workspace/TS100/src/LIS2DH12.cpp b/workspace/TS100/src/LIS2DH12.cpp index c0b6b822..40f9be53 100644 --- a/workspace/TS100/src/LIS2DH12.cpp +++ b/workspace/TS100/src/LIS2DH12.cpp @@ -7,39 +7,32 @@ #include #include "cmsis_os.h" -LIS2DH12::LIS2DH12(FRToSI2C* i2cHandle) { - i2c = i2cHandle; -} + +typedef struct { + const uint8_t reg; + const uint8_t value; +} LIS_REG; + +static const LIS_REG i2c_registers[] = { + {LIS_CTRL_REG1, 0x17}, // 25Hz + {LIS_CTRL_REG2, 0b00001000}, // Highpass filter off + {LIS_CTRL_REG3, 0b01100000}, // Setup interrupt pins + {LIS_CTRL_REG4, 0b00001000}, // Block update mode off, HR on + {LIS_CTRL_REG5, 0b00000010}, + {LIS_CTRL_REG6, 0b01100010}, + //Basically setup the unit to run, and enable 4D orientation detection + {LIS_INT2_CFG, 0b01111110}, //setup for movement detection + {LIS_INT2_THS, 0x28}, + {LIS_INT2_DURATION, 64}, + {LIS_INT1_CFG, 0b01111110}, + {LIS_INT1_THS, 0x28}, + {LIS_INT1_DURATION, 64} +}; void LIS2DH12::initalize() { - I2C_RegisterWrite(LIS_CTRL_REG1, 0x17); //25Hz - I2C_RegisterWrite(LIS_CTRL_REG2, 0b00001000); //Highpass filter off - I2C_RegisterWrite(LIS_CTRL_REG3, 0b01100000); //Setup interrupt pins - I2C_RegisterWrite(LIS_CTRL_REG4, 0b00001000); //Block update mode off,HR on - I2C_RegisterWrite(LIS_CTRL_REG5, 0b00000010); - I2C_RegisterWrite(LIS_CTRL_REG6, 0b01100010); - - //Basically setup the unit to run, and enable 4D orientation detection - I2C_RegisterWrite(LIS_INT2_CFG, 0b01111110); //setup for movement detection - I2C_RegisterWrite(LIS_INT2_THS, 0x28); - I2C_RegisterWrite(LIS_INT2_DURATION, 64); - I2C_RegisterWrite(LIS_INT1_CFG, 0b01111110); //setup for movement detection - I2C_RegisterWrite(LIS_INT1_THS, 0x28); - I2C_RegisterWrite(LIS_INT1_DURATION, 64); - -} - -//0=no change, 1= right handed, 2= left handed -uint8_t LIS2DH12::getOrientation() { - // 8=right handed,4=left,16=flat - //So we ignore if not 8/4 - uint8_t pos = I2C_RegisterRead(LIS_INT2_SRC); - if (pos == 8) - return 1; - else if (pos == 4) - return 2; - else - return 0; + for (size_t index = 0; index < (sizeof(i2c_registers) / sizeof(i2c_registers[0])); index++) { + I2C_RegisterWrite(i2c_registers[index].reg, i2c_registers[index].value); + } } void LIS2DH12::getAxisReadings(int16_t* x, int16_t* y, int16_t* z) { @@ -52,9 +45,6 @@ void LIS2DH12::getAxisReadings(int16_t* x, int16_t* y, int16_t* z) { (*z) = ((uint16_t) (tempArr[5] << 8 | tempArr[4])); } -void LIS2DH12::setSensitivity(uint8_t threshold, uint8_t filterTime) { -} - void LIS2DH12::I2C_RegisterWrite(uint8_t reg, uint8_t data) { i2c->Mem_Write(LIS2DH_I2C_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, &data, 1); diff --git a/workspace/TS100/src/MMA8652FC.cpp b/workspace/TS100/src/MMA8652FC.cpp index 4890650a..5825a880 100644 --- a/workspace/TS100/src/MMA8652FC.cpp +++ b/workspace/TS100/src/MMA8652FC.cpp @@ -8,9 +8,26 @@ #include #include "cmsis_os.h" -MMA8652FC::MMA8652FC(FRToSI2C* i2cHandle) { - i2c = i2cHandle; -} +typedef struct { + const uint8_t reg; + const uint8_t val; +} MMA_REG; + +static const MMA_REG i2c_registers[] = { + {CTRL_REG2, 0}, //Normal mode + {CTRL_REG2, 0x40}, // Reset all registers to POR values + {FF_MT_CFG_REG, 0x78}, // Enable motion detection for X, Y, Z axis, latch disabled + {PL_CFG_REG, 0x40}, //Enable the orientation detection + {PL_COUNT_REG, 200}, //200 count debounce + {PL_BF_ZCOMP_REG, 0b01000111}, //Set the threshold to 42 degrees + {P_L_THS_REG, 0b10011100}, //Up the trip angles + {CTRL_REG4, 0x01 | (1 << 4)}, // Enable dataready interrupt & orientation interrupt + {CTRL_REG5, 0x01}, // Route data ready interrupts to INT1 ->PB5 ->EXTI5, leaving orientation routed to INT2 + {CTRL_REG2, 0x12}, //Set maximum resolution oversampling + {XYZ_DATA_CFG_REG, (1 << 4)}, //select high pass filtered data + {HP_FILTER_CUTOFF_REG, 0x03}, //select high pass filtered data + {CTRL_REG1, 0x19} // ODR=12 Hz, Active mode +}; void MMA8652FC::I2C_RegisterWrite(uint8_t reg, uint8_t data) { i2c->Mem_Write( MMA8652FC_I2C_ADDRESS, reg, I2C_MEMADD_SIZE_8BIT, &data, 1); @@ -24,47 +41,34 @@ uint8_t MMA8652FC::I2C_RegisterRead(uint8_t reg) { return tx_data[0]; } void MMA8652FC::initalize() { + size_t index = 0; + //send all the init commands to the unit - I2C_RegisterWrite(CTRL_REG2, 0); //Normal mode - I2C_RegisterWrite( CTRL_REG2, 0x40); // Reset all registers to POR values + + I2C_RegisterWrite(i2c_registers[index].reg, i2c_registers[index].val); index++; + I2C_RegisterWrite(i2c_registers[index].reg, i2c_registers[index].val); index++; + HAL_Delay(2); // ~1ms delay - I2C_RegisterWrite(FF_MT_CFG_REG, 0x78); // Enable motion detection for X, Y, Z axis, latch disabled - - I2C_RegisterWrite(PL_CFG_REG, 0x40); //Enable the orientation detection - I2C_RegisterWrite(PL_COUNT_REG, 200); //200 count debounce - I2C_RegisterWrite(PL_BF_ZCOMP_REG, 0b01000111); //Set the threshold to 42 degrees - I2C_RegisterWrite(P_L_THS_REG, 0b10011100); //Up the trip angles - I2C_RegisterWrite( CTRL_REG4, 0); // Disable IRQ's - I2C_RegisterWrite( CTRL_REG5, 0x01); // Route data ready interrupts to INT1 ->PB5 ->EXTI5, leaving orientation routed to INT2 - I2C_RegisterWrite( CTRL_REG2, 0x12); //Set maximum resolution oversampling - I2C_RegisterWrite( XYZ_DATA_CFG_REG, (1 << 4)); //select high pass filtered data - I2C_RegisterWrite( HP_FILTER_CUTOFF_REG, 0x03); //select high pass filtered data - - I2C_RegisterWrite( CTRL_REG1, 0x19); // ODR=12 Hz, Active mode + while (index < (sizeof(i2c_registers) / sizeof(i2c_registers[0]))) { + I2C_RegisterWrite(i2c_registers[index].reg, i2c_registers[index].val); index++; + } } -void MMA8652FC::setSensitivity(uint8_t threshold, uint8_t filterTime) { - uint8_t sens = 9 * 2 + 17; - sens -= 2 * threshold; - I2C_RegisterWrite( CTRL_REG1, 0); // sleep mode - I2C_RegisterWrite(FF_MT_THS_REG, (sens & 0x7F));// Set accumulation threshold - I2C_RegisterWrite(FF_MT_COUNT_REG, filterTime); // Set debounce threshold - I2C_RegisterWrite( CTRL_REG1, 0x31); // ODR=12 Hz, Active mode -} - -uint8_t MMA8652FC::getOrientation() { +Orientation MMA8652FC::getOrientation() { //First read the PL_STATUS register uint8_t plStatus = I2C_RegisterRead(PL_STATUS_REG); if ((plStatus & 0b10000000) == 0b10000000) { plStatus >>= 1; //We don't need the up/down bit plStatus &= 0x03; //mask to the two lower bits + //0 == left handed //1 == right handed - return plStatus == 0 ? 2 : 1; - } else - return 0; + return static_cast(plStatus); + } + + return ORIENTATION_FLAT; } void MMA8652FC::getAxisReadings(int16_t *x, int16_t *y, int16_t *z) { uint8_t tempArr[6]; diff --git a/workspace/TS100/src/main.cpp b/workspace/TS100/src/main.cpp index 44249442..3ba2a409 100644 --- a/workspace/TS100/src/main.cpp +++ b/workspace/TS100/src/main.cpp @@ -881,7 +881,7 @@ void startMOVTask(void const *argument) { #if ACCELDEBUG uint32_t max = 0; #endif - uint8_t rotation = 0; + Orientation rotation = ORIENTATION_FLAT; for (;;) { int32_t threshold = 1500 + (9 * 200); threshold -= systemSettings.sensitivity * 200; // 200 is the step size @@ -894,8 +894,8 @@ void startMOVTask(void const *argument) { rotation = accel.getOrientation(); } if (systemSettings.OrientationMode == 2) { - if (rotation != 0) { - lcd.setRotation(rotation == 2); // link the data through + if (rotation != ORIENTATION_FLAT) { + lcd.setRotation(rotation == ORIENTATION_LEFT_HAND); // link the data through } } datax[currentPointer] = (int32_t) tx;