/*
* meas_tasks.c
*
* Created on: Sep 5, 2024
* Author: jakubski
*/
#include "meas_tasks.h"
#include "adc_buffers.h"
#include "measurements.h"
#include "node-red-config.h"
#include "peripherial.h"
#include "cmsis_os.h"
#include "main.h"
#ifdef PV_BOARD
#define VOLTAGES_COUNT 1
#define CURRENTS_COUNT 1
#else
#define VOLTAGES_COUNT 3
#define CURRENTS_COUNT 3
#endif
#define CIRC_BUFF_LEN 10
osThreadId_t adc1MeasTaskHandle = NULL;
osThreadId_t adc2MeasTaskHandle = NULL;
osThreadId_t adc3MeasTaskHandle = NULL;
osThreadId_t limiterSwitchTaskHandle = NULL;
osThreadId_t encoderTaskHandle = NULL;
osMessageQueueId_t adc1MeasDataQueue = NULL;
osMessageQueueId_t adc2MeasDataQueue = NULL;
osMessageQueueId_t adc3MeasDataQueue = NULL;
osMessageQueueId_t limiterSwitchDataQueue = NULL;
osMessageQueueId_t encoderDataQueue = NULL;
osMutexId_t vRefmVMutex;
osMutexId_t resMeasurementsMutex;
osMutexId_t sensorsInfoMutex;
osMutexId_t ILxRefMutex;
volatile uint32_t vRefmV = 3000;
RESMeasurements resMeasurements = { 0 };
SesnorsInfo sensorsInfo = { 0 };
uint16_t ILxRef[CURRENTS_COUNT] = { 0 };
extern TIM_HandleTypeDef htim3;
extern TIM_OC_InitTypeDef motorXYTimerConfigOC;
extern osTimerId_t motorXTimerHandle;
extern osTimerId_t motorYTimerHandle;
//extern osMutexId_t positionSettingMutex;
void MeasTasksInit (void) {
vRefmVMutex = osMutexNew (NULL);
resMeasurementsMutex = osMutexNew (NULL);
sensorsInfoMutex = osMutexNew (NULL);
ILxRefMutex = osMutexNew (NULL);
adc1MeasDataQueue = osMessageQueueNew (8, sizeof (ADC1_Data), NULL);
adc2MeasDataQueue = osMessageQueueNew (8, sizeof (ADC2_Data), NULL);
adc3MeasDataQueue = osMessageQueueNew (8, sizeof (ADC3_Data), NULL);
osThreadAttr_t osThreadAttradc1MeasTask = { 0 };
osThreadAttr_t osThreadAttradc2MeasTask = { 0 };
osThreadAttr_t osThreadAttradc3MeasTask = { 0 };
osThreadAttradc1MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
osThreadAttradc1MeasTask.priority = (osPriority_t)osPriorityRealtime;
osThreadAttradc2MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
osThreadAttradc2MeasTask.priority = (osPriority_t)osPriorityRealtime;
osThreadAttradc3MeasTask.stack_size = configMINIMAL_STACK_SIZE * 2;
osThreadAttradc3MeasTask.priority = (osPriority_t)osPriorityNormal;
adc1MeasTaskHandle = osThreadNew (ADC1MeasTask, NULL, &osThreadAttradc1MeasTask);
adc2MeasTaskHandle = osThreadNew (ADC2MeasTask, NULL, &osThreadAttradc2MeasTask);
adc3MeasTaskHandle = osThreadNew (ADC3MeasTask, NULL, &osThreadAttradc3MeasTask);
limiterSwitchDataQueue = osMessageQueueNew (8, sizeof (LimiterSwitchData), NULL);
osThreadAttr_t osThreadAttradc1LimiterSwitchTask = { 0 };
osThreadAttradc1LimiterSwitchTask.stack_size = configMINIMAL_STACK_SIZE * 2;
osThreadAttradc1LimiterSwitchTask.priority = (osPriority_t)osPriorityNormal;
limiterSwitchTaskHandle = osThreadNew (LimiterSwitchTask, NULL, &osThreadAttradc1LimiterSwitchTask);
encoderDataQueue = osMessageQueueNew (16, sizeof (EncoderData), NULL);
osThreadAttr_t osThreadAttrEncoderTask = { 0 };
osThreadAttrEncoderTask.stack_size = configMINIMAL_STACK_SIZE * 2;
osThreadAttrEncoderTask.priority = (osPriority_t)osPriorityNormal;
encoderTaskHandle = osThreadNew (EncoderTask, encoderDataQueue, &osThreadAttrEncoderTask);
}
void ADC1MeasTask (void* arg) {
float circBuffer[VOLTAGES_COUNT][CIRC_BUFF_LEN] = { 0 };
float rms[VOLTAGES_COUNT] = { 0 };
;
ADC1_Data adcData = { 0 };
uint32_t circBuffPos = 0;
float gainCorrection = 1.0;
while (pdTRUE) {
osMessageQueueGet (adc1MeasDataQueue, &adcData, 0, osWaitForever);
#ifdef GAIN_AUTO_CORRECTION
if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
gainCorrection = (float)vRefmV;
osMutexRelease (vRefmVMutex);
}
gainCorrection = gainCorrection / EXT_VREF_mV;
#endif
for (uint8_t i = 0; i < VOLTAGES_COUNT; i++) {
float val = adcData.adcDataBuffer[i] * deltaADC * U_CHANNEL_CONST * gainCorrection * U_MeasCorrectionData[i].gain + U_MeasCorrectionData[i].offset;
circBuffer[i][circBuffPos] = val;
rms[i] = 0.0;
for (uint8_t c = 0; c < CIRC_BUFF_LEN; c++) {
rms[i] += circBuffer[i][c];
}
rms[i] = rms[i] / CIRC_BUFF_LEN;
if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
if (fabs (resMeasurements.voltagePeak[i]) < fabs (val)) {
resMeasurements.voltagePeak[i] = val;
}
resMeasurements.voltageRMS[i] = rms[i];
resMeasurements.power[i] = resMeasurements.voltageRMS[i] * resMeasurements.currentRMS[i];
osMutexRelease (resMeasurementsMutex);
}
}
++circBuffPos;
circBuffPos = circBuffPos % CIRC_BUFF_LEN;
if (osMutexAcquire (ILxRefMutex, osWaitForever) == osOK) {
uint8_t refIdx = 0;
for (uint8_t i = (uint8_t)IL1Ref; i <= (uint8_t)IL3Ref; i++) {
ILxRef[refIdx++] = adcData.adcDataBuffer[i];
}
osMutexRelease (ILxRefMutex);
}
float fanFBVoltage = adcData.adcDataBuffer[FanFB] * deltaADC * -4.35 + 12;
if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
sensorsInfo.fanVoltage = fanFBVoltage;
osMutexRelease (sensorsInfoMutex);
}
}
}
void ADC2MeasTask (void* arg) {
float circBuffer[CURRENTS_COUNT][CIRC_BUFF_LEN] = { 0 };
float rms[CURRENTS_COUNT] = { 0 };
ADC2_Data adcData = { 0 };
uint32_t circBuffPos = 0;
float gainCorrection = 1.0;
while (pdTRUE) {
osMessageQueueGet (adc2MeasDataQueue, &adcData, 0, osWaitForever);
if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
gainCorrection = (float)vRefmV;
osMutexRelease (vRefmVMutex);
}
gainCorrection = gainCorrection / EXT_VREF_mV;
float ref[CURRENTS_COUNT] = { 0 };
if (osMutexAcquire (ILxRefMutex, osWaitForever) == osOK) {
for (uint8_t i = 0; i < CURRENTS_COUNT; i++) {
ref[i] = (float)ILxRef[i];
}
osMutexRelease (ILxRefMutex);
}
for (uint8_t i = 0; i < CURRENTS_COUNT; i++) {
float adcVal = (float)adcData.adcDataBuffer[i];
float val = (adcVal - ref[i]) * deltaADC * I_CHANNEL_CONST * gainCorrection * I_MeasCorrectionData[i].gain + I_MeasCorrectionData[i].offset;
circBuffer[i][circBuffPos] = val;
rms[i] = 0.0;
for (uint8_t c = 0; c < CIRC_BUFF_LEN; c++) {
rms[i] += circBuffer[i][c];
}
rms[i] = rms[i] / CIRC_BUFF_LEN;
if (osMutexAcquire (resMeasurementsMutex, osWaitForever) == osOK) {
if (resMeasurements.currentPeak[i] < val) {
resMeasurements.currentPeak[i] = val;
}
resMeasurements.currentRMS[i] = rms[i];
osMutexRelease (resMeasurementsMutex);
}
}
++circBuffPos;
circBuffPos = circBuffPos % CIRC_BUFF_LEN;
}
}
void ADC3MeasTask (void* arg) {
float motorXSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
float motorYSensCircBuffer[CIRC_BUFF_LEN] = { 0 };
float pvT1CircBuffer[CIRC_BUFF_LEN] = { 0 };
float pvT2CircBuffer[CIRC_BUFF_LEN] = { 0 };
uint32_t circBuffPos = 0;
ADC3_Data adcData = { 0 };
while (pdTRUE) {
osMessageQueueGet (adc3MeasDataQueue, &adcData, 0, osWaitForever);
uint32_t vRef = __LL_ADC_CALC_VREFANALOG_VOLTAGE (adcData.adcDataBuffer[VrefInt], LL_ADC_RESOLUTION_16B);
if (osMutexAcquire (vRefmVMutex, osWaitForever) == osOK) {
vRefmV = vRef;
osMutexRelease (vRefmVMutex);
}
float motorXCurrentSense = adcData.adcDataBuffer[motorXSense] * deltaADC * 10 / 8.33333;
float motorYCurrentSense = adcData.adcDataBuffer[motorYSense] * deltaADC * 10 / 8.33333;
motorXSensCircBuffer[circBuffPos] = motorXCurrentSense;
motorYSensCircBuffer[circBuffPos] = motorYCurrentSense;
pvT1CircBuffer[circBuffPos] = adcData.adcDataBuffer[pvTemp1] * deltaADC * 45.33333333 - 63;
pvT2CircBuffer[circBuffPos] = adcData.adcDataBuffer[pvTemp2] * deltaADC * 45.33333333 - 63;
float motorXAveCurrent = 0;
float motorYAveCurrent = 0;
float pvT1AveTemp = 0;
float pvT2AveTemp = 0;
for (uint8_t i = 0; i < CIRC_BUFF_LEN; i++) {
motorXAveCurrent += motorXSensCircBuffer[i];
motorYAveCurrent += motorYSensCircBuffer[i];
#ifdef PV_BOARD
pvT1AveTemp += pvT1CircBuffer[i];
pvT2AveTemp += pvT2CircBuffer[i];
#endif
}
motorXAveCurrent /= CIRC_BUFF_LEN;
motorYAveCurrent /= CIRC_BUFF_LEN;
pvT1AveTemp /= CIRC_BUFF_LEN;
pvT2AveTemp /= CIRC_BUFF_LEN;
if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
if (sensorsInfo.motorXStatus == 1) {
sensorsInfo.motorXAveCurrent = motorXAveCurrent;
if (sensorsInfo.motorXPeakCurrent < motorXCurrentSense) {
sensorsInfo.motorXPeakCurrent = motorXCurrentSense;
}
}
if (sensorsInfo.motorYStatus == 1) {
sensorsInfo.motorYAveCurrent = motorYAveCurrent;
if (sensorsInfo.motorYPeakCurrent < motorYCurrentSense) {
sensorsInfo.motorYPeakCurrent = motorYCurrentSense;
}
}
sensorsInfo.pvTemperature[0] = pvT1AveTemp;
sensorsInfo.pvTemperature[1] = pvT2AveTemp;
osMutexRelease (sensorsInfoMutex);
}
++circBuffPos;
circBuffPos = circBuffPos % CIRC_BUFF_LEN;
}
}
void LimiterSwitchTask (void* arg) {
LimiterSwitchData limiterSwitchData = { 0 };
limiterSwitchData.gpioPin = GPIO_PIN_8;
for (uint8_t i = 0; i < 6; i++) {
limiterSwitchData.pinState = HAL_GPIO_ReadPin (GPIOD, limiterSwitchData.gpioPin);
osMessageQueuePut (limiterSwitchDataQueue, &limiterSwitchData, 0, 0);
limiterSwitchData.gpioPin = limiterSwitchData.gpioPin << 1;
if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
sensorsInfo.positionXWeak = 1;
sensorsInfo.positionYWeak = 1;
osMutexRelease (sensorsInfoMutex);
}
}
while (pdTRUE) {
osMessageQueueGet (limiterSwitchDataQueue, &limiterSwitchData, 0, osWaitForever);
if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
switch (limiterSwitchData.gpioPin) {
case GPIO_PIN_8:
sensorsInfo.limitYSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitYSwitchCenter == 1)
{
sensorsInfo.currentYPosition = 50;
sensorsInfo.positionYWeak = 0;
}
break;
case GPIO_PIN_9:
sensorsInfo.limitYSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitYSwitchDown == 1)
{
sensorsInfo.currentYPosition = 0;
sensorsInfo.positionYWeak = 0;
}
break;
case GPIO_PIN_10:
sensorsInfo.limitXSwitchCenter = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitXSwitchCenter == 1)
{
sensorsInfo.currentXPosition = 50;
sensorsInfo.positionXWeak = 0;
}
break;
case GPIO_PIN_11:
sensorsInfo.limitYSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitYSwitchUp == 1)
{
sensorsInfo.currentYPosition = 100;
sensorsInfo.positionYWeak = 0;
}
break;
case GPIO_PIN_12:
sensorsInfo.limitXSwitchUp = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitXSwitchUp == 1)
{
sensorsInfo.currentXPosition = 100;
sensorsInfo.positionXWeak = 0;
}
break;
case GPIO_PIN_13:
sensorsInfo.limitXSwitchDown = limiterSwitchData.pinState == GPIO_PIN_SET ? 1 : 0;
if (sensorsInfo.limitXSwitchDown == 1)
{
sensorsInfo.currentXPosition = 0;
sensorsInfo.positionXWeak = 0;
}
break;
default: break;
}
if ((sensorsInfo.limitXSwitchDown == 1) || (sensorsInfo.limitXSwitchUp == 1)) {
sensorsInfo.motorXStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_1, TIM_CHANNEL_2, motorXTimerHandle, 0, 0, sensorsInfo.limitXSwitchUp, sensorsInfo.limitXSwitchDown);
}
if ((sensorsInfo.limitYSwitchDown == 1) || (sensorsInfo.limitYSwitchUp == 1)) {
sensorsInfo.motorYStatus = MotorControl (&htim3, &motorXYTimerConfigOC, TIM_CHANNEL_3, TIM_CHANNEL_4, motorYTimerHandle, 0, 0, sensorsInfo.limitYSwitchUp, sensorsInfo.limitYSwitchDown);
}
osMutexRelease (sensorsInfoMutex);
}
}
}
void EncoderTask (void* arg) {
EncoderData encoderData = { 0 };
osMessageQueueId_t encoderQueue = (osMessageQueueId_t)arg;
while (pdTRUE) {
osMessageQueueGet (encoderQueue, &encoderData, 0, osWaitForever);
if (osMutexAcquire (sensorsInfoMutex, osWaitForever) == osOK) {
if (encoderData.axe == encoderAxeX) {
if (encoderData.direction == encoderCW) {
sensorsInfo.pvEncoderX += 360.0 / ENCODER_X_IMP_PER_TURN;
} else {
sensorsInfo.pvEncoderX -= 360.0 / ENCODER_X_IMP_PER_TURN;
}
float currentPercentPos = 100 * sensorsInfo.pvEncoderX / MAX_X_AXE_ANGLE;
currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
DbgLEDToggle(DBG_LED2);
} else {
if (encoderData.direction == encoderCW) {
sensorsInfo.pvEncoderY += 360.0 / ENCODER_Y_IMP_PER_TURN;
} else {
sensorsInfo.pvEncoderY -= 360.0 / ENCODER_Y_IMP_PER_TURN;
}
float currentPercentPos = 100 * sensorsInfo.pvEncoderY / MAX_X_AXE_ANGLE;
currentPercentPos = currentPercentPos < 0 ? 0 : currentPercentPos;
sensorsInfo.currentXPosition = currentPercentPos > 100 ? 100 : currentPercentPos;
DbgLEDToggle(DBG_LED3);
}
osMutexRelease (sensorsInfoMutex);
}
}
}