STM32FOC电机控制角度速度计算源代码MC_State_Observer

/******************** （C） COPYRIGHT 2007 STMicroelectronics ********************
* File Name          : MC_State_Observer.c
* Author             : IMS Systems Lab 
* Date First Issued  : 21/11/07
* Description        : This module implements the State Observer of 
*                      the PMSM B-EMF thus identifying rotor speed and position
*
********************************************************************************
* History:
* 21/11/07 v1.0
********************************************************************************
* 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.
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/

#include “stm32f10x_type.h“
#include “MC_type.h“
#include “MC_State_Observer.h“

/* Private define ------------------------------------------------------------*/
#define BUFFER_SIZE （u8）64

/* Private variables ---------------------------------------------------------*/
static s32 wIalfa_estwIbeta_est;
static s32 wBemf_alfa_est wBemf_beta_est;
static s32 wSpeed_PI_integral_sum;
static s16 hBemf_alfa_esthBemf_beta_est;

static s16 hSpeed_Buffer[BUFFER_SIZE];
static s16 hRotor_El_Angle;
static s16 hRotor_Speed_dpp;


static bool Max_Speed_Out Min_Speed_Out Is_Speed_Reliable;
static u8 bSpeed_Buffer_Index;

extern const s16 hSin_Cos_Table[256];

static volatile s16 hC2 hC4;
static s16 hF1 hF2 hF3 hC1 hC3hC5 hC6;

static volatile s16 hSpeed_P_Gain hSpeed_I_Gain;
static s32 wMotorMaxSpeed_dpp;
static u16 hPercentageFactor;

/* Private function prototypes -----------------------------------------------*/
s16 Calc_Rotor_Speed（s16 s16）;
s16 Speed_PI（s16 s16）;
void Store_Rotor_Speed（s16）;

//function of MC_Clarke_Park.c
Trig_Components Trig_Functions（s16）;

/*******************************************************************************
* Function Name : STO_Calc_Rotor_Angle  角度
* Description : This function implements the State Observer of a
*       	PMSM back-emfs. Thus motor speed and rotor angle are calculated
*	        and stored in the variables hRotor_SpeedhRotor_El_Angle.
* Input : Stator voltage reference values Valpha Vbeta （as provided by RevPark
*         through variable Stat_Volt_alfa_beta）
*	  Stator current values IalphaIbeta （as provided by Clark trasformation
*	  through variable Stat_Curr_alfa_beta）  DC bus voltage.                          // 定子的电压与电流有关
* Output : None
* Return : None
***************************