Putting guards on I2C

3 files changed, 60 insertions, 45 deletions

diff --git a/workspace/TS100/src/MMA8652FC.cpp b/workspace/TS100/src/MMA8652FC.cpp
index 9b6b992a..5b31de14 100644
--- a/workspace/TS100/src/MMA8652FC.cpp
+++ b/workspace/TS100/src/MMA8652FC.cpp
@@ -6,6 +6,7 @@
  */
 
 #include <MMA8652FC.hpp>
+#include "cmsis_os.h"
 
 MMA8652FC::MMA8652FC(I2C_HandleTypeDef* i2cHandle) {
 	i2c = i2cHandle;
@@ -54,8 +55,10 @@ void MMA8652FC::setSensitivity(uint8_t threshold, uint8_t filterTime) {
 }
 
 bool MMA8652FC::getOrientation() {
-	//First read the PL_STATUS register
+	//First read the PL_STATUS registertaskENTER_CRITICAL();
+	taskENTER_CRITICAL();
 	uint8_t plStatus = I2C_RegisterRead(PL_STATUS_REG);
+	taskEXIT_CRITICAL();
 	plStatus >>= 1;    //We don't need the up/down bit
 	plStatus &= 0x03;    //mask to the two lower bits
 	//0 == left handed
@@ -64,23 +67,14 @@ bool MMA8652FC::getOrientation() {
 	return !plStatus;
 }
 void MMA8652FC::getAxisReadings(int16_t *x, int16_t *y, int16_t *z) {
-	uint8_t temp = 0;
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*x) = temp;
-	(*x) <<= 8;
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_X_LSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*x) |= temp;
-
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_Y_MSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*y) = temp;
-	(*y) <<= 8;
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_Y_LSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*y) |= temp;
-
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_Z_MSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*z) = temp;
-	(*z) <<= 8;
-	HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_Z_LSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) &temp, 1, 0xFFFF);
-	(*z) |= temp;
-
+	uint8_t tempArr[6];
+	taskENTER_CRITICAL();
+	while (HAL_I2C_Mem_Read(i2c, MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, I2C_MEMADD_SIZE_8BIT, (uint8_t*) tempArr, 6,
+			0xFFFF) != HAL_OK) {
+		HAL_Delay(5);
+	}
+	taskEXIT_CRITICAL();
+	(*x) = tempArr[0] << 8 | tempArr[1];
+	(*y) = tempArr[2] << 8 | tempArr[3];
+	(*z) = tempArr[4] << 8 | tempArr[5];
 }
diff --git a/workspace/TS100/src/OLED.cpp b/workspace/TS100/src/OLED.cpp
index bb194fde..147b4031 100644
--- a/workspace/TS100/src/OLED.cpp
+++ b/workspace/TS100/src/OLED.cpp
@@ -8,6 +8,7 @@
 #include <OLED.hpp>
 #include <string.h>
 #include "Translation.h"
+#include "cmsis_os.h"
 /*Setup params for the OLED screen*/
 /*http://www.displayfuture.com/Display/datasheet/controller/SSD1307.pdf*/
 /*All commands are prefixed with 0x80*/
@@ -84,8 +85,10 @@ void OLED::refresh() {
 	screenBuffer[11] = 0x01;
 
 	screenBuffer[12] = 0x40;    //start of data marker
+	taskENTER_CRITICAL();
 
 	HAL_I2C_Master_Transmit(i2c, DEVICEADDR_OLED, screenBuffer, 12 + 96 * 2 + 1, 0xFFFF);
+	taskEXIT_CRITICAL();
 
 }
 
@@ -136,7 +139,10 @@ void OLED::displayOnOff(bool on) {
 			data[3] = 0x10;
 			data[5] = 0xAE;
 		}
+		taskENTER_CRITICAL();
+
 		HAL_I2C_Master_Transmit(i2c, DEVICEADDR_OLED, data, 6, 0xFFFF);
+		taskEXIT_CRITICAL();
 		displayOnOffState = on;
 	}
 }
@@ -151,7 +157,10 @@ void OLED::setRotation(bool leftHanded) {
 			OLED_Setup_Array[11] = 0xC0;
 			OLED_Setup_Array[19] = 0xA0;
 		}
+		taskENTER_CRITICAL();
+
 		HAL_I2C_Master_Transmit(i2c, DEVICEADDR_OLED, (uint8_t*) OLED_Setup_Array, 50, 0xFFFF);
+		taskEXIT_CRITICAL();
 		inLeftHandedMode = leftHanded;
 	}
 }
diff --git a/workspace/TS100/src/main.cpp b/workspace/TS100/src/main.cpp
index 3add4af9..fc41badc 100644
--- a/workspace/TS100/src/main.cpp
+++ b/workspace/TS100/src/main.cpp
@@ -9,6 +9,7 @@
 #include "string.h"
 #include "gui.h"
 #include "stdlib.h"
