基于磁链观测器无感foc的代码解读

只看该作者 · 2025-1-4 11:53

#申请原创# @21小跑堂
无感foc与有感foc的区别只是有没有接入编码器、HALL传感器的区别；
有感foc通过编码器、HALL识别初始角度，计算电角度；
无感foc有许多方案进行电角度识别，高频注入、磁链观测等多种方案，这次只说明磁链观测器的方案解读；
先是foc的配置，一般有但电阻、双电阻、三电阻识别，常用的双电阻识别较多；
理论上双电阻可以计算出第三路电流；
foc大致流程
通过三路电流，Ia,Ib,Ic值，使用clark变化，得到Iα和Iβ；
使用Park变化得到d轴电流和q轴电流；
通过pid电流环使用Id/Iq得到Vd/Vq;
根据编码器或其它方式得到电角度；
使用逆向park变化通过电角度参数得到Vα和Vβ；
计算SVPWM；
根据Vα和Vβ获取当前所在电角度扇区，计算SVPWM的Ta,Tb,Tc值，输出到三相桥的pwm控制；

磁链观测器源码

复制



/***************************************/

//函数功能：观测器初始化

//输入参数：无

//返回值：        无

//备注： 

/***************************************/

void flux_observer_init(void)

{

    Foc_observer.Rs = MOTOR_RS;

    Foc_observer.Ls = MOTOR_LS;

    Foc_observer.Flux = MOTOR_FLUX; 

    

    Foc_observer.PLL_kp = 0.27f;

    Foc_observer.PLL_ki = 0.028f;

    Foc_observer.PLL_Err = 0.0f;

    Foc_observer.PLL_Interg = 0.0f;

    Foc_observer.PLL_Ui = 0.0f;

    

    Foc_observer.Ctrl_ts = 0.0001f; 

    Foc_observer.Gain = 20000.0f;

    Foc_observer.Err = 0.0f;

    Foc_observer.Theta = 0.0f;

    Foc_observer.speed_hz = 0.0f;

    

    flux_in_wb[0] = 0.0f;

    flux_in_wb[1] = 0.0f;

    

    fluxS_in_wb[0] = 0.0f;

    fluxS_in_wb[1] = 0.0f;

    

    fluxR_in_wb[0] = 0.0f;

    fluxR_in_wb[1] = 0.0f;

    

    speed_calc_cnt = 0;

    speed_acc = 0.0f;

    speed_now = 0.0f;

}



/***************************************/

//函数功能：  磁链观测器

//输入参数：无

//返回值：        无

//备注： 

/***************************************/

void flux_observer(void)

{

    float VoltRs[2];

    float VoltdPhi[2];

    

    float g_fluxfluxR = 0.0f;

    float sin_theta = 0.0f;

    float cos_theta = 0.0f;

    

    //calc alpha axis flux

    VoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha;

    VoltdPhi[0] = Foc_calc.Valpha  - VoltRs[0];

    VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain;

    flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts;

    

    //calc beta axis flux 

    VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta;

    VoltdPhi[1] = Foc_calc.Vbeta  - VoltRs[1];

    VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain;

    flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts;

    

    //calc flux in stator 

    fluxS_in_wb[0] = Foc_observer.Ls * Foc_calc.Ialpha;

    fluxS_in_wb[1] = Foc_observer.Ls * Foc_calc.Ibeta;

    

    //calc flux in rotor

    fluxR_in_wb[0] = flux_in_wb[0] - fluxS_in_wb[0];

    fluxR_in_wb[1] = flux_in_wb[1] - fluxS_in_wb[1];

    

    g_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1];



    Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR;

    

    sin_theta = arm_sin_f32(Foc_observer.Theta);

    cos_theta = arm_cos_f32(Foc_observer.Theta);        



    Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta;

    Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki;        

    Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg;   

    

    Foc_observer.Theta += Foc_observer.PLL_Ui;

    

    if (Foc_observer.Theta < 0.0f)

    {

        Foc_observer.Theta += MATH_2PI;  

    }

    else if (Foc_observer.Theta > MATH_2PI)

    {

        Foc_observer.Theta -= MATH_2PI;  

    }

    

    if (speed_calc_cnt < 10)

    {

        speed_acc += Foc_observer.PLL_Ui;

        speed_calc_cnt++;

    }

    else

    {

        speed_now = speed_acc / (0.001f * MATH_2PI);

        Foc_observer.speed_hz = Foc_observer.speed_hz * 0.99f + speed_now * 0.01f;

        

        speed_acc = 0.0f;

        speed_calc_cnt = 0;

    }

}

