From 0cef5f8eb673b89d772de3426162e8d55b03a3da Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Mon, 4 Jan 2021 18:22:33 +1100 Subject: [PATCH 1/5] Create approx driver for setup --- workspace/TS100/Core/Drivers/MSA301.cpp | 40 +++++++++++++++++++ workspace/TS100/Core/Drivers/MSA301.h | 27 +++++++++++++ workspace/TS100/Core/Drivers/MSA301_defines.h | 34 ++++++++++++++++ 3 files changed, 101 insertions(+) create mode 100644 workspace/TS100/Core/Drivers/MSA301.cpp create mode 100644 workspace/TS100/Core/Drivers/MSA301.h create mode 100644 workspace/TS100/Core/Drivers/MSA301_defines.h diff --git a/workspace/TS100/Core/Drivers/MSA301.cpp b/workspace/TS100/Core/Drivers/MSA301.cpp new file mode 100644 index 0000000000..8b35b628ac --- /dev/null +++ b/workspace/TS100/Core/Drivers/MSA301.cpp @@ -0,0 +1,40 @@ +/* + * MSA301.cpp + * + * Created on: 3 Jan 2021 + * Author: Ralim + */ + +#include +#include "MSA301_defines.h" +#define MSA301_I2C_ADDRESS 0x4C +bool MSA301::detect() { + return FRToSI2C::probe(MSA301_I2C_ADDRESS); +} + +static const FRToSI2C::I2C_REG i2c_registers[] = { // + // + { MSA301_REG_ODR, 0b00001000, 1 }, //X/Y/Z enabled @ 250Hz + { MSA301_REG_POWERMODE, 0b0001001, 1 }, // Normal mode + { MSA301_REG_RESRANGE, 0b00000001, 0 }, // 14bit resolution @ 4G range + { MSA301_REG_ORIENT_HY, 0b01000000, 0 }, // 4*62.5mg hyst, no blocking, symmetrical + { MSA301_REG_ORIENT_HY, 0b01000000, 0 }, // 4*62.5mg hyst, no blocking, symmetrical + + }; + +bool MSA301::initalize() { + // Enable X/Y/Z + // Normal mode + // 250Hz filter + // 4G range + // 14 bit resolution + + return FRToSI2C::writeRegistersBulk(MSA301_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); +} + +Orientation MSA301::getOrientation() { + //read MSA301_REG_ORIENT_STATUS +} + +void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { +} diff --git a/workspace/TS100/Core/Drivers/MSA301.h b/workspace/TS100/Core/Drivers/MSA301.h new file mode 100644 index 0000000000..b77e500d9e --- /dev/null +++ b/workspace/TS100/Core/Drivers/MSA301.h @@ -0,0 +1,27 @@ +/* + * MSA301.h + * + * Created on: 3 Jan 2021 + * Author: Ralim + */ + +#ifndef DRIVERS_MSA301_H_ +#define DRIVERS_MSA301_H_ +#include "I2C_Wrapper.hpp" +#include "BSP.h" + +class MSA301 { +public: + //Returns true if this accelerometer is detected + static bool detect(); + //Init any internal state + static bool initalize(); + // Reads the I2C register and returns the orientation + static Orientation getOrientation(); + //Return the x/y/z axis readings as signed int16's + static void getAxisReadings(int16_t &x, int16_t &y, int16_t &z); + +private: +}; + +#endif /* DRIVERS_MSA301_H_ */ diff --git a/workspace/TS100/Core/Drivers/MSA301_defines.h b/workspace/TS100/Core/Drivers/MSA301_defines.h new file mode 100644 index 0000000000..12ebc62cae --- /dev/null +++ b/workspace/TS100/Core/Drivers/MSA301_defines.h @@ -0,0 +1,34 @@ +/* + * MSA301_defines.h + * + * Created on: 3 Jan 2021 + * Author: Ralim + */ + +#ifndef DRIVERS_MSA301_DEFINES_H_ +#define DRIVERS_MSA301_DEFINES_H_ +//Definitions from Adafruit <3 + +#define MSA301_REG_PARTID 0x01 ///< Register that contains the part ID +#define MSA301_REG_OUT_X_L 0x02 ///< Register address for X axis lower byte +#define MSA301_REG_OUT_X_H 0x03 ///< Register address for X axis higher byte +#define MSA301_REG_OUT_Y_L 0x04 ///< Register address for Y axis lower byte +#define MSA301_REG_OUT_Y_H 0x05 ///< Register address for Y axis higher byte +#define MSA301_REG_OUT_Z_L 0x06 ///< Register address for Z axis lower byte +#define MSA301_REG_OUT_Z_H 0x07 ///< Register address for Z axis higher byte +#define MSA301_REG_MOTIONINT 0x09 ///< Register address for motion interrupt +#define MSA301_REG_DATAINT 0x0A ///< Register address for data interrupt +#define MSA301_REG_CLICKSTATUS 0x0B ///< Register address for click/doubleclick status +#define MSA301_REG_RESRANGE 0x0F ///< Register address for resolution range +#define MSA301_REG_ODR 0x10 ///< Register address for data rate setting +#define MSA301_REG_POWERMODE 0x11 ///< Register address for power mode setting +#define MSA301_REG_INTSET0 0x16 ///< Register address for interrupt setting #0 +#define MSA301_REG_INTSET1 0x17 ///< Register address for interrupt setting #1 +#define MSA301_REG_INTMAP0 0x19 ///< Register address for interrupt map #0 +#define MSA301_REG_INTMAP1 0x1A ///< Register address for interrupt map #1 +#define MSA301_REG_TAPDUR 0x2A ///< Register address for tap duration +#define MSA301_REG_TAPTH 0x2B ///< Register address for tap threshold +#define MSA301_REG_ORIENT_HY 0x2C ///< Register address for orientation Hysteresis +#define MSA301_REG_ORIENT_STATUS 0x0C ///< Register address for orientation hysteresis + +#endif /* DRIVERS_MSA301_DEFINES_H_ */ From adc2b1ff709fd07be7c4088ad0bd12339feddd6b Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Mon, 4 Jan 2021 20:45:10 +1100 Subject: [PATCH 2/5] Update MSA301 driver to fix orientation detection and cleanup --- workspace/TS100/Core/Drivers/MSA301.cpp | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/workspace/TS100/Core/Drivers/MSA301.cpp b/workspace/TS100/Core/Drivers/MSA301.cpp index 8b35b628ac..34f879bce1 100644 --- a/workspace/TS100/Core/Drivers/MSA301.cpp +++ b/workspace/TS100/Core/Drivers/MSA301.cpp @@ -18,23 +18,32 @@ static const FRToSI2C::I2C_REG i2c_registers[] = { // { MSA301_REG_POWERMODE, 0b0001001, 1 }, // Normal mode { MSA301_REG_RESRANGE, 0b00000001, 0 }, // 14bit resolution @ 4G range { MSA301_REG_ORIENT_HY, 0b01000000, 0 }, // 4*62.5mg hyst, no blocking, symmetrical - { MSA301_REG_ORIENT_HY, 0b01000000, 0 }, // 4*62.5mg hyst, no blocking, symmetrical + { MSA301_REG_INTSET0, 1 << 6, 0 }, // Turn on orientation detection (by enabling its interrupt) }; bool MSA301::initalize() { - // Enable X/Y/Z - // Normal mode - // 250Hz filter - // 4G range - // 14 bit resolution - return FRToSI2C::writeRegistersBulk(MSA301_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0])); } Orientation MSA301::getOrientation() { - //read MSA301_REG_ORIENT_STATUS + uint8_t temp = 0; + FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_ORIENT_STATUS, &temp, 1); + switch (temp) { + case 112: + return Orientation::ORIENTATION_LEFT_HAND; + case 96: + return Orientation::ORIENTATION_RIGHT_HAND; + } + return Orientation::ORIENTATION_FLAT; } void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { + uint8_t temp[6]; + //Bulk read all 6 regs + FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_OUT_X_L, temp, 6); + x = int16_t(((int16_t) temp[1]) << 8 | temp[0]) >> 2; + y = int16_t(((int16_t) temp[3]) << 8 | temp[2]) >> 2; + z = int16_t(((int16_t) temp[5]) << 8 | temp[4]) >> 2; + } From ad9f2a4f954f7d868fe48ae700fb9242469a61df Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Mon, 4 Jan 2021 21:02:24 +1100 Subject: [PATCH 3/5] Link MSA301 into normal MOV handler --- .../TS100/Core/BSP/Miniware/Model_Config.h | 1 + workspace/TS100/Core/Threads/MOVThread.cpp | 30 +++++++++++-------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/workspace/TS100/Core/BSP/Miniware/Model_Config.h b/workspace/TS100/Core/BSP/Miniware/Model_Config.h index f7beb79e78..664208ad2b 100644 --- a/workspace/TS100/Core/BSP/Miniware/Model_Config.h +++ b/workspace/TS100/Core/BSP/Miniware/Model_Config.h @@ -36,6 +36,7 @@ #ifdef MODEL_TS80P #define ACCEL_LIS +#define ACCEL_MSA #define POW_PD #define POW_QC #define TEMP_NTC diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp index 84ff386eb8..2705e72d73 100755 --- a/workspace/TS100/Core/Threads/MOVThread.cpp +++ b/workspace/TS100/Core/Threads/MOVThread.cpp @@ -10,6 +10,7 @@ #include "I2C_Wrapper.hpp" #include "LIS2DH12.hpp" #include "MMA8652FC.hpp" +#include "MSA301.h" #include "QC3.h" #include "Settings.h" #include "TipThermoModel.h" @@ -24,36 +25,41 @@ uint8_t accelInit = 0; TickType_t lastMovementTime = 0; void detectAccelerometerVersion() { + PCBVersion = 99; #ifdef ACCEL_MMA if (MMA8652FC::detect()) { - PCBVersion = 1; - if (!MMA8652FC::initalize()) { - PCBVersion = 99; + + if (MMA8652FC::initalize()) { + PCBVersion = 1; } } else #endif #ifdef ACCEL_LIS if (LIS2DH12::detect()) { - PCBVersion = 2; // Setup the ST Accelerometer - if (!LIS2DH12::initalize()) { - PCBVersion = 99; + if (LIS2DH12::initalize()) { + PCBVersion = 2; } } else #endif #ifdef ACCEL_BMA if (BMA223::detect()) { - PCBVersion = 3; // Setup the ST Accelerometer - if (!BMA223::initalize()) { - PCBVersion = 99; + if (BMA223::initalize()) { + PCBVersion = 3; + } + } else +#endif +#ifdef ACCEL_MSA + if (MSA301::detect()) { + // Setup the MSA301 Accelerometer + if (MSA301::initalize()) { + PCBVersion = 4; } } else #endif { - PCBVersion = 99; - systemSettings.SleepTime = 0; - systemSettings.ShutdownTime = 0; // No accel -> disable sleep + //disable imu sensitivity systemSettings.sensitivity = 0; } From 1cfc12d45f2c1f12298d052c7e8f8a9d6e84d545 Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Mon, 4 Jan 2021 21:03:09 +1100 Subject: [PATCH 4/5] Update MOVThread.cpp --- workspace/TS100/Core/Threads/MOVThread.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp index 2705e72d73..8ec58a9268 100755 --- a/workspace/TS100/Core/Threads/MOVThread.cpp +++ b/workspace/TS100/Core/Threads/MOVThread.cpp @@ -82,6 +82,12 @@ inline void readAccelerometer(int16_t &tx, int16_t &ty, int16_t &tz, Orientation BMA223::getAxisReadings(tx, ty, tz); rotation = BMA223::getOrientation(); } else +#endif +#ifdef ACCEL_MSA + if (PCBVersion == 3) { + MSA301::getAxisReadings(tx, ty, tz); + rotation = MSA301::getOrientation(); + } else #endif { //do nothing :( From a5a7463178ae77608378f395768c7cb8b68eb26a Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Mon, 4 Jan 2021 21:37:04 +1100 Subject: [PATCH 5/5] Fix build for MSA --- workspace/TS100/Core/Drivers/MSA301.cpp | 3 +- workspace/TS100/Core/Threads/GUIThread.cpp | 7 +- workspace/TS100/Core/Threads/MOVThread.cpp | 183 ++++++++++----------- 3 files changed, 95 insertions(+), 98 deletions(-) diff --git a/workspace/TS100/Core/Drivers/MSA301.cpp b/workspace/TS100/Core/Drivers/MSA301.cpp index 34f879bce1..ceaf452276 100644 --- a/workspace/TS100/Core/Drivers/MSA301.cpp +++ b/workspace/TS100/Core/Drivers/MSA301.cpp @@ -34,8 +34,9 @@ Orientation MSA301::getOrientation() { return Orientation::ORIENTATION_LEFT_HAND; case 96: return Orientation::ORIENTATION_RIGHT_HAND; + default: + return Orientation::ORIENTATION_FLAT; } - return Orientation::ORIENTATION_FLAT; } void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) { diff --git a/workspace/TS100/Core/Threads/GUIThread.cpp b/workspace/TS100/Core/Threads/GUIThread.cpp index 644cd09fa1..373e73a132 100755 --- a/workspace/TS100/Core/Threads/GUIThread.cpp +++ b/workspace/TS100/Core/Threads/GUIThread.cpp @@ -394,8 +394,8 @@ static void display_countdown(int sleepThres) { * mode is triggered. */ int lastEventTime = lastButtonTime < lastMovementTime ? lastMovementTime : lastButtonTime; - int downCount = sleepThres - xTaskGetTickCount() + lastEventTime; - if (downCount > 99000) { + TickType_t downCount = sleepThres - xTaskGetTickCount() + lastEventTime; + if (downCount > (99 * TICKS_SECOND)) { OLED::printNumber(downCount / 60000 + 1, 2); OLED::print(SymbolMinutes); } else { @@ -645,10 +645,10 @@ static void gui_solderingMode(uint8_t jumpToSleep) { void showDebugMenu(void) { uint8_t screen = 0; ButtonState b; + OLED::setFont(1); // small font for (;;) { OLED::clearScreen(); // Ensure the buffer starts clean OLED::setCursor(0, 0); // Position the cursor at the 0,0 (top left) - OLED::setFont(1); // small font OLED::print(SymbolVersionNumber); // Print version number OLED::setCursor(0, 8); // second line OLED::print(DebugMenu[screen]); @@ -717,7 +717,6 @@ void showDebugMenu(void) { } #endif if (poweredbyPD) { - OLED::printNumber(2, 1); } else { diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp index f3405b28fa..d35ebd89d9 100644 --- a/workspace/TS100/Core/Threads/MOVThread.cpp +++ b/workspace/TS100/Core/Threads/MOVThread.cpp @@ -25,51 +25,49 @@ uint8_t accelInit = 0; TickType_t lastMovementTime = 0; void detectAccelerometerVersion() { - PCBVersion = 99; + DetectedAccelerometerVersion = 99; #ifdef ACCEL_MMA if (MMA8652FC::detect()) { - if (MMA8652FC::initalize()) { - PCBVersion = 1; + DetectedAccelerometerVersion = 1; } } else #endif #ifdef ACCEL_LIS - if (LIS2DH12::detect()) { - // Setup the ST Accelerometer - if (LIS2DH12::initalize()) { - PCBVersion = 2; - } - } else + if (LIS2DH12::detect()) { + // Setup the ST Accelerometer + if (LIS2DH12::initalize()) { + DetectedAccelerometerVersion = 2; + } + } else #endif #ifdef ACCEL_BMA if (BMA223::detect()) { // Setup the ST Accelerometer if (BMA223::initalize()) { - PCBVersion = 3; + DetectedAccelerometerVersion = 3; } } else #endif #ifdef ACCEL_MSA - if (MSA301::detect()) { - // Setup the MSA301 Accelerometer - if (MSA301::initalize()) { - PCBVersion = 4; - } - } else + if (MSA301::detect()) { + // Setup the MSA301 Accelerometer + if (MSA301::initalize()) { + DetectedAccelerometerVersion = 4; + } + } else #endif - { - // disable imu sensitivity - systemSettings.sensitivity = 0; - } + { + // disable imu sensitivity + systemSettings.sensitivity = 0; + } } -inline void readAccelerometer(int16_t &tx, int16_t &ty, int16_t &tz, - Orientation &rotation) { +inline void readAccelerometer(int16_t &tx, int16_t &ty, int16_t &tz, Orientation &rotation) { #ifdef ACCEL_LIS - if (DetectedAccelerometerVersion == 2) { - LIS2DH12::getAxisReadings(tx, ty, tz); - rotation = LIS2DH12::getOrientation(); - } else + if (DetectedAccelerometerVersion == 2) { + LIS2DH12::getAxisReadings(tx, ty, tz); + rotation = LIS2DH12::getOrientation(); + } else #endif #ifdef ACCEL_MMA if (DetectedAccelerometerVersion == 1) { @@ -84,78 +82,77 @@ inline void readAccelerometer(int16_t &tx, int16_t &ty, int16_t &tz, } else #endif #ifdef ACCEL_MSA - if (PCBVersion == 3) { - MSA301::getAxisReadings(tx, ty, tz); - rotation = MSA301::getOrientation(); - } else + if (DetectedAccelerometerVersion == 4) { + MSA301::getAxisReadings(tx, ty, tz); + rotation = MSA301::getOrientation(); + } else #endif - { - // do nothing :( - } + { + // do nothing :( + } } void startMOVTask(void const *argument __unused) { - postRToSInit(); - detectAccelerometerVersion(); - osDelay(50); // wait ~50ms for setup of accel to finalise - lastMovementTime = 0; - // Mask 2 seconds if we are in autostart so that if user is plugging in and - // then putting in stand it doesnt wake instantly - if (systemSettings.autoStartMode) - osDelay(2 * TICKS_SECOND); + postRToSInit(); + detectAccelerometerVersion(); + osDelay(TICKS_100MS / 2); // wait ~50ms for setup of accel to finalise + lastMovementTime = 0; + // Mask 2 seconds if we are in autostart so that if user is plugging in and + // then putting in stand it doesnt wake instantly + if (systemSettings.autoStartMode) + osDelay(2 * TICKS_SECOND); - int16_t datax[MOVFilter] = {0}; - int16_t datay[MOVFilter] = {0}; - int16_t dataz[MOVFilter] = {0}; - uint8_t currentPointer = 0; - int16_t tx = 0, ty = 0, tz = 0; - int32_t avgx, avgy, avgz; - if (systemSettings.sensitivity > 9) - systemSettings.sensitivity = 9; - Orientation rotation = ORIENTATION_FLAT; - for (;;) { - int32_t threshold = 1500 + (9 * 200); - threshold -= systemSettings.sensitivity * 200; // 200 is the step size - readAccelerometer(tx, ty, tz, rotation); - if (systemSettings.OrientationMode == 2) { - if (rotation != ORIENTATION_FLAT) { - OLED::setRotation(rotation == - ORIENTATION_LEFT_HAND); // link the data through - } - } - datax[currentPointer] = (int32_t)tx; - datay[currentPointer] = (int32_t)ty; - dataz[currentPointer] = (int32_t)tz; - if (!accelInit) { - for (uint8_t i = currentPointer + 1; i < MOVFilter; i++) { - datax[i] = (int32_t)tx; - datay[i] = (int32_t)ty; - dataz[i] = (int32_t)tz; - } - accelInit = 1; - } - currentPointer = (currentPointer + 1) % MOVFilter; - avgx = avgy = avgz = 0; - // calculate averages - for (uint8_t i = 0; i < MOVFilter; i++) { - avgx += datax[i]; - avgy += datay[i]; - avgz += dataz[i]; - } - avgx /= MOVFilter; - avgy /= MOVFilter; - avgz /= MOVFilter; + int16_t datax[MOVFilter] = { 0 }; + int16_t datay[MOVFilter] = { 0 }; + int16_t dataz[MOVFilter] = { 0 }; + uint8_t currentPointer = 0; + int16_t tx = 0, ty = 0, tz = 0; + int32_t avgx, avgy, avgz; + if (systemSettings.sensitivity > 9) + systemSettings.sensitivity = 9; + Orientation rotation = ORIENTATION_FLAT; + for (;;) { + int32_t threshold = 1500 + (9 * 200); + threshold -= systemSettings.sensitivity * 200; // 200 is the step size + readAccelerometer(tx, ty, tz, rotation); + if (systemSettings.OrientationMode == 2) { + if (rotation != ORIENTATION_FLAT) { + OLED::setRotation(rotation == ORIENTATION_LEFT_HAND); // link the data through + } + } + datax[currentPointer] = (int32_t) tx; + datay[currentPointer] = (int32_t) ty; + dataz[currentPointer] = (int32_t) tz; + if (!accelInit) { + for (uint8_t i = currentPointer + 1; i < MOVFilter; i++) { + datax[i] = (int32_t) tx; + datay[i] = (int32_t) ty; + dataz[i] = (int32_t) tz; + } + accelInit = 1; + } + currentPointer = (currentPointer + 1) % MOVFilter; + avgx = avgy = avgz = 0; + // calculate averages + for (uint8_t i = 0; i < MOVFilter; i++) { + avgx += datax[i]; + avgy += datay[i]; + avgz += dataz[i]; + } + avgx /= MOVFilter; + avgy /= MOVFilter; + avgz /= MOVFilter; - // Sum the deltas - int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz)); - // So now we have averages, we want to look if these are different by more - // than the threshold + // Sum the deltas + int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz)); + // So now we have averages, we want to look if these are different by more + // than the threshold - // If movement has occurred then we update the tick timer - if (error > threshold) { - lastMovementTime = xTaskGetTickCount(); - } + // If movement has occurred then we update the tick timer + if (error > threshold) { + lastMovementTime = xTaskGetTickCount(); + } - osDelay(100); // Slow down update rate - power_check(); - } + osDelay(TICKS_100MS); // Slow down update rate + power_check(); + } }