+
 //C++ objects
 OLED lcd(&hi2c1);
 MMA8652FC accel(&hi2c1);
@@ -72,7 +73,7 @@ int main(void) {
 	}
 }
 void GUIDelay() {
-	osDelay(50);
+	osDelay(60);
 }
 ButtonState getButtonState() {
 	/*
@@ -620,7 +621,7 @@ static void gui_solderingMode() {
 	}
 
 }
-
+#define ACCELDEBUG 0
 /* StartGUITask function */
 void startGUITask(void const * argument) {
 	/*
@@ -654,12 +655,13 @@ void startGUITask(void const * argument) {
 	if (showBootLogoIfavailable())
 		waitForButtonPressOrTimeout(1000);
 	HAL_IWDG_Refresh(&hiwdg);
-	/*
-	 for (;;) {
-	 HAL_IWDG_Refresh(&hiwdg);
-	 osDelay(100);
-	 }
-	 */
+#if ACCELDEBUG
+
+	for (;;) {
+		HAL_IWDG_Refresh(&hiwdg);
+		osDelay(100);
+	}
+#endif
 //^ Kept here for a way to block this thread
 	for (;;) {
 		ButtonState buttons = getButtonState();
@@ -839,27 +841,32 @@ void startPIDTask(void const * argument) {
 		osDelay(100);    // 10 Hz temp loop
 	}
 }
-#define MOVFilter 4
+#define MOVFilter 8
 void startMOVTask(void const * argument) {
 	osDelay(4000);    //wait for accel to stabilize
 	int16_t datax[MOVFilter];
 	int16_t datay[MOVFilter];
 	int16_t dataz[MOVFilter];
 	uint8_t currentPointer = 0;
-	memset(datax, 0, MOVFilter);
-	memset(datay, 0, MOVFilter);
-	memset(dataz, 0, MOVFilter);
+	memset(datax, 0, MOVFilter * sizeof(int16_t));
+	memset(datay, 0, MOVFilter * sizeof(int16_t));
+	memset(dataz, 0, MOVFilter * sizeof(int16_t));
 	int16_t tx, ty, tz;
 	int32_t avgx, avgy, avgz;
+	if (systemSettings.sensitivity > 9)
+		systemSettings.sensitivity = 9;
+#if ACCELDEBUG
+	uint32_t max = 0;
+#endif
 
 	for (;;) {
-		int32_t threshold = 600 + (9 * 120);
-		threshold -= systemSettings.sensitivity * 120;
+		int32_t threshold = 800 + (9 * 200);
+		threshold -= systemSettings.sensitivity * 200;    // 200 is the step size
 		accel.getAxisReadings(&tx, &ty, &tz);
 
-		datax[currentPointer] = tx;
-		datay[currentPointer] = ty;
-		dataz[currentPointer] = tz;
+		datax[currentPointer] = (int32_t) tx;
+		datay[currentPointer] = (int32_t) ty;
+		dataz[currentPointer] = (int32_t) tz;
 		currentPointer = (currentPointer + 1) % MOVFilter;
 #if ACCELDEBUG
 		//Debug for Accel
@@ -874,14 +881,20 @@ void startMOVTask(void const * argument) {
 		avgy /= MOVFilter;
 		avgz /= MOVFilter;
 		lcd.setFont(1);
-		lcd.setCursor(0,0);
-		lcd.printNumber(abs(avgx - tx), 5);
+		lcd.setCursor(0, 0);
+		lcd.printNumber(abs(avgx - (int32_t) tx), 5);
 		lcd.print(" ");
-		lcd.printNumber(abs(avgy - ty), 5);
+		lcd.printNumber(abs(avgy - (int32_t) ty), 5);
+		if ((abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz)) > max)
+			max = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz));
 		lcd.setCursor(0, 8);
-		lcd.printNumber(abs(avgz - tz), 5);
-		lcd.refresh();
+		lcd.printNumber(max, 5);
+		lcd.print(" ");
 
+		lcd.printNumber((abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz)), 5);
+		lcd.refresh();
+		if (HAL_GPIO_ReadPin(KEY_A_GPIO_Port, KEY_A_Pin) == GPIO_PIN_RESET)
+			max = 0;
 #endif
 		//Only run the actual processing if the sensitivity is set (aka we are enabled)
 		if (systemSettings.sensitivity) {
@@ -898,14 +911,13 @@ void startMOVTask(void const * argument) {
 
 			//So now we have averages, we want to look if these are different by more than the threshold
 			int32_t error = (abs(avgx - tx) + abs(avgy - ty) + abs(avgz - tz));
-
+			//If error has occured then we update the tick timer
 			if (error > threshold) {
 				lastMovementTime = HAL_GetTick();
-
 			}
 		}
 
-		osDelay(20);    //Slow down update rate
+		osDelay(100);    //Slow down update rate
 
 	}
 }