这段代码实现了基于磁场定向控制（FOC）的电机磁通观测器，主要功能包括：1. **计算α轴和β轴的磁通**：通过电压、电阻和电流计算α轴和β轴的磁通。2. **计算定子和转子磁通**：根据电流和电感计算定子磁通，并从总磁通中减去定子磁通得到转子磁通。3. **计算磁通误差和PLL误差**：用于调整角度估计。4. **更新角度和速度**：根据PLL误差更新角度，并计算当前电机速度。flowchart TD A[开始] --> B{计算α轴磁通} B --> C{计算β轴磁通} C --> D{计算定子磁通} D --> E{计算转子磁通} E --> F{计算磁通误差} F --> G{计算PLL误差} G --> H{更新角度} H --> I{检查角度范围} I -->|角度小于0| J[角度加2π] I -->|角度大于2π| K[角度减2π] I -->|否则| L{计算速度} L -->|计数小于10| M[累加PLL输出] L -->|计数等于10| N[计算当前速度] N --> O[更新速度计数] O --> P[结束]```### 解释- **开始**：函数入口。- **计算α轴磁通** 和 **计算β轴磁通**：分别计算α轴和β轴的磁通。- **计算定子磁通**：根据电流和电感计算定子磁通。- **计算转子磁通**：从总磁通中减去定子磁通得到转子磁通。- **计算磁通误差**：计算磁通误差用于后续调整。- **计算PLL误差**：计算PLL误差用于角度更新。- **更新角度**：根据PLL误差更新角度。- **检查角度范围**：确保角度在合理范围内，必要时进行修正。- **计算速度**：根据PLL输出计算当前速度。- **累加PLL输出**：当计数小于10时，累加PLL输出。- **计算当前速度**：当计数等于10时，计算当前速度并重置计数。- **更新速度计数**：更新速度计数以便下一次循环。- **结束**：函数结束。这个流程图展示了代码的主要逻辑步骤，帮助理解代码的执行顺序和条件分支。

### 函数功能解释这段代码是 `flux_observer` 函数的核心部分，主要实现了基于磁场定向控制（FOC）的电机磁通观测器的计算逻辑。以下是详细的功能解释：#### 1. 变量声明与初始化```cfloat VoltRs[2];float VoltdPhi[2];float g_fluxfluxR = 0.0f;float sin_theta = 0.0f;float cos_theta = 0.0f;```- **VoltRs**：存储α轴和β轴的电阻压降。- **VoltdPhi**：存储α轴和β轴的磁通变化。- **g_fluxfluxR**：用于存储转子磁通的平方和。- **sin_theta** 和 **cos_theta**：用于存储角度的正弦和余弦值。#### 2. 计算α轴磁通```c//calc alpha axis fluxVoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha;VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0];VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain;flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts;```- **计算电阻压降**：`VoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha`，其中 `Foc_observer.Rs` 是电机电阻，`Foc_calc.Ialpha` 是α轴电流。- **计算磁通变化**：`VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0]`，即α轴电压减去电阻压降。- **添加误差修正**：`VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。- **更新总磁通**：`flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts`，根据时间步长更新α轴磁通。#### 3. 计算β轴磁通```c//calc beta axis flux VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta;VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1];VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain;flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts;```- **计算电阻压降**：`VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta`，其中 `Foc_calc.Ibeta` 是β轴电流。- **计算磁通变化**：`VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1]`，即β轴电压减去电阻压降。- **添加误差修正**：`VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。- **更新总磁通**：`flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts`，根据时间步长更新β轴磁通。#### 4. 计算定子磁通```c//calc flux in stator fluxS_in_wb[0] = Foc_observer.Ls * Foc_calc.Ialpha;fluxS_in_wb[1] = Foc_observer.Ls * Foc_calc.Ibeta;```- **计算定子磁通**：根据电感 (`Foc_observer.Ls`) 和电流 (`Foc_calc.Ialpha`, `Foc_calc.Ibeta`) 计算定子磁通。#### 5. 计算转子磁通```c//calc flux in rotorfluxR_in_wb[0] = flux_in_wb[0] - fluxS_in_wb[0];fluxR_in_wb[1] = flux_in_wb[1] - fluxS_in_wb[1];```- **计算转子磁通**：从总磁通中减去定子磁通得到转子磁通。#### 6. 计算磁通误差```cg_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1];Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR;```- **计算转子磁通的平方和**：`g_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1]`。- **更新磁通误差**：`Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR`，即期望磁通的平方减去实际磁通的平方和。#### 7. 计算PLL误差```csin_theta = arm_sin_f32(Foc_observer.Theta);cos_theta = arm_cos_f32(Foc_observer.Theta); Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta;Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki;       Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg; ```- **计算正弦和余弦值**：使用 `arm_sin_f32` 和 `arm_cos_f32` 计算当前角度的正弦和余弦。- **计算PLL误差**：`Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta`。- **更新PLL积分项**：`Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki`。- **计算PLL输出**：`Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg`。#### 8. 更新角度```cFoc_observer.Theta += Foc_observer.PLL_Ui;if (Foc_observer.Theta < 0.0f){ Foc_observer.Theta += MATH_2PI;  }else if (Foc_observer.Theta > MATH_2PI){ Foc_observer.Theta -= MATH_2PI;  }```- **更新角度**：根据PLL输出更新角度。- **确保角度在合理范围内**：如果角度小于0，则加2π；如果角度大于2π，则减2π。#### 9. 计算速度```cif (speed_calc_cnt < 10){ speed_acc += Foc_observer.PLL_Ui; speed_calc_cnt++;}else{ speed_now = speed_acc / (0.001f * MATH_2PI); Foc_observer.speed_hz = Foc_observer.speed_hz * 0.99f + speed_now * 0.01f;       speed_acc = 0.0f; speed_calc_cnt = 0;}```- **累加PLL输出**：当计数小于10时，累加PLL输出。- **计算当前速度**：当计数等于10时，计算平均速度并更新速度估计。- **重置计数**：重置速度累加器和计数器以便下一次循环。### 总结这段代码通过一系列计算，实现了对电机α轴和β轴磁通的观测，并根据观测结果调整角度和速度。具体步骤包括计算电阻压降、磁通变化、定子和转子磁通、磁通误差、PLL误差，以及更新角度和速度。### 函数功能解释这段代码是 `flux_observer` 函数的核心部分，主要实现了基于磁场定向控制（FOC）的电机磁通观测器的计算逻辑。以下是详细的功能解释：#### 1. 变量声明与初始化```cfloat VoltRs[2];float VoltdPhi[2];float g_fluxfluxR = 0.0f;float sin_theta = 0.0f;float cos_theta = 0.0f;```- **VoltRs**：存储α轴和β轴的电阻压降。- **VoltdPhi**：存储α轴和β轴的磁通变化。- **g_fluxfluxR**：用于存储转子磁通的平方和。- **sin_theta** 和 **cos_theta**：用于存储角度的正弦和余弦值。#### 2. 计算α轴磁通```c//calc alpha axis fluxVoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha;VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0];VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain;flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts;```- **计算电阻压降**：`VoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha`，其中 `Foc_observer.Rs` 是电机电阻，`Foc_calc.Ialpha` 是α轴电流。- **计算磁通变化**：`VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0]`，即α轴电压减去电阻压降。- **添加误差修正**：`VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。- **更新总磁通**：`flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts`，根据时间步长更新α轴磁通。#### 3. 计算β轴磁通```c//calc beta axis flux VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta;VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1];VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain;flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts;```- **计算电阻压降**：`VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta`，其中 `Foc_calc.Ibeta` 是β轴电流。- **计算磁通变化**：`VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1]`，即β轴电压减去电阻压降。- **添加误差修正**：`VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。- **更新总磁通**：`flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts`，根据时间步长更新β轴磁通。#### 4. 计算定子磁通```c//calc flux in stator fluxS_in_wb[0] = Foc_observer.Ls * Foc_calc.Ialpha;fluxS_in_wb[1] = Foc_observer.Ls * Foc_calc.Ibeta;```- **计算定子磁通**：根据电感 (`Foc_observer.Ls`) 和电流 (`Foc_calc.Ialpha`, `Foc_calc.Ibeta`) 计算定子磁通。#### 5. 计算转子磁通```c//calc flux in rotorfluxR_in_wb[0] = flux_in_wb[0] - fluxS_in_wb[0];fluxR_in_wb[1] = flux_in_wb[1] - fluxS_in_wb[1];```- **计算转子磁通**：从总磁通中减去定子磁通得到转子磁通。#### 6. 计算磁通误差```cg_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1];Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR;```- **计算转子磁通的平方和**：`g_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1]`。- **更新磁通误差**：`Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR`，即期望磁通的平方减去实际磁通的平方和。#### 7. 计算PLL误差```csin_theta = arm_sin_f32(Foc_observer.Theta);cos_theta = arm_cos_f32(Foc_observer.Theta); Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta;Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki;       Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg; ```- **计算正弦和余弦值**：使用 `arm_sin_f32` 和 `arm_cos_f32` 计算当前角度的正弦和余弦。- **计算PLL误差**：`Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta`。- **更新PLL积分项**：`Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki`。- **计算PLL输出**：`Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg`。#### 8. 更新角度```cFoc_observer.Theta += Foc_observer.PLL_Ui;if (Foc_observer.Theta < 0.0f){ Foc_observer.Theta += MATH_2PI;  }else if (Foc_observer.Theta > MATH_2PI){ Foc_observer.Theta -= MATH_2PI;  }```- **更新角度**：根据PLL输出更新角度。- **确保角度在合理范围内**：如果角度小于0，则加2π；如果角度大于2π，则减2π。#### 9. 计算速度```cif (speed_calc_cnt < 10){ speed_acc += Foc_observer.PLL_Ui; speed_calc_cnt++;}else{ speed_now = speed_acc / (0.001f * MATH_2PI); Foc_observer.speed_hz = Foc_observer.speed_hz * 0.99f + speed_now * 0.01f;       speed_acc = 0.0f; speed_calc_cnt = 0;}```- **累加PLL输出**：当计数小于10时，累加PLL输出。- **计算当前速度**：当计数等于10时，计算平均速度并更新速度估计。- **重置计数**：重置速度累加器和计数器以便下一次循环。### 总结这段代码通过一系列计算，实现了对电机α轴和β轴磁通的观测，并根据观测结果调整角度和速度。具体步骤包括计算电阻压降、磁通变化、定子和转子磁通、磁通误差、PLL误差，以及更新角度和速度。
### 函数功能解释

