mc_acim_drive.c

STM8S105 BLDC源代码

#endif    
  
  DriveState = DRIVE_RUN;
  
  tempRetVal = DriveStatus;
  
  //check drive status  
  if (DriveStatus == FUNCTION_ENDED)
  {
    DriveStatus = FUNCTION_RUNNING;
  }

#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY)) 
  ACIM_UpdateInfo();
#endif

  return tempRetVal;
}

/*******************************************************************************
* Function Name : driveWait
* Description   : It's called by the relative state machine function.
*                 1.it updates the info towards LCD, DAC;
* Input         : None.
* Output        : None.
* Return        : FUNCTION_ENDED.
*******************************************************************************/
MC_FuncRetVal_t driveWait(void)
{
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY)) 
  ACIM_UpdateInfo();
#endif
  return FUNCTION_ENDED;
}

/*******************************************************************************
* Function Name : driveStop
* Description   : It's called by the relative state machine function.
*                 1. puts the control variables to zero;
*                 2. calls the relative low level function (that disables the PWM
*                 outputs);
* Input         : None.
* Output        : None.
* Return        : FUNCTION_ENDED.
*******************************************************************************/
MC_FuncRetVal_t driveStop(void)
{
  //output control variables set to zero (through virtual registers)
  dev_driveStop();
  *pModulationIndex = 0;
  *pFreq = 0;
  
  //local state machine is set to IDLE state
  DriveState = DRIVE_IDLE;

  return FUNCTION_ENDED;
}

/*******************************************************************************
* Function Name : driveFault
* Description   : It's called by the relative state machine function.
*                 It updates the info towards LCD, DAC, LED;
* Input         : None.
* Output        : None.
* Return        : FUNCTION_ENDED.
*******************************************************************************/
MC_FuncRetVal_t driveFault(void)
{
#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_OFF);
  pdevice->ios.out8(VDEV_OUT8_LED_2,LED_OFF);  
  pdevice->ios.out8(VDEV_OUT8_LED_1,LED_ON);
#endif    
  return FUNCTION_ENDED;
}


/*****************      PRIVATE FUNCTIONS DEFINITIONS     *********************/


/*******************************************************************************
* Function Name : ACIM_StartupEnded
* Description   : The function is awakened at the end of the time period defined
*                 by virtual timer ACIMSTARTUP.
*                 1. Speed Closed loop mode: if the local (MC_ACIM_Drive.c) state
*                 machine is still in DRIVE_STARTUP state, the function will put the
*                 variable DriveStatus to FUNCTION_ERROR;at that point, function
*                 driveStartUp will return FUNCTION_ERROR to the main state
*                 machine, which will go to FAULT state;
*                 2. Speed open loop and tacho sensing: the motor speed is checked
*                 vs parameter STARTUP_VAL_SPEED (MC_ACIM_Drive_Param.h): if it
*                 is higher/lower than this threshold, the function will put the variable
*                 DriveStatus to FUNCTION_ENDED/FUNCTION_ERROR; at that point,
*                 function driveStartUp will return FUNCTION_ENDED/FUNCTION_ERROR
*                 to the main state machine, which will go to RUN/FAULT state;
*                 3. Speed open loop: variable DriveStatus is put to
*                 FUNCTION_ENDED; therefore, function driveStartUp will return
*                 FUNCTION_ENDED and the main state machine will go to RUN state
* Input         : None.
* Output        : None.
* Return        : None.
*******************************************************************************/
void ACIM_StartupEnded(void)
{
#ifdef SPEED_CLOSED_LOOP
  if (DriveState == DRIVE_STARTUP)
  {
    //time allowed for startup procedure is ended
    DriveStatus = FUNCTION_ERROR;
  }
#elif defined SPEED_OPEN_LOOP
#ifdef TACHO
  //startup ended successfully if motor speed is higher than STARTUP_VAL_SPEED
  if (actspeed_HzMec > ACIMmotor->pACIM_Const->hStartup_val_speed)
  {
    DriveStatus = FUNCTION_ENDED;
  }
  else
  {
    DriveStatus = FUNCTION_ERROR;
  }    
#else
  //startup ended
  DriveStatus = FUNCTION_ENDED;  
#endif
#endif
}

