From 129f76f19b1dbd23933b9f68a457942089f0284c Mon Sep 17 00:00:00 2001 From: mmi Date: Wed, 11 Oct 2017 07:19:14 +0000 Subject: [PATCH] Updated HW test items for SWo git-svn-id: https://svn.vbchaos.nl/svn/hsb/trunk@248 05563f52-14a8-4384-a975-3d1654cca0fa --- .../3 - Implementation/0 - Code/HAL/Makefile | 1 + .../3 - Implementation/0 - Code/HAL/inc/PID.h | 10 +- .../3 - Implementation/0 - Code/HAL/src/PID.c | 61 +++++++-- .../0 - Code/Platform/inc/platform.h | 2 + .../0 - Code/Platform/src/keypadMatrix.c | 6 +- .../0 - Code/Platform/src/oli_stm32_h107.c | 4 + .../0 - Code/hsb-mrts/inc/repairProcess.h | 5 + .../0 - Code/hsb-mrts/src/main.c | 7 +- .../0 - Code/hsb-mrts/src/repairMenu.c | 19 +-- .../0 - Code/hsb-mrts/src/repairProcess.c | 124 +++++++++++++----- 10 files changed, 179 insertions(+), 60 deletions(-) diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/Makefile b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/Makefile index 1e832f7..262f52c 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/Makefile +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/Makefile @@ -29,6 +29,7 @@ KeyboardDevice.o \ Logger.o \ MAX5715.o \ nhd0420.o \ +PID.o \ storm700.o diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/inc/PID.h b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/inc/PID.h index 3fa058c..b5b6419 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/inc/PID.h +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/inc/PID.h @@ -39,7 +39,7 @@ // Constant and macro definitions // ----------------------------------------------------------------------------- - +#define PID_FIXED_POINT_FACTOR (1000) // ----------------------------------------------------------------------------- // Type definitions. @@ -47,10 +47,13 @@ struct Pid { + int iTerm; int Kp; // proportional constant int Ki; // integration constant int Kd; // differential constant int input_d1; // Input t-1 + int iMin; + int iMax; bool initialized; }; @@ -73,7 +76,7 @@ struct Pid * @todo * ----------------------------------------------------------------------------- */ -extern ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd); +extern ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd, int iMin, int iMax); /** ---------------------------------------------------------------------------- @@ -81,6 +84,7 @@ extern ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd); * Calculate * * @param self The PID object + * @param input The input * @param error the error input to calculate * * @return int calculated value @@ -88,6 +92,6 @@ extern ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd); * @todo * ----------------------------------------------------------------------------- */ -extern int PID_calculate(struct Pid* self, int error); +extern int PID_calculate(struct Pid* self, int input, int error); #endif /* INC_PID_H_ */ diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/src/PID.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/src/PID.c index fec88a7..ac1d0e7 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/src/PID.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/HAL/src/PID.c @@ -27,6 +27,8 @@ #include "PID.h" +#include "Logger.h" + // ----------------------------------------------------------------------------- // Constant and macro definitions // ----------------------------------------------------------------------------- @@ -55,17 +57,29 @@ // ----------------------------------------------------------------------------- -ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd) +ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd, int iMin, int iMax) { ErrorStatus returnValue = SUCCESS; if (!self->initialized) { - self->Kd = Kd; - self->Ki = Ki; - self->Kp = Kp; - self->input_d1 = 0; - self->initialized = true; + + if ((Kp >= 0) && (Ki >= 0) && (Kd >= 0)) + { + self->iTerm = 0; + self->Kd = Kd; + self->Ki = Ki; + self->Kp = Kp; + self->input_d1 = 0; + self->iMin = iMin; + self->iMax = iMax; + + self->initialized = true; + } + else + { + returnValue = ERROR; + } } else { @@ -75,18 +89,45 @@ ErrorStatus PID_construct(struct Pid* self, int Kp, int Ki, int Kd) } -int PID_calculate(struct Pid* self, int error) +int PID_calculate(struct Pid* self, int input, int error) { int returnValue = 0; + int dTerm; + int pTerm; + + input *= PID_FIXED_POINT_FACTOR; + error *= PID_FIXED_POINT_FACTOR; + if (self->initialized) { + // Calculate integral + self->iTerm += (self->Ki * error); + // Control integrator + if (self->iTerm > self->iMax) + { + self->iTerm = self->iMax; + } + else if(self->iTerm < self->iMin) + { + self->iTerm = self->iMin; + } + // Calculate differential + dTerm = (input - self->input_d1) * self->Kd; + + // Calculate proportional + pTerm = self->Kp * error; + + + returnValue = (self->iTerm + dTerm + pTerm) / PID_FIXED_POINT_FACTOR; + self->input_d1 = input; + } + else + { + returnValue = 0; } return returnValue; - - - } diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/inc/platform.h b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/inc/platform.h index 4978c5c..99106c9 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/inc/platform.h +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/inc/platform.h @@ -83,6 +83,8 @@ extern struct Keypad* const keypad; extern struct Gpio* const ledGreen; extern struct Gpio* const ledOrange; extern struct Gpio* const power6v5Enable; +extern struct Gpio* const tesla1; +extern struct Gpio* const tesla2; extern struct Gpio* const interlock1; extern struct Gpio* const interlock2; extern struct Gpio* const solenoid; diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/keypadMatrix.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/keypadMatrix.c index b2a807c..98f365d 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/keypadMatrix.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/keypadMatrix.c @@ -157,11 +157,15 @@ static ErrorStatus read(const struct IODevice* self, char* buffer, size_t length { struct KeypadQueueItem rxQueueItem; - if (xQueueReceive(keypad->rxQueue, &rxQueueItem, portMAX_DELAY) == pdTRUE) + if (xQueueReceive(keypad->rxQueue, &rxQueueItem, 0) == pdTRUE) { *actualLength = sizeof(struct KeypadQueueItem) / sizeof (char); memcpy(buffer, &rxQueueItem, sizeof(struct KeypadQueueItem) / sizeof (char)); } + else + { + returnValue = ERROR; + } } else { diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/oli_stm32_h107.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/oli_stm32_h107.c index 8c6f59b..a0005ac 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/oli_stm32_h107.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/Platform/src/oli_stm32_h107.c @@ -124,6 +124,8 @@ static struct Gpio _ledOrange = {.initialized = false}; static struct Gpio _power6v5Enable = {.initialized = false}; static struct Gpio _interlock1 = {.initialized = false}; static struct Gpio _interlock2 = {.initialized = false}; +static struct Gpio _tesla1 = {.initialized = false}; +static struct Gpio _tesla2 = {.initialized = false}; static struct Gpio _solenoid = {.initialized = false}; static struct Gpio _mcp0Relay = {.initialized = false}; static struct Gpio _mcp1Relay = {.initialized = false}; @@ -167,6 +169,8 @@ struct Gpio* const ledOrange = &_ledOrange; struct Gpio* const power6v5Enable = & _power6v5Enable; struct Gpio* const interlock1 = &_interlock1; struct Gpio* const interlock2 = &_interlock2; +struct Gpio* const tesla1 = &_tesla1; +struct Gpio* const tesla2 = &_tesla2; struct Gpio* const solenoid = & _solenoid; struct Gpio* const mcp0Relay = &_mcp0Relay; struct Gpio* const mcp1Relay = &_mcp1Relay; diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/inc/repairProcess.h b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/inc/repairProcess.h index f0c3411..9a53a37 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/inc/repairProcess.h +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/inc/repairProcess.h @@ -40,6 +40,8 @@ #include "repairPreset.h" +#include "PID.h" + // ----------------------------------------------------------------------------- // Constant and macro definitions // ----------------------------------------------------------------------------- @@ -88,6 +90,9 @@ struct RepairProcess const struct MAX5715_DAC* dacRow1; const struct MAX5715_DAC* dacRow2; const struct MAX5715_DAC* dacRow3; + struct Pid pidRow1; + struct Pid pidRow2; + struct Pid pidRow3; }; // ----------------------------------------------------------------------------- diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/main.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/main.c index 7db3edf..45ab8ef 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/main.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/main.c @@ -232,7 +232,7 @@ static void initTask(void* parameters) MAX5715Channel_construct(&dac->dac[1], dac, 1); MAX5715Channel_construct(&dac->dac[2], dac, 2); - xTaskCreate(printSystemInfoTask, (const char* const)"SysInfoTask", 512, NULL, 0, &sysTaskHandle); +// xTaskCreate(printSystemInfoTask, (const char* const)"SysInfoTask", 512, NULL, 0, &sysTaskHandle); // Let start screen stay for 5 seconds vTaskDelay(INIT_START_SCREEN_DELAY); @@ -244,6 +244,8 @@ static void initTask(void* parameters) hwTestItems.power6v5Enable = power6v5Enable; hwTestItems.interlock1 = interlock1; hwTestItems.interlock2 = interlock2; + hwTestItems.tesla1 = tesla1; + hwTestItems.tesla1 = tesla2; hwTestItems.solenoid = solenoid; hwTestItems.mcp0Relay = mcp0Relay; hwTestItems.mcp1Relay = mcp1Relay; @@ -252,7 +254,8 @@ static void initTask(void* parameters) hwTestItems.cat1Relay = cat1Relay; hwTestItems.cat2Relay = cat2Relay; hwTestItems.teslaLock = teslaLock; - hwTestItems.pcba = pcba; + hwTestItems.pcba = pcba; + hwTestItems.keypad = keypad; // EEPROM TO BE DONE HwValidationMenu_construct(hwValidation, &uart1->device, &hwTestItems, 1, 512); diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairMenu.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairMenu.c index 4b9904b..c6e9fd4 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairMenu.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairMenu.c @@ -144,18 +144,6 @@ static void repairMenu_task(void* parameters) Display_clearLine(self->display, 3); - char key; - Keypad_KeyState keyState; - if (Storm700_readKey(storm700, &key, &keyState) == SUCCESS) - { - LOGGER_DEBUG(mainLog, "Got key %c %d", key, keyState); - } - else - { - LOGGER_ERROR(mainLog, "Getting key failed"); - } - - struct RepairProcessParameters rpParameters; rpParameters.adcRow1 = &adc1->channel[0]; rpParameters.adcRow2 = &adc1->channel[1]; @@ -164,8 +152,11 @@ static void repairMenu_task(void* parameters) rpParameters.dacRow2 = &dac->dac[1]; rpParameters.dacRow3 = &dac->dac[2]; - struct RepairPresetParameters presetStage1 = {.voltage = 0xA00, .duration = 300, .softstartDuration = 120}; - struct RepairPresetParameters presetStage2 = {.voltage = 0xE00, .duration = 120, .softstartDuration = 30}; +// struct RepairPresetParameters presetStage1 = {.voltage = 0xE00, .duration = 300, .softstartDuration = 120}; +// struct RepairPresetParameters presetStage2 = {.voltage = 0x600, .duration = 120, .softstartDuration = 30}; + + struct RepairPresetParameters presetStage1 = {.voltage = 0xE00, .duration = 300, .softstartDuration = 120}; + struct RepairPresetParameters presetStage2 = {.voltage = 0x600, .duration = 120, .softstartDuration = 30}; struct RepairPreset repairPreset; repairPreset.numberOfStages = 2; diff --git a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairProcess.c b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairProcess.c index 373afa9..329bb38 100644 --- a/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairProcess.c +++ b/S - Software/0 - HSB MRTS Kathode-MCP/3 - Implementation/0 - Code/hsb-mrts/src/repairProcess.c @@ -33,6 +33,7 @@ #include "repairProcess.h" #include "repairPreset.h" +#include "internalADC.h" #include "Logger.h" #include "MAX5715.h" @@ -54,16 +55,13 @@ // ----------------------------------------------------------------------------- - // ----------------------------------------------------------------------------- // Function declarations // ----------------------------------------------------------------------------- static void repairProcess_task(void* parameters); -static void SignalProfileGenerator(); - - +static int SignalProfileGenerator(uint32_t startTime, uint32_t currentTime, struct RepairPreset* repairPreset, int presetIndex); // ----------------------------------------------------------------------------- // Function definitions @@ -100,6 +98,15 @@ ErrorStatus repairProcess_construct(struct RepairProcess* self, struct RepairPro self->dacRow2 = parameters->dacRow2; self->dacRow3 = parameters->dacRow3; + self->pidRow1.initialized = false; + self->pidRow2.initialized = false; + self->pidRow3.initialized = false; + + + PID_construct(&self->pidRow1, 10, 10, 10, 0, 10000000); + PID_construct(&self->pidRow2, 100, 0, 0, 0, 10000000); + PID_construct(&self->pidRow3, 10, 10, 10, 0, 10000000); + LOGGER_INFO(mainLog, "Repair Process task started"); } else @@ -159,14 +166,25 @@ static void repairProcess_task(void* parameters) struct RepairProcess* self = (struct RepairProcess*)parameters; uint8_t presetIndex = 0; + uint32_t startTime = 0; uint32_t softStartTimer = 0; uint32_t voltageHoldTimer = 0; - uint16_t voltageTarget = 0; + uint32_t pidError; + uint16_t voltageRow1 = 0; uint16_t voltageRow2 = 0; uint16_t voltageRow3 = 0; + uint16_t adcVoltageRow1 = 0; + uint16_t adcVoltageRow2 = 0; + uint16_t adcVoltageRow3 = 0; + + uint16_t voltageTarget = 0; + uint16_t dacVoltageRow1 = 0; + uint16_t dacVoltageRow2 = 0; + uint16_t dacVoltageRow3 = 0; + // Reset the seconds counter to 0 self->secondsCounter = 0; @@ -183,6 +201,7 @@ static void repairProcess_task(void* parameters) LOGGER_DEBUG(mainLog, "Repair Process: Preparing new stage of repair process"); // Prepare a new repair process //Load the timers + startTime = self->secondsCounter; softStartTimer = self->secondsCounter + self->repairPreset->preset[presetIndex].softstartDuration; LOGGER_DEBUG(mainLog, "Softstart timer is %d (%d + %d)", softStartTimer, self->secondsCounter, self->repairPreset->preset[presetIndex].softstartDuration); voltageHoldTimer = self->secondsCounter + self->repairPreset->preset[presetIndex].duration; @@ -211,24 +230,29 @@ static void repairProcess_task(void* parameters) { // Perform softstart / ramp-up -// if (PCBA_getInstance()->pcba == Tesla) -// { -// // Tesla repair only runs ADC row2 and DAC row2 -// calculateSoftStartStep(self->secondsCounter, softStartTimer, voltageTarget, &voltageRow2); -// LOGGER_DEBUG(mainLog, "Softstart running -> new target is %x", voltageRow2); -// MAX5715Channel_setValue(self->dacRow2, voltageRow2); -// } -// else if ((PCBA_getInstance()->pcba == Anode) || (PCBA_getInstance()->pcba == CathodeMCP)) -// { -// calculateSoftStartStep(self->secondsCounter, softStartTimer, voltageTarget, &voltageRow1); -// calculateSoftStartStep(self->secondsCounter, softStartTimer, voltageTarget, &voltageRow2); -// calculateSoftStartStep(self->secondsCounter, softStartTimer, voltageTarget, &voltageRow3); -// LOGGER_DEBUG(mainLog, "Softstart running -> new target is %x %x %x", voltageRow1, voltageRow2, voltageRow3); -// -// MAX5715Channel_setValue(self->dacRow1, voltageRow1); -// MAX5715Channel_setValue(self->dacRow2, voltageRow2); -// MAX5715Channel_setValue(self->dacRow3, voltageRow3); -// } + LOGGER_DEBUG(mainLog, "------------------------------"); + + if (PCBA_getInstance()->pcba == Tesla) + { + // Tesla repair only runs ADC row2 and DAC row2 + voltageRow2 = SignalProfileGenerator(startTime, self->secondsCounter, self->repairPreset, presetIndex); + LOGGER_DEBUG(mainLog, "Softstart running -> new target is %x", voltageRow2); + ADCChannel_read(self->adcRow3, &adcVoltageRow2); + LOGGER_DEBUG(mainLog, "Softstart running ->ADC reads %x", adcVoltageRow2); + pidError = voltageRow2 - adcVoltageRow2; + LOGGER_DEBUG(mainLog, "Softstart running ->PID Error %x", pidError); + int pidCalc = PID_calculate(&self->pidRow2, voltageRow2, pidError); + LOGGER_DEBUG(mainLog, "Softstart running ->PID calculates %x", pidCalc); + + if (pidCalc >= 0) + { + MAX5715Channel_setValue(self->dacRow2, pidCalc); + } + } + else if ((PCBA_getInstance()->pcba == Anode) || (PCBA_getInstance()->pcba == CathodeMCP)) + { + + } // Check for end of softstart if (softStartTimer < self->secondsCounter) @@ -242,19 +266,29 @@ static void repairProcess_task(void* parameters) case VOLTAGE_HOLD: { // Actual repair state - hold target voltage until duration has passed - LOGGER_DEBUG(mainLog, "Voltage Hold running -> target is %x", voltageTarget); - + LOGGER_DEBUG(mainLog, "------------------------------"); if (PCBA_getInstance()->pcba == Tesla) { // Tesla repair only runs ADC row2 and DAC row2 - MAX5715Channel_setValue(self->dacRow2, voltageTarget); + + voltageRow2 = SignalProfileGenerator(startTime, self->secondsCounter, self->repairPreset, presetIndex); + LOGGER_DEBUG(mainLog, "Voltage Hold running -> new target is %x", voltageRow2); + ADCChannel_read(self->adcRow3, &adcVoltageRow2); + LOGGER_DEBUG(mainLog, "Voltage Hold running ->ADC reads %x", adcVoltageRow2); + pidError = voltageRow2 - adcVoltageRow2; + LOGGER_DEBUG(mainLog, "Voltage Hold running ->PID Error %x", pidError); + int pidCalc = PID_calculate(&self->pidRow2, voltageRow2, pidError); + LOGGER_DEBUG(mainLog, "Voltage Hold running ->PID calculates %x", pidCalc); + + if (pidCalc >= 0) + { + MAX5715Channel_setValue(self->dacRow2, pidCalc); + } } else if ((PCBA_getInstance()->pcba == Anode) || (PCBA_getInstance()->pcba == CathodeMCP)) { - MAX5715Channel_setValue(self->dacRow1, voltageTarget); - MAX5715Channel_setValue(self->dacRow2, voltageTarget); - MAX5715Channel_setValue(self->dacRow3, voltageTarget); + } // Check for end of softstart @@ -304,7 +338,7 @@ static void repairProcess_task(void* parameters) MAX5715Channel_setValue(self->dacRow2, voltageRow2); MAX5715Channel_setValue(self->dacRow3, voltageRow3); - LOGGER_DEBUG(mainLog, "Repair process finished"); +// LOGGER_DEBUG(mainLog, "Repair process finished"); break; } @@ -321,4 +355,34 @@ static void repairProcess_task(void* parameters) } +static int SignalProfileGenerator(uint32_t startTime, uint32_t currentTime, struct RepairPreset* repairPreset, int presetIndex) +{ + int returnValue = 0; + // Differ between softstart period and voltage hold + if (currentTime - startTime < repairPreset->preset[presetIndex].softstartDuration) + { + // Still in Softstart + int startVoltage = 0; + + // If first preset, start voltage is 0 + if (presetIndex == 0) + { + startVoltage = 0; + } + else + { + // Softstart for another stage - start voltage is hold voltage of previous preset + startVoltage = repairPreset->preset[presetIndex - 1].voltage; + } + returnValue = ((repairPreset->preset[presetIndex].voltage - startVoltage) / repairPreset->preset[presetIndex].softstartDuration) * (currentTime - startTime) + startVoltage; + } + else + { + // In voltage hold + returnValue = repairPreset->preset[presetIndex].voltage; + } + + + return returnValue; +}