flux_observer函数的核心部分，主要实现了基于磁场定向控制（FOC）的电机磁通观测器的计算逻辑。以下是详细的功能解释：

#### 1. 变量声明与初始化
float VoltRs[2];
float VoltdPhi[2];

float g_fluxfluxR = 0.0f;
float sin_theta = 0.0f;
float cos_theta = 0.0f;
```
- **VoltRs**：存储α轴和β轴的电阻压降。
- **VoltdPhi**：存储α轴和β轴的磁通变化。
- **g_fluxfluxR**：用于存储转子磁通的平方和。
- **sin_theta** 和 **cos_theta**：用于存储角度的正弦和余弦值。

#### 2. 计算α轴磁通
//calc alpha axis flux
VoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha;
VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0];
VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain;
flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts;
```
- **计算电阻压降**：`VoltRs[0] = Foc_observer.Rs * Foc_calc.Ialpha`，其中 `Foc_observer.Rs` 是电机电阻，`Foc_calc.Ialpha` 是α轴电流。
- **计算磁通变化**：`VoltdPhi[0] = Foc_calc.Valpha - VoltRs[0]`，即α轴电压减去电阻压降。
- **添加误差修正**：`VoltdPhi[0] += fluxR_in_wb[0] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。
- **更新总磁通**：`flux_in_wb[0] += VoltdPhi[0] * Foc_observer.Ctrl_ts`，根据时间步长更新α轴磁通。

#### 3. 计算β轴磁通
//calc beta axis flux
VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta;
VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1];
VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain;
flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts;
```
- **计算电阻压降**：`VoltRs[1] = Foc_observer.Rs * Foc_calc.Ibeta`，其中 `Foc_calc.Ibeta` 是β轴电流。
- **计算磁通变化**：`VoltdPhi[1] = Foc_calc.Vbeta - VoltRs[1]`，即β轴电压减去电阻压降。
- **添加误差修正**：`VoltdPhi[1] += fluxR_in_wb[1] * Foc_observer.Err * Foc_observer.Gain`，根据转子磁通误差进行修正。
- **更新总磁通**：`flux_in_wb[1] += VoltdPhi[1] * Foc_observer.Ctrl_ts`，根据时间步长更新β轴磁通。