/*******************************************************************************
* Function Name : ACIM_UpdateInfo
* Description   : It updates motor drive informations towards the LCD UI and
*                 DAC module. Informations are refreshed with the periodicity
*                 defined by virtual timer ACIMUPDATEINFO (VTIM6)
* Input         : None.
* Output        : None.
* Return        : None.
*******************************************************************************/
#if ((defined DISPLAY)||(defined DAC_FUNCTIONALITY))
void ACIM_UpdateInfo(void)
{  
  if (vtimer_TimerElapsed(V_TIM_ACIMUPDATEINFO))
  {
    vtimer_SetTimer(V_TIM_ACIMUPDATEINFO,V_TIM_ACIMUPDATEINFO_PERIOD,0);
    {
      u16 hBusV;
			
#ifdef TACHO
      ACIM_SetMeasuredRotorSpeed_RPM(Tacho_GetSpeed_RPM());
#endif

      ACIM_Set_HeatsinkTemp((u8)(((((u16)(*pHeatsinkTemp))*4) +
                            ACIMmotor->pACIM_Const->bNTC_alpha*25 -
                            ACIMmotor->pACIM_Const->bNTC_beta)/
                            ACIMmotor->pACIM_Const->bNTC_alpha));
      
      hBusV = (u16)((*pBusVoltage * (s32)(ACIMmotor->pACIM_Const->hDigit_to_BusV_Conv))/256);
      ACIM_Set_Bus_Voltage(hBusV);
      
#ifdef DAC_FUNCTIONALITY
      dev_DACUpdateValues(DAC_CH_1, (u8)(vout));
      dev_DACUpdateValues(DAC_CH_2, (u8)(freqout/4));
      //Modify the lines above in order to monitor other variables, for instance
      //as it done below. Consider that the DAC functionality has an 8 bit
      //resolution, a suitable scaling factor should be applied to user defined
      //variables
      //dev_DACUpdateValues(DAC_CH_?, (u8)(actspeed_HzEl/4));
      //dev_DACUpdateValues(DAC_CH_?, (u8)(hSlip*2+128));
#endif      
    }
  }
}
#endif

