mc_acim_drive.c

STM8S105 BLDC源代码

/******************** (C) COPYRIGHT 2008 STMicroelectronics ********************
* File Name          : MC_ACIM_Drive.c
* Author             : IMS Systems Lab 
* Date First Issued  : mm/dd/yyy
* Description        : ACIM drive implementation module
********************************************************************************
* History:
* mm/dd/yyyy ver. x.y.z
********************************************************************************
* THE PRESENT SOFTWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE TIME.
* AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY DIRECT,
* INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING FROM THE
* CONTENT OF SUCH SOFTWARE AND/OR THE USE MADE BY CUSTOMERS OF THE CODING
* INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* THIS SOURCE CODE IS PROTECTED BY A LICENSE.
* FOR MORE INFORMATION PLEASE CAREFULLY READ THE LICENSE AGREEMENT FILE LOCATED
* IN THE ROOT DIRECTORY OF THIS FIRMWARE PACKAGE.
*******************************************************************************/

/******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "stm8s_lib.h"
#include "MC_Drive.h"
#include "MC_dev_drive.h"
#include "MC_ACIM_Motor.h"
#include "MC_ACIM_conf.h"
#include "MC_pid_regulators.h"
#include "MC_vtimer.h"
#include "MC_dev_DAC.h"
#include "MC_ACIM_User_Interface_Param.h"

/* Private typedef -----------------------------------------------------------*/
//ACIMDrive state machine status
typedef enum 
{DRIVE_IDLE,DRIVE_STARTUP,DRIVE_RUN,DRIVE_FAULT} DriveState_t;

/* Private define ------------------------------------------------------------*/
//Virtual timers definitions
#define V_TIM_ACIMDRIVE VTIM4
#define V_TIM_ACIMSTARTUP VTIM5
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY))
#define V_TIM_ACIMUPDATEINFO VTIM6
#define V_TIM_ACIMUPDATEINFO_PERIOD 5
#endif
#define V_TIM_ACIMSTARTUPINIT VTIM7
#define V_TIM_ACIMSTARTUPINIT_PERIOD 5

#define MAX_SPEED_FEEDBACK_HZMEC (s16)(MAX_SPEED_FEEDBACK/6)

/* Private functions ---------------------------------------------------------*/
void ACIM_Drive(void);
void ACIM_StartupEnded(void);
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY))
void ACIM_UpdateInfo(void);
#endif

#ifdef SPEED_CLOSED_LOOP
void ACIM_MTPA_Control(void);
void ACIM_VF_Control(void);
void ACIM_StartUp_ClosedLoop(void);
#endif
#if ((defined SPEED_OPEN_LOOP)||(defined SPEED_OPEN_LOOP_TACHO_SENSING))
void ACIM_StartUp_OpenLoop(void);
void ACIM_OpenLoop(void);
#endif
#if (((defined SPEED_OPEN_LOOP)||(defined SPEED_OPEN_LOOP_TACHO_SENSING))&&\
    (OPENLOOP_CONTROLMODE==SPEED_OPENLOOP_LOAD_COMPENSATION))
s16 ACIM_LoadCompensation(s16 hTargetSpeedHzEl);
#endif

/* Private variable ----------------------------------------------------------*/
//pointers to virtual registers
static pu8 pModulationIndex;
static pu8 pHeatsinkTemp;
static pu16 pFreq;
static pu16 pBusVoltage;

//pointer to motorstruct
static PACIM_Struct_t ACIMmotor;

static DriveState_t DriveState;
MC_FuncRetVal_t DriveStatus;
static control_mode_t OperationControlMode;

static u16 vout;
static u16 freqout;
static s16 hSlip;
static u16 hVFConstant;
static s16 actspeed_HzEl;
static s16 actspeed_HzMec;
static s16 targetspeed_HzEl;
static u8 bV0;

#if ((defined SPEED_OPEN_LOOP)||(defined SPEED_OPEN_LOOP_TACHO_SENSING))
static volatile s32 wAccelerationSteps;
static volatile s16 hDeltaSpeedStep;
static volatile s32 wDeltaSpeedIncr;
static volatile s16 hSpeedReference;
static volatile s16 hInitialSpeed;
static volatile s16 act_targetspeed_HzEl;
#endif

