pwrctrl_sm.c

Go to the documentation of this file.

 
#include <xc.h>
#include <stdint.h> // include standard integer data types
#include <stdbool.h> // include standard boolean data types
#include <stddef.h> // include standard definition data types
#include <stdlib.h> // for abs function, see how it works
                               
 
// PSFB header files
#include "pwrctrl.h"
//#include "config/macros.h"
#include "device/device.h"
#include "fault/fault.h"
//#include "config/config.h"
#include "pwrctrl_pwm.h"
#include "system/pins.h"
 
 
// temp
//uint16_t current_da = 1303;
float temp;
uint16_t ref_diff;
uint16_t delay_count = 2;
 
//PRIVATE FUNCTIONS
static void PCS_INIT_handler(POWER_CONTROL_t* pcInstance);
static void PCS_WAIT_IF_FAULT_ACTIVE_handler(POWER_CONTROL_t* pcInstance);
static void PCS_STANDBY_handler(POWER_CONTROL_t* pcInstance);
static void PCS_SOFT_START_handler(POWER_CONTROL_t* pcInstance);
static void PCS_UP_AND_RUNNING_handler(POWER_CONTROL_t* pcInstance);
static void PCS_PRECHARGE_handler(POWER_CONTROL_t* pcInstance);
 
 
void PwrCtrl_StateMachine(POWER_CONTROL_t* pcInstance)
{ 
    switch (pcInstance->State)
    {
        case PWRCTRL_STATE_INITIALIZE:
            PCS_INIT_handler(pcInstance);
            break;    
 
        case PWRCTRL_STATE_FAULT_DETECTION:
            PCS_WAIT_IF_FAULT_ACTIVE_handler(pcInstance);
            break;
 
        case PWRCTRL_STATE_STANDBY:
            PCS_STANDBY_handler(pcInstance);
            break;
 
        case PWRCTRL_STATE_SOFT_START:
            PCS_SOFT_START_handler(pcInstance);
            break;
 
        case PWRCTRL_STATE_UP_AND_RUNNING:
            PCS_UP_AND_RUNNING_handler(pcInstance);
            break;
            
        case PWRCTRL_STATE_PRECHARGE:
            PCS_PRECHARGE_handler(pcInstance);
            break;
 
        default:
            pcInstance->State = PWRCTRL_STATE_INITIALIZE;
            break;      
    }
}
 
 
static void PCS_INIT_handler(POWER_CONTROL_t* pcInstance)
{
    // Current sensor calibration is complete. Update the offset of the current sensor
    // pcInstance->Data.ISecSensorOffset = Dev_CurrentSensor_Get_Offset();
    
    // Ensure PWM output is disabled
    // PwrCtrl_PWM_Disable();
    
    //Disable short circuit for now  -> Temporary should go in fault set/reset
    // FAULT_EN_SetLow();
    
    // Reset fault objects status bits
    Fault_Reset();
    
    //reset control loop
    PwrCtrl_Reset();
 
        //disable Iloop here
    pcInstance->ILoop.Enable = 0;
    //disable Vloop here
    pcInstance->VLoop.Enable = 0;
 
    // Clear power control fault active bit
    pcInstance->Status.bits.FaultActive = 0;
    // Clear power control running bit
    pcInstance->Status.bits.Running = 0;
    // Clear power control enable bit
    pcInstance->Properties.Enable = 0;
    
    
    Dev_LED_Off(LED_BOARD_GREEN);
    Dev_LED_Off(LED_BOARD_RED);
 
    //add delay of some ms to for values to be stable
    if (delay_count-- == 0) {
        delay_count = 3;
        // review the
        if( PwrCtrl_UpdateAverage(
            &pcInstance->PrimaryCT_Offset,
            PwrCtrl_GetAdc_Ipri_ct()) !=0 
            &&
        PwrCtrl_UpdateAverage(
            &pcInstance->SecondarySh_Offset,
            PwrCtrl_GetAdc_Isec_shunt()) !=0
            &&
        PwrCtrl_UpdateAverage(
            &pcInstance->VoutCalibratingAveraging,
            pcInstance->Data.VOutVoltage) !=0        
            )
        {
            pcInstance->Data.IPriSensorOffset = pcInstance->PrimaryCT_Offset.AverageValue;
            pcInstance->Data.ISecSensorOffset = pcInstance->SecondarySh_Offset.AverageValue;
            pcInstance->VoutCalibrate.real_value = pcInstance->VoutCalibratingAveraging.AverageValue;
        
            //34 amps
            pcInstance->SecRec.Threshold_high = pcInstance->Data.ISecSensorOffset + 421;
            //30amps
            pcInstance->SecRec.Threshold_low = pcInstance->Data.ISecSensorOffset + 372;
            
            pcInstance->VoutCalibrate.calculated_value = 2293; // for 12 volts
            pcInstance->VoutCalibrate.gain_factor = 
                    (float)psfb_ptr->VoutCalibrate.calculated_value/ 
                    (float)psfb_ptr->VoutCalibrate.real_value;
            
            pcInstance->State = PWRCTRL_STATE_PRECHARGE;
 
        FAULT_EN_SetHigh();
        }    
    }
    
 
    // if (dev_AreOffsetsCalculated() == 1){
    //     pcInstance->Data.ISecSensorOffset = dev_Get_SecondaryShuntOffset();
    //     pcInstance->Data.IPriSensorOffset = dev_Get_PrimaryCTOffset();
        
    //     //34 amps
    //     pcInstance->SecRec.Threshold_high = pcInstance->Data.ISecSensorOffset + 421;
    //     //30amps
    //     pcInstance->SecRec.Threshold_low = pcInstance->Data.ISecSensorOffset + 372;
 
    //     pcInstance->State = PWRCTRL_STATE_PRECHARGE;
 
 
    //     FAULT_EN_SetHigh();
    // }
    // else {
    //     dev_MeasureOffsets();
    // }
 
    //pcInstance->State = PWRCTRL_STATE_PRECHARGE;
    
}
 
 
static void PCS_WAIT_IF_FAULT_ACTIVE_handler(POWER_CONTROL_t* pcInstance)
{   
    if (pcInstance->Fault.FaultDetected == 0)
    {
        pcInstance->Status.bits.FaultActive = 0;
        FAULT_EN_SetLow();               // exiting fault, making sure secondary current has died down
        dev_MeasureOffsets_Initialize(); // measure offsets again.
        pcInstance->State = PWRCTRL_STATE_INITIALIZE; // reinit system
    }
    
        // Check for fault event 
    if (pcInstance->Fault.FaultDetected)
    {
        // Clear power control enable bit
        pcInstance->Properties.Enable = 0;
 
        // override precharge , if fault happens in precharge, to clear precharge bit
       pcInstance->Precharge.PrechargeEnabled = 0 ;
        
        // State back to STATE_FAULT_DETECTION
        pcInstance->State = PWRCTRL_STATE_FAULT_DETECTION;
    }
    
    Dev_LED_Blink(LED_BOARD_RED);
    Dev_LED_Off(LED_BOARD_GREEN);
}
 
 
static void PCS_PRECHARGE_handler(POWER_CONTROL_t* pcInstance)
{
    // Check for fault event 
    if (pcInstance->Fault.FaultDetected)
    {
        // Clear power control enable bit
        pcInstance->Properties.Enable = 0;
        
        // State back to STATE_FAULT_DETECTION
        pcInstance->State = PWRCTRL_STATE_FAULT_DETECTION;
        return;     //TODO: fix state machine and fault jumps
    }
    //    // NOTE: Power control enable is controlled externally 
   else if ((pcInstance->Properties.Enable)&&(pcInstance->Precharge.precharged == 1))
   {       
       // Reset fault objects status bits
       Fault_Reset();
           
       // Reset the power control properties and control loop histories
       PwrCtrl_Reset();
 
       // Update PWM distribution
//        PwrCtrl_PWM_Update(&psfb);
 
       // Enable PWM physical output
       //PwrCtrl_PWM_Enable();
 
       // Enable power control running bit
       pcInstance->Status.bits.Running = 1;
 
       // override precharge 
       pcInstance->Precharge.PrechargeEnabled = 0 ;
 
       // Next State assigned to STATE_SOFT_START
       pcInstance->State = PWRCTRL_STATE_STANDBY;
       
   }
 
   else if (pcInstance->Precharge.PrechargeEnabled == 1) {
//        pcInstance->Precharge.maxDutyCycle = 42;        
        if (pcInstance->Precharge.DutyCycle < pcInstance->Precharge.maxDutyCycle) {
            pcInstance->Precharge.delayCounter = pcInstance->Precharge.delayCounter + 1;
            if (pcInstance->Precharge.delayCounter > 25){                       //
                Nop();
                pcInstance->Precharge.delayCounter = 0;
                pcInstance->Precharge.DutyCycle = pcInstance->Precharge.DutyCycle + 1;
                }
            }
        else {
            pcInstance->Precharge.precharged = 1;
        }     
        PwrCtrl_PWM_SetDutyCyclePrimary(pcInstance->Precharge.DutyCycle);
        pcInstance->State = PWRCTRL_STATE_PRECHARGE;
        Dev_LED_Blink(LED_BOARD_GREEN);
        Dev_LED_Off(LED_BOARD_RED);
   }
   
   else if(pcInstance->Precharge.PrechargeEnabled == 0) {
        pcInstance->Precharge.DutyCycle = 0;
        PwrCtrl_PWM_SetDutyCyclePrimary(pcInstance->Precharge.DutyCycle);
        PwrCtrl_PWM_Stop_Switching();
        pcInstance->Precharge.precharged = 0;
        pcInstance->State = PWRCTRL_STATE_PRECHARGE;
   }
}
 
 
 