#### 4. 计算定子磁通
//calc flux in stator
fluxS_in_wb[0] = Foc_observer.Ls * Foc_calc.Ialpha;
fluxS_in_wb[1] = Foc_observer.Ls * Foc_calc.Ibeta;
```
- **计算定子磁通**：根据电感 (`Foc_observer.Ls`) 和电流 (`Foc_calc.Ialpha`, `Foc_calc.Ibeta`) 计算定子磁通。

#### 5. 计算转子磁通
//calc flux in rotor
fluxR_in_wb[0] = flux_in_wb[0] - fluxS_in_wb[0];
fluxR_in_wb[1] = flux_in_wb[1] - fluxS_in_wb[1];
```
- **计算转子磁通**：从总磁通中减去定子磁通得到转子磁通。

#### 6. 计算磁通误差
g_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1];
Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR;
```
- **计算转子磁通的平方和**：`g_fluxfluxR = fluxR_in_wb[0] * fluxR_in_wb[0] + fluxR_in_wb[1] * fluxR_in_wb[1]`。
- **更新磁通误差**：`Foc_observer.Err = Foc_observer.Flux * Foc_observer.Flux - g_fluxfluxR`，即期望磁通的平方减去实际磁通的平方和。

#### 7. 计算PLL误差
sin_theta = arm_sin_f32(Foc_observer.Theta);
cos_theta = arm_cos_f32(Foc_observer.Theta);

Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta;
Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki;
Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg;
```
- **计算正弦和余弦值**：使用 `arm_sin_f32` 和 `arm_cos_f32` 计算当前角度的正弦和余弦。
- **计算PLL误差**：`Foc_observer.PLL_Err = fluxR_in_wb[1] * cos_theta - fluxR_in_wb[0] * sin_theta`。
- **更新PLL积分项**：`Foc_observer.PLL_Interg += Foc_observer.PLL_Err * Foc_observer.PLL_ki`。
- **计算PLL输出**：`Foc_observer.PLL_Ui = Foc_observer.PLL_Err * Foc_observer.PLL_kp + Foc_observer.PLL_Interg`。