/*******************************************************************************
* Function Name : ACIM_Drive
* Description   : The function is awakened at the end of the time period defined
*                 by virtual timer ACIMSTARTUP (VTIM4).
*                 This routine handles all the phases of the motor drive.
*                 SPEED CLOSED LOOP MODE:--------------------------------------
*                 1. motor speed is read and converted in electrical frequency;
*                 2. action is undertaken according to local (MC_ACIM_Drive.c)
*                 state machine:
*                 DRIVE_IDLE -> no actions
*                 DRIVE_STARTUP -> function ACIM_StartUp_ClosedLoop is called;
*                 DRIVE_RUN -> a)target speed is read; b)motor speed is checked
*                 vs parameters MIN_RUN_SPEED and MAX_SPEED_FEEDBACK 
*                 (MC_ACIM_Drive_Param.h): if it's outside the allowed range,
*                 the function will put the variable DriveStatus to
*                 FUNCTION_ERROR; at that point, function driveRun will return 
*                 FUNCTION_ERROR to the main state machine, which will go to 
*                 FAULT state; c) function ACIM_VF_Control or ACIM_MTPA_Control
*                 is called according to the actual control mode;
*                 3. low level control variables updated (virtual registers
*                 VDEV_REG16_ACIM_FREQUENCY, VDEV_REG8_ACIM_MODULATION_INDEX);
*                 4. the low level function dev_driveRun is called;
*                 SPEED OPEN LOOP MODE:----------------------------------------
*                 1. (tacho sensing enabled) motor speed is read;
*                 2. action is undertaken according to local (MC_ACIM_Drive.c)
*                 state machine:
*                 DRIVE_IDLE -> no actions
*                 DRIVE_STARTUP -> function ACIM_StartUp_OpenLoop is called;
*                 DRIVE_RUN -> a)target speed is read; b) (tacho sensing enabled)
*                 motor speed is checked vs parameters MIN_RUN_SPEED and 
*                 MAX_SPEED_FEEDBACK (MC_ACIM_Drive_Param.h): if it's outside 
*                 the allowed range, the function will put the variable 
*                 DriveStatus to FUNCTION_ERROR; at that point, function driveRun
*                 will return FUNCTION_ERROR to the main state machine,which will
*                 go to FAULT state; c) (load compensation enabled) function
*                 ACIM_LoadCompensation is called; d)function ACIM_OpenLoop is
*                 called;
*                 3. low level control variables updated (virtual registers
*                 VDEV_REG16_ACIM_FREQUENCY, VDEV_REG8_ACIM_MODULATION_INDEX);
*                 4. the low level function dev_driveRun is called;
* Input         : None.
* Output        : None.
* Return        : None.
*******************************************************************************/
void ACIM_Drive(void)
{  
  vtimer_SetTimer(V_TIM_ACIMDRIVE,ACIMmotor->pACIM_Const->bControlLoop_Period_ms,(void*)(&ACIM_Drive));
  
#ifdef TACHO
  //reads the motor speed, mechanical Hz*10
  actspeed_HzMec = Tacho_CalcSpeed_HzMec();
#endif

#ifdef SPEED_CLOSED_LOOP
  //motor speed is converted, electrical Hz*10 
  actspeed_HzEl = actspeed_HzMec * ACIMmotor->pACIM_Const->bMotor_pole_Pairs;
#endif
  
#ifdef SET_TARGET_SPEED_BY_POTENTIOMETER
  //reads target speed from potentiometer and loads it in drive structure
  ACIM_SetTargetRotorSpeed_RPM_HzEl((s16)
                                    (((u32)(ACIMmotor->pACIM_Const->hMax_Speed)*
                                      ACIMmotor->pACIM_Var->hUserADC)/255));
#endif  
  
  switch (DriveState)
  {
  case DRIVE_IDLE:
    {
      DriveStatus = FUNCTION_RUNNING;
    }
    break;
  case DRIVE_STARTUP:
    {
#ifdef SPEED_CLOSED_LOOP
      ACIM_StartUp_ClosedLoop();
#elif defined SPEED_OPEN_LOOP
      ACIM_StartUp_OpenLoop();
#endif      
    }
    break;
  case DRIVE_RUN:
    {
      //reads target speed from drive structure
      targetspeed_HzEl = ACIM_GetTargetRotorSpeed_HzEl();
      
#ifdef TACHO
      //motor speed should be within MIN_RUN_SPEED / MAX_SPEED_FEEDBACK
      if (actspeed_HzMec < ACIMmotor->pACIM_Const->hMin_run_speed)
      {
        DriveStatus = FUNCTION_ERROR;        
      }
      else if (actspeed_HzMec > MAX_SPEED_FEEDBACK_HZMEC)
      {
        DriveStatus = FUNCTION_ERROR;
      }
      else
#endif
      {
#ifdef SPEED_CLOSED_LOOP
        //const V/f + slip control / const slip + V control according to
        //actual control mode
        if (ACIMmotor->pACIM_Var->Actual_Control_Mode == SPEED_CLOSEDLOOP_VF)
        {
          ACIM_VF_Control();          
        }
        else if (ACIMmotor->pACIM_Var->Actual_Control_Mode == SPEED_CLOSEDLOOP_MTA)
        {
          ACIM_MTPA_Control();
        }
#elif defined SPEED_OPEN_LOOP
#if (OPENLOOP_CONTROLMODE==SPEED_OPENLOOP_LOAD_COMPENSATION)
        //slip frequency chosen as function of motor speed (load compensation LUT)
        ACIMmotor->pACIM_Var->hSlip = ACIM_LoadCompensation(hSpeedReference);
        
        //V/f ratio could be also defined as function of motor speed; in that
        //case, a function 'f2' and an additional LUT are to be implemented 
        //ACIMmotor->pACIM_Var->hVFConstant = f2(hSpeedReference);                  
#endif 
        hVFConstant = ACIMmotor->pACIM_Var->hVFConstant;
        hSlip = ACIMmotor->pACIM_Var->hSlip;
        ACIM_OpenLoop();
#endif
      } 
    }
    break;    
  }
  
  //low level control variables updated through virtual registers
  //VDEV_REG16_ACIM_FREQUENCY / VDEV_REG8_ACIM_MODULATION_INDEX)  
  *pModulationIndex = (u8)(vout);
  *pFreq = (freqout);
  
  dev_driveRun();
}

#ifdef SPEED_CLOSED_LOOP
/*******************************************************************************
* Function Name : ACIM_MTPA_Control
* Description   : This function implements the MTPA strategy, control area 1
*                 (constant slip applied, V/f ratio managed by 
*                 the dedicated MTPA PID regulator).
*                 The operation mode controller, area1 -> area2 transition,
*                 is implemented.