static void PCS_STANDBY_handler(POWER_CONTROL_t* pcInstance)
{
    // Check for fault event 
    if (pcInstance->Fault.FaultDetected)
    {
        // Clear power control enable bit
        pcInstance->Properties.Enable = 0;
        
        // State back to STATE_FAULT_DETECTION
        pcInstance->State = PWRCTRL_STATE_FAULT_DETECTION;
    }
    
        // Check if Enable bit has been cleared
    else if (!pcInstance->Properties.Enable) 
    {
        // State back to STATE_INIT
        pcInstance->State = PWRCTRL_STATE_INITIALIZE; 
    }
 
    // NOTE: Power control enable is controlled externally 
    else if (pcInstance->Properties.Enable)
    {
        // Reset fault objects status bits
        // Fault_Reset();
            
        // Reset the power control properties and control loop histories
        // PwrCtrl_Reset();
 
        // Update PWM distribution
//        PwrCtrl_PWM_Update(&psfb);
 
        // Enable power control running bit
        pcInstance->Status.bits.Running = 1;
 
        // Next State assigned to STATE_SOFT_START
        pcInstance->State = PWRCTRL_STATE_SOFT_START;
        
        // Enable PWM physical output
        // PwrCtrl_PWM_Enable();
 
        // enable SR flag to be controlled by current
        pcInstance->SecRec.SR_Enabled = 1;
        //Enable Iloop here
        pcInstance->ILoop.Enable = 1;
        //Enable Vloop here
        pcInstance->VLoop.Enable = 1;
 
        //reference set to calibrated 12V 
        float vreftem = (float)2293 * pcInstance->VoutCalibrate.gain_factor; // 2293 = 12 V (2293/4095 * 3.3 * Gain) Gain =154mv/v
        pcInstance->VLoop.Reference =                           (uint16_t)vreftem;
        pcInstance->Properties.VSecReference =                  (uint16_t)vreftem;
        pcInstance->Droop.Droop_Voltage_Reference_from_PBV =    (uint16_t)vreftem;
    }
 
    Dev_LED_On(LED_BOARD_GREEN);
    Dev_LED_Off(LED_BOARD_RED);
}
 
 
static void PCS_SOFT_START_handler(POWER_CONTROL_t* pcInstance)
{
    // Check for fault event 
    if (pcInstance->Fault.FaultDetected)
    {
        // Clear power control enable bit
        pcInstance->Properties.Enable = 0;
        
        // State back to STATE_FAULT_DETECTION
        pcInstance->State = PWRCTRL_STATE_FAULT_DETECTION;
    }
  
    // Check if Enable bit has been cleared
    else if (!pcInstance->Properties.Enable) 
    {
        // Disable PWM physical PRIMARY output
        //PwrCtrl_PWM_Disable();
        PwrCtrl_PWM_Stop_Switching();
        
        // Clear power control running bit
        pcInstance->Status.bits.Running = 0;
        
        // State back to STATE_INIT
        pcInstance->State = PWRCTRL_STATE_STANDBY; 
    }
 
     else
    {   
        // Ramp Up the Voltage, Current and Power reference
        uint16_t rampComplete = PwrCtrl_RampReference(&pcInstance->VRamp);  
        
        if (rampComplete)
            // Next State assigned to STATE_ONLINE
            pcInstance->State = PWRCTRL_STATE_UP_AND_RUNNING; 
    }
        
    Dev_LED_Blink(LED_BOARD_GREEN);
    Dev_LED_Off(LED_BOARD_RED);
}
 
 
static void PCS_UP_AND_RUNNING_handler(POWER_CONTROL_t* pcInstance)
{    
    // Check for fault event 
    if (pcInstance->Fault.FaultDetected)
    {
        // Clear power control enable bit
        pcInstance->Properties.Enable = 0;
        
        // State back to STATE_FAULT_DETECTION
        pcInstance->State = PWRCTRL_STATE_FAULT_DETECTION;
    }
    
    else if (!pcInstance->Properties.Enable) 
        // Check if Enable bit has been cleared
        {
            // Disable PWM physical output
            //PwrCtrl_PWM_Disable();
            PwrCtrl_PWM_Stop_Switching();
 
            // Clear power control running bit
            pcInstance->Status.bits.Running = 0;
 
            // State back to STATE_STANDBY AND THEN INITIALITZE
            pcInstance->State = PWRCTRL_STATE_STANDBY; 
            Dev_LED_Off(LED_BOARD_GREEN);
        }      
        //removing it for now
        
    // Check if there is change in power control references    
        
    else if (pcInstance->VLoop.Reference != pcInstance->Properties.VSecReference)
    {    
            // State back to STATE_SOFT_START
            pcInstance->State = PWRCTRL_STATE_SOFT_START;
    }
    
    else {
        // average last 1000 ->each sample 10us, called every 100us * 100 times 
        PwrCtrl_DroopAverage();
        // droop update
        if (++pcInstance->Droop.Droop_Delay_Counter>=999)
        {
            temp = pcInstance->ISecAveraging.AverageValue - pcInstance->Data.ISecSensorOffset; // * 0.073;
            if (temp > 10) // avoid gar
            {
                temp = temp * 0.0855;   //droop variable 0.0855 adc counts per amp:
                pcInstance->Droop.ref_diff = (uint16_t)temp;
            }
            else 
                pcInstance->Droop.ref_diff = 0;
            
            if (pcInstance->Droop.ref_diff > 400) pcInstance->Droop.ref_diff = 0 ; // voltage drop of 1 volt results in 191 counts, something is wrong, set droop to 0
            if (pcInstance->Droop.ref_diff > 200) pcInstance->Droop.ref_diff = 191; // clamp to 1 V in normal operation
            
            pcInstance->Properties.VSecReference = pcInstance->Droop.Droop_Voltage_Reference_from_PBV  - pcInstance->Droop.ref_diff ;
            //pcInstance->Droop.Droop_New_Voltage_Reference = pcInstance->Droop.Droop_New_Voltage_Reference >> 8;
            //pcInstance->Properties.VSecReference = pcInstance->Droop.Droop_Voltage_Reference_from_PBV  - pcInstance->Droop.Droop_New_Voltage_Reference ; 
            pcInstance->Droop.Droop_Delay_Counter = 0;
        } 
    }  
    Dev_LED_Blink(LED_BOARD_GREEN);
    Dev_LED_Off(LED_BOARD_RED);
}