From a13f501dfd219e6991603f0341912bda0550fa92 Mon Sep 17 00:00:00 2001 From: "Ben V. Brown" Date: Fri, 29 May 2020 21:54:09 +1000 Subject: [PATCH] More refactoring for the movement thread --- workspace/TS100/Core/BSP/BSP.h | 2 + workspace/TS100/Core/BSP/Miniware/BSP.cpp | 42 ++++++- workspace/TS100/Core/Inc/main.hpp | 4 +- workspace/TS100/Core/Src/FreeRTOSHooks.c | 8 ++ workspace/TS100/Core/Src/main.cpp | 131 +-------------------- workspace/TS100/Core/Threads/MOVThread.cpp | 109 +++++++++++++++++ 6 files changed, 164 insertions(+), 132 deletions(-) create mode 100644 workspace/TS100/Core/Threads/MOVThread.cpp diff --git a/workspace/TS100/Core/BSP/BSP.h b/workspace/TS100/Core/BSP/BSP.h index 2efaeeac..fea1ee46 100644 --- a/workspace/TS100/Core/BSP/BSP.h +++ b/workspace/TS100/Core/BSP/BSP.h @@ -38,6 +38,8 @@ uint8_t getButtonB(); // This should toggle the SCL line until SDA goes high to end the current transaction void unstick_I2C(); +//Reboot the IC when things go seriously wrong +void reboot(); #ifdef __cplusplus } #endif diff --git a/workspace/TS100/Core/BSP/Miniware/BSP.cpp b/workspace/TS100/Core/BSP/Miniware/BSP.cpp index 21eaa3cb..80abda39 100644 --- a/workspace/TS100/Core/BSP/Miniware/BSP.cpp +++ b/workspace/TS100/Core/BSP/Miniware/BSP.cpp @@ -4,11 +4,12 @@ #include "Setup.h" #include "history.hpp" #include "Pins.h" +#include "main.hpp" #include "history.hpp" +#include "FRToSI2C.hpp" volatile uint16_t PWMSafetyTimer = 0; volatile uint8_t pendingPWM = 0; - //2 second filter (ADC is PID_TIM_HZ Hz) history rawTempFilter = { { 0 }, 0, 0 }; void resetWatchdog() { @@ -210,3 +211,42 @@ uint8_t getButtonB() { return HAL_GPIO_ReadPin(KEY_B_GPIO_Port, KEY_B_Pin) == GPIO_PIN_RESET ? 1 : 0; } + +/* + * Catch the IRQ that says that the conversion is done on the temperature + * readings coming in Once these have come in we can unblock the PID so that it + * runs again + */ +void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc) { + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + if (hadc == &hadc1) { + if (pidTaskNotification) { + vTaskNotifyGiveFromISR(pidTaskNotification, + &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); + } + } +} +void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { + FRToSI2C::CpltCallback(); +} +void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { + FRToSI2C::CpltCallback(); +} +void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { + FRToSI2C::CpltCallback(); +} +void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c __unused) { + + FRToSI2C::CpltCallback(); +} +void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c __unused) { + + FRToSI2C::CpltCallback(); +} +void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { + FRToSI2C::CpltCallback(); +} +void reboot() { + NVIC_SystemReset(); +} diff --git a/workspace/TS100/Core/Inc/main.hpp b/workspace/TS100/Core/Inc/main.hpp index 6258bede..37bc9816 100644 --- a/workspace/TS100/Core/Inc/main.hpp +++ b/workspace/TS100/Core/Inc/main.hpp @@ -25,7 +25,9 @@ void vApplicationStackOverflowHook(xTaskHandle *pxTask, void startGUITask(void const *argument); void startPIDTask(void const *argument); void startMOVTask(void const *argument); -extern TaskHandle_t pidTaskNotification ; +extern TaskHandle_t pidTaskNotification; +extern uint8_t accelInit; +extern uint32_t lastMovementTime; #ifdef __cplusplus } #endif diff --git a/workspace/TS100/Core/Src/FreeRTOSHooks.c b/workspace/TS100/Core/Src/FreeRTOSHooks.c index d5a15dd7..1e9c3dc6 100644 --- a/workspace/TS100/Core/Src/FreeRTOSHooks.c +++ b/workspace/TS100/Core/Src/FreeRTOSHooks.c @@ -22,3 +22,11 @@ void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer, *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE; /* place for user code */ } + +void vApplicationStackOverflowHook(xTaskHandle *pxTask, + signed portCHAR *pcTaskName) { + (void) pxTask; + (void) pcTaskName; +// We dont have a good way to handle a stack overflow at this point in time + reboot(); +} diff --git a/workspace/TS100/Core/Src/main.cpp b/workspace/TS100/Core/Src/main.cpp index 2694a4a8..ed12e0bf 100644 --- a/workspace/TS100/Core/Src/main.cpp +++ b/workspace/TS100/Core/Src/main.cpp @@ -2,10 +2,10 @@ #include "BSP.h" -#include #include #include #include "LIS2DH12.hpp" +#include #include #include #include "Settings.h" @@ -18,8 +18,6 @@ uint8_t PCBVersion = 0; // File local variables uint32_t currentTempTargetDegC = 0; // Current temperature target in C -uint8_t accelInit = 0; -uint32_t lastMovementTime = 0; bool settingsWereReset = false; // FreeRTOS variables @@ -94,92 +92,6 @@ int main(void) { } } -#define MOVFilter 8 -void startMOVTask(void const *argument __unused) { - OLED::setRotation(true); - -#ifdef MODEL_TS80 - startQC(systemSettings.voltageDiv); - while (pidTaskNotification == 0) - osDelay(30); // To ensure we return after idealQCVoltage/tip resistance - - seekQC((systemSettings.cutoutSetting) ? 120 : 90, - systemSettings.voltageDiv); // this will move the QC output to the preferred voltage to start with - -#else - osDelay(250); // wait for accelerometer to stabilize -#endif - - 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 = 0, avgy = 0, avgz = 0; - if (systemSettings.sensitivity > 9) - systemSettings.sensitivity = 9; -#ifdef ACCELDEBUG - uint32_t max = 0; -#endif - 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; - - // 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(); - } - - osDelay(100); // Slow down update rate -#ifdef MODEL_TS80 - seekQC((systemSettings.cutoutSetting) ? 120 : 90, - systemSettings.voltageDiv); // Run the QC seek again if we have drifted too much -#endif - } -} // Second last page of flash set aside for logo image. #define FLASH_LOGOADDR (0x8000000 | 0xF800) @@ -199,44 +111,3 @@ bool showBootLogoIfavailable() { return true; } -/* - * Catch the IRQ that says that the conversion is done on the temperature - * readings coming in Once these have come in we can unblock the PID so that it - * runs again - */ -void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc) { - BaseType_t xHigherPriorityTaskWoken = pdFALSE; - if (hadc == &hadc1) { - if (pidTaskNotification) { - vTaskNotifyGiveFromISR(pidTaskNotification, - &xHigherPriorityTaskWoken); - portYIELD_FROM_ISR(xHigherPriorityTaskWoken); - } - } -} -void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { - FRToSI2C::CpltCallback(); -} -void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { - FRToSI2C::CpltCallback(); -} -void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { - FRToSI2C::CpltCallback(); -} -void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c __unused) { - - FRToSI2C::CpltCallback(); -} -void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c __unused) { - - FRToSI2C::CpltCallback(); -} -void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c __unused) { - FRToSI2C::CpltCallback(); -} -void vApplicationStackOverflowHook(xTaskHandle *pxTask __unused, - signed portCHAR *pcTaskName __unused) { - -// We dont have a good way to handle a stack overflow at this point in time - NVIC_SystemReset(); -} diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp new file mode 100644 index 00000000..70c0727b --- /dev/null +++ b/workspace/TS100/Core/Threads/MOVThread.cpp @@ -0,0 +1,109 @@ +/* + * MOVThread.cpp + * + * Created on: 29 May 2020 + * Author: Ralim + */ + +#include "main.hpp" +#include "BSP.h" +#include "power.hpp" +#include "history.hpp" +#include "TipThermoModel.h" +#include "cmsis_os.h" +#include "FreeRTOS.h" +#include "task.h" +#include "Settings.h" +#include "FRToSI2C.hpp" +#include "stdlib.h" +#include "LIS2DH12.hpp" +#include + +#define MOVFilter 8 +uint8_t accelInit = 0; +uint32_t lastMovementTime = 0; +void startMOVTask(void const *argument __unused) { + OLED::setRotation(true); + +#ifdef MODEL_TS80 + startQC(systemSettings.voltageDiv); + while (pidTaskNotification == 0) + osDelay(30); // To ensure we return after idealQCVoltage/tip resistance + + seekQC((systemSettings.cutoutSetting) ? 120 : 90, + systemSettings.voltageDiv); // this will move the QC output to the preferred voltage to start with + +#else + osDelay(250); // wait for accelerometer to stabilize +#endif + + 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 = 0, avgy = 0, avgz = 0; + if (systemSettings.sensitivity > 9) + systemSettings.sensitivity = 9; +#ifdef ACCELDEBUG + uint32_t max = 0; +#endif + 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; + + // 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(); + } + + osDelay(100); // Slow down update rate +#ifdef MODEL_TS80 + seekQC((systemSettings.cutoutSetting) ? 120 : 90, + systemSettings.voltageDiv); // Run the QC seek again if we have drifted too much +#endif + } +}