/*******************************************************************************
* Function Name : driveInit
* Description   : It's called by the relative state machine function.
*                 This function initializes local pointers to the virtual
*                 registers and drive structure. It initializes the low level
*                 drive module by calling the function dev_driveInit.
*                 It also starts the ACIMDRIVE virtual timer which calls the 
*                 ACIM_Drive function with a periodicity settled by parameter
*                 CONTROL_LOOP_PERIOD (MC_ACIM_Drive_Param.h)
* Input         : None.
* Output        : None.
* Return        : None.
*******************************************************************************/
void driveInit(pvdev_device_t pdevice)
{	
  dev_driveInit(pdevice);
#ifdef TACHO  
  Tacho_Init(pdevice);
#endif
  pModulationIndex = pdevice->regs.r8+VDEV_REG8_ACIM_MODULATION_INDEX;
  pHeatsinkTemp = pdevice->regs.r8+VDEV_REG8_HEATSINK_TEMPERATURE;
  pFreq = pdevice->regs.r16+VDEV_REG16_ACIM_FREQUENCY;
  pBusVoltage =  pdevice->regs.r16 + VDEV_REG16_BOARD_BUS_VOLTAGE;  
  
  ACIMmotor = Get_ACIM_Struct();
	
  DriveState = DRIVE_IDLE;
  DriveStatus = FUNCTION_RUNNING;

  if (vtimer_TimerElapsed(V_TIM_ACIMDRIVE))
  {
    vtimer_SetTimer(V_TIM_ACIMDRIVE,ACIMmotor->pACIM_Const->bControlLoop_Period_ms,
                    (void*)(&ACIM_Drive)); 
  }  
}

/*******************************************************************************
* Function Name : driveIdle
* Description   : It's called by the relative state machine function.
*                 It updates the local (MC_ACIM_Drive.c) state machine to IDLE
*                 state. It updates the info towards LCD and DAC and LED
*                 (if defined) and reset the ACIMSTARTUPINIT timer
* Input         : None.
* Output        : None.
* Return        : None.
*******************************************************************************/
void driveIdle(void)
{
#ifdef LED_UI
  pvdev_device_t pdevice = vdev_get();
  pdevice->ios.out8(VDEV_OUT8_LED_4,LED_ON);
  pdevice->ios.out8(VDEV_OUT8_LED_3,LED_OFF);  
  pdevice->ios.out8(VDEV_OUT8_LED_2,LED_OFF);
  pdevice->ios.out8(VDEV_OUT8_LED_1,LED_OFF);  
#endif
  DriveState = DRIVE_IDLE;
  vtimer_KillTimer(V_TIM_ACIMSTARTUPINIT);
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY))  
  ACIM_UpdateInfo();
#endif
}

/*******************************************************************************
* Function Name : driveStartUpInit
* Description   : It's called by the relative state machine function.
*                 This routine handles these phases of the startup procedure:
*                 1.(speed sensor enabled) Check the motor speed: if it's higher
*                 then the threshold STALL_SPEED (MC_ACIM_Drive_Param.h) the
*                 startup is aborted and an error message "MOTOR IS RUNNING"
*                 is issued;
*                 2.(speed closed loop): checks the operation mode to be applied
*                 (V/f and slip control or MTPA)(operation mode won't be changed
*                 any longer during START or RUN states), initializes the
*                 integral terms of the PID controller(s);
*                 3. reads the desired speed direction (speed direction won't
*                 be reversible any longer during START or RUN states);
*                 4. initializes control variables to zero;
*                 5. calls the relative low level function (that enables the PWM
*                 outputs);
*                 6. Initializes the virtual timer ACIMSTARTUPINIT, used to
*                 charge the high side drivers' bootstrap capacitors.
* Input         : None.
* Output        : None.
* Return        : FUNCTION_ENDED.
*******************************************************************************/
MC_FuncRetVal_t driveStartUpInit(void)
{
  MC_FuncRetVal_t tempRetVal;

#ifdef TACHO
  
  //speed check, the motor should be at standstill before startup proc init  
  actspeed_HzMec = Tacho_GetSpeed_HzMec();
  if (actspeed_HzMec > ACIMmotor->pACIM_Const->hStall_speed)
  {
    tempRetVal = FUNCTION_ERROR;        
  }  
  else
#endif
  {
    s16 targetspeed_RPM;
    
#ifdef SPEED_CLOSED_LOOP
    
    //checks the operation mode to be applied: V/f or MTPA    
    OperationControlMode = ACIMmotor->pACIM_Var->Control_Mode;
    ACIMmotor->pACIM_Var->Actual_Control_Mode = OperationControlMode;
        
    //initializes to zero the integral terms of the PID controller(s)
    ACIMmotor->pACIM_Const->pPID_VF_Struct->pPID_Var->wIntegral = 0;
    ACIMmotor->pACIM_Const->pPID_MTPA_Struct->pPID_Var->wIntegral = 0;      
#endif
    
    //Speed direction reading    
    targetspeed_RPM = ACIM_GetTargetRotorSpeed_RPM();
    
    if (targetspeed_RPM >= 0)
    {
      ACIMmotor->pACIM_Var->bDirection = +1;
    }
    else
    {
      ACIMmotor->pACIM_Var->bDirection = -1;
    }

    //output initialization (through virtual registers)
    *pModulationIndex = 0;
    *pFreq = 0;
    vout = 0;
    freqout = 0;
    
    dev_driveStartUpInit();
    
    //bootstrap capacitors charging phase is started 
    vtimer_SetTimer(V_TIM_ACIMSTARTUPINIT,V_TIM_ACIMSTARTUPINIT_PERIOD,0);
    
    tempRetVal = FUNCTION_ENDED;
  }
  
  return tempRetVal;
}

