Fix mixtake in I2C probe checkv2.10.1

4 files changed, 74 insertions, 85 deletions

diff --git a/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp b/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
index ad727723..785e3b25 100644
--- a/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/I2C_Wrapper.cpp
@@ -30,10 +30,10 @@ bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
 		// RToS is active, run threading

 		// Get the mutex so we can use the I2C port

 		// Wait up to 1 second for the mutex

-		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {

+		if (xSemaphoreTake(I2CSemaphore, (TickType_t)500) == pdTRUE) {

 #ifdef I2CUSESDMA

 			if (HAL_I2C_Mem_Read(&hi2c1, DevAddress, MemAddress,

-					I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {

+			I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {

 

 				I2C_Unstick();

 				xSemaphoreGive(I2CSemaphore);

@@ -78,22 +78,15 @@ void FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
 		// RToS is active, run threading

 		// Get the mutex so we can use the I2C port

 		// Wait up to 1 second for the mutex

-		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {

-#ifdef I2CUSESDMA

+		if (xSemaphoreTake(I2CSemaphore, (TickType_t)500) == pdTRUE) {

 			if (HAL_I2C_Mem_Write(&hi2c1, DevAddress, MemAddress,

-					I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {

+			I2C_MEMADD_SIZE_8BIT, pData, Size, 500) != HAL_OK) {

 

 				I2C_Unstick();

 				xSemaphoreGive(I2CSemaphore);

 			}

 			xSemaphoreGive(I2CSemaphore);

-#else

-			if (HAL_I2C_Mem_Write(&hi2c1, DevAddress, MemAddress, I2C_MEMADD_SIZE_8BIT, pData,

-							Size, 5000) != HAL_OK) {

-			}

-			xSemaphoreGive(I2CSemaphore);

-#endif

-		} else {

+

 		}

 	}

 

@@ -130,11 +123,9 @@ void FRToSI2C::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
 

 bool FRToSI2C::probe(uint16_t DevAddress) {

 	uint8_t buffer[1];

-	if (Mem_Read(DevAddress, 0,  buffer, 1)) {

-		//ACK'd

-		return true;

-	}

-	return false;

+	return HAL_I2C_Mem_Read(&hi2c1, DevAddress, 0x0F, I2C_MEMADD_SIZE_8BIT,

+			buffer, 1, 1000) == HAL_OK;

+

 }

 

 void FRToSI2C::I2C_Unstick() {

diff --git a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
index 0c262a95..deb78e5d 100644
--- a/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
+++ b/workspace/TS100/Core/BSP/Miniware/postRTOS.cpp
@@ -7,7 +7,8 @@
 #include "power.hpp"
 #include "stdlib.h"
 #include "task.h"
-
+#include "I2C_Wrapper.hpp"
 void postRToSInit() {
     // Any after RTos setup
+	FRToSI2C::FRToSInit();
 }
diff --git a/workspace/TS100/Core/Threads/GUIThread.cpp b/workspace/TS100/Core/Threads/GUIThread.cpp
index 668b60b7..13e94ea0 100644
--- a/workspace/TS100/Core/Threads/GUIThread.cpp
+++ b/workspace/TS100/Core/Threads/GUIThread.cpp
@@ -10,9 +10,8 @@ extern "C" {
 #include <MMA8652FC.hpp>
 #include <gui.hpp>
 #include <history.hpp>
-#include <main.hpp>
+#include "main.hpp"
 #include <power.hpp>
-
 #include "../../configuration.h"
 #include "Buttons.hpp"
 #include "LIS2DH12.hpp"
@@ -23,7 +22,7 @@ extern "C" {
 #include "stdlib.h"
 #include "string.h"
 #include "unit.h"
-extern uint8_t PCBVersion;
+
 // File local variables
 extern uint32_t currentTempTargetDegC;
 extern uint8_t accelInit;
@@ -597,7 +596,7 @@ void showDebugMenu(void) {
 
 /* StartGUITask function */
 void startGUITask(void const *argument __unused) {
-    FRToSI2C::FRToSInit();
+
     uint8_t tempWarningState = 0;
     bool buttonLockout = false;
     bool tempOnDisplay = false;
diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp
index e88cefce..5d236ba1 100644
--- a/workspace/TS100/Core/Threads/MOVThread.cpp
+++ b/workspace/TS100/Core/Threads/MOVThread.cpp
@@ -23,71 +23,69 @@
 uint8_t accelInit = 0;

 uint32_t lastMovementTime = 0;

 void startMOVTask(void const *argument __unused) {

-    OLED::setRotation(true);

-    postRToSInit();

-    power_probe();

-    while (pidTaskNotification == 0) osDelay(30);  // Wait for PID to start

+	OLED::setRotation(systemSettings.OrientationMode & 1);

+	postRToSInit();

+	power_probe();

+	lastMovementTime = 0;

+	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

 

-    OLED::setRotation(systemSettings.OrientationMode & 1);

-    lastMovementTime = 0;

-    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

+		if (PCBVersion == 2) {

+			LIS2DH12::getAxisReadings(tx, ty, tz);

+			rotation = LIS2DH12::getOrientation();

+		} else if (PCBVersion == 1) {

+			MMA8652FC::getAxisReadings(tx, ty, tz);

+			rotation = MMA8652FC::getOrientation();

+		}

+		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;

 

-        if (PCBVersion == 2) {

-            LIS2DH12::getAxisReadings(tx, ty, tz);

-            rotation = LIS2DH12::getOrientation();

-        } else if (PCBVersion == 1) {

-            MMA8652FC::getAxisReadings(tx, ty, tz);

-            rotation = MMA8652FC::getOrientation();

-        }

-        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 error has occurred then we update the tick timer

+		if (error > threshold) {

+			lastMovementTime = xTaskGetTickCount();

+		}

 

-        // If error has occurred then we update the tick timer

-        if (error > threshold) {

-            lastMovementTime = xTaskGetTickCount();

-        }

-

-        osDelay(100);  // Slow down update rate

-        power_check();

-    }

+		osDelay(100);  // Slow down update rate

+		power_check();

+	}

 }