#### 8. 更新角度
Foc_observer.Theta += Foc_observer.PLL_Ui;

if (Foc_observer.Theta < 0.0f)
{
Foc_observer.Theta += MATH_2PI;
}
else if (Foc_observer.Theta > MATH_2PI)
{
Foc_observer.Theta -= MATH_2PI;
}
```
- **更新角度**：根据PLL输出更新角度。
- **确保角度在合理范围内**：如果角度小于0，则加2π；如果角度大于2π，则减2π。

#### 9. 计算速度
if (speed_calc_cnt < 10)
{
speed_acc += Foc_observer.PLL_Ui;
speed_calc_cnt++;
}
else
{
speed_now = speed_acc / (0.001f * MATH_2PI);
Foc_observer.speed_hz = Foc_observer.speed_hz * 0.99f + speed_now * 0.01f;

speed_acc = 0.0f;
speed_calc_cnt = 0;
}
```
- **累加PLL输出**：当计数小于10时，累加PLL输出。
- **计算当前速度**：当计数等于10时，计算平均速度并更新速度估计。
- **重置计数**：重置速度累加器和计数器以便下一次循环。

这段代码通过一系列计算，实现了对电机α轴和β轴磁通的观测，并根据观测结果调整角度和速度。具体步骤包括计算电阻压降、磁通变化、定子和转子磁通、磁通误差、PLL误差，以及更新角度和速度。

基于磁链观测器无感foc的代码解读

技术新星奖章