/*******************************************************************************
* Function Name : driveStartUp
* Description   : It's called by the relative state machine function.
*                 This routine handles these phases of the startup procedure:
*                 1.it ends the bootstrap capacitor charging as soon as virtual
*                 timer ACIMSTARTUPINIT is elapsed. At that point, it updates
*                 the local state machine to DRIVE_STARTUP state and settles the
*                 virtual timer ACIMSTARTUP to the value allowed for the startup
*                 process (parameter STARTUP_DURATION, MC_ACIM_Drive_Param.h)
*                 2.Updates the info towards LCD, DAC and LED;
*                 3.Returns the value defined by variable DriveStatus; 
*                 4.If DriveStatus is FUNCTION_ENDED, it's reset to
*                 FUNCTION_RUNNING
* Input         : None.
* Output        : None.
* Return        : DriveStatus.
*******************************************************************************/
MC_FuncRetVal_t driveStartUp(void)
{
  MC_FuncRetVal_t tempRetVal;
  
#ifdef LED_UI
  pvdev_device_t pdevice = vdev_get();
  pdevice->ios.out8(VDEV_OUT8_LED_4,LED_OFF);
  pdevice->ios.out8(VDEV_OUT8_LED_3,LED_ON);
#endif

  if (vtimer_TimerElapsed(V_TIM_ACIMSTARTUPINIT))
  {
    //bootstrap capacitor charging phase is ended
    vtimer_SetTimer(V_TIM_ACIMSTARTUPINIT,65535,0);
    vtimer_KillTimer(V_TIM_ACIMSTARTUP);
    
    //start-up procedure enabled
    DriveState = DRIVE_STARTUP;
  }
  
  tempRetVal = DriveStatus;
  
  //check drive status  
  if (DriveStatus == FUNCTION_ENDED)
  {
    //startup procedure ended, DriveStatus reset to FUNCTION_RUNNING
    DriveStatus = FUNCTION_RUNNING;
  }
  
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY))   
  ACIM_UpdateInfo();
#endif

  return tempRetVal;
}

/*******************************************************************************
* Function Name : driveRun
* Description   : It's called by the relative state machine function.
*                 1.It updates or confirms the local state machine to DRIVE_RUN
*                 state; updates the info towards LCD, DAC and LED;
*                 2.Returns the value defined by variable DriveStatus; 
*                 3.If DriveStatus is FUNCTION_ENDED, it's reset to
*                 FUNCTION_RUNNING
* Input         : None.
* Output        : None.
* Return        : DriveStatus.
*******************************************************************************/
MC_FuncRetVal_t driveRun(void)
{
  MC_FuncRetVal_t tempRetVal;
  
#ifdef LED_UI
  pvdev_device_t pdevice = vdev_get();
  pdevice->ios.out8(VDEV_OUT8_LED_3,LED_OFF);
  pdevice->ios.out8(VDEV_OUT8_LED_2,LED_ON);