【国产FPGA 正点原子DFPGL22G开发板】五、鉴相器代码编写

Jimmy_Arareb

【国产FPGA 正点原子DFPGL22G开发板】五、鉴相器代码编写 [复制链接]

# 五、 鉴相器代码编写

## 1.鉴相器原理

在之前的帖子中实现的乘积检波器可以从旋转变压器的两路输出中提取出正余弦包络，为了实现旋转变压器作为角度传感器测量角度和角速度的功能，还需要进一步从正余弦包络中提取角度和角速度信息。

由于正余弦包络的数学表达式为角度的正余弦值，因此最直接的鉴相方法为计算正余弦包络的反正切函数得到角度，再取微分得到角速度。但实际信号中存在噪声，反正切函数对噪声非常敏感，并且微分操作会进一步放大高频噪声。反正切法仅适用于不需要获取角速度且对精度要求不高的场合。

实际工程中一般使用II型锁相环从包络中提取角度和角速度，其传递函数分别为二阶系统，可以通过选择合适的参数获得合适的带宽和阻尼比，抑制噪声的同时实现对角度和角速度的快速跟踪。

## 2.鉴相器仿真

在Simulink中搭建鉴相器模型，输入两路正余弦信号，观察输出，角度及角速度均收敛至目标值。

<ignore_js_op>

<a href="javascript:;" onclick="showWindow(this.id, this.getAttribute('url'), 'get', 0);" id="savephoto_678387" url="home.php?mod=spacecp&ac=album&op=saveforumphoto&aid=678387&handlekey=savephoto_678387">保存到相册</a>

</p>

</div>
<div class="tip_horn"></div>
</div>

</ignore_js_op>

<ignore_js_op>

<a href="javascript:;" onclick="showWindow(this.id, this.getAttribute('url'), 'get', 0);" id="savephoto_678388" url="home.php?mod=spacecp&ac=album&op=saveforumphoto&aid=678388&handlekey=savephoto_678388">保存到相册</a>

</p>

</div>
<div class="tip_horn"></div>
</div>

</ignore_js_op>

> 鉴相器Simulink仿真模型

在鉴相器内部，利用正余弦关系$$ sin\theta cos\hat{\theta}-cos\theta sin\hat{\theta}=sin(\theta -\hat{\theta})\approx \theta -\hat{\theta} $$小角度下近似得到角度跟踪误差，送入PI控制器中。仿真中控制器为离散形式，系统为II型系统，一般取阻尼比为0.707，带宽取系统最大带宽的5-10倍左右，整定锁相环参数。

<ignore_js_op>

<a href="javascript:;" onclick="showWindow(this.id, this.getAttribute('url'), 'get', 0);" id="savephoto_678389" url="home.php?mod=spacecp&ac=album&op=saveforumphoto&aid=678389&handlekey=savephoto_678389">保存到相册</a>

</p>

</div>
<div class="tip_horn"></div>
</div>

</ignore_js_op>

> Simulink仿真结果

跟踪角度及角速度在第一圈以内收敛到实际值，且无稳态误差。

## 3.鉴相器代码

将上述模型定点化后，使用HDL Coder输出代码并做修改，代码如下：

```verilog
// -------------------------------------------------------------
// 
// File Name: hdl_prj\hdlsrc\pll_hdl_fixed\PLL.v
// Created: 2023-02-12 17:26:10
// 
// Generated by MATLAB 9.13 and HDL Coder 4.0
// 
// 
// -- -------------------------------------------------------------
// -- Rate and Clocking Details
// -- -------------------------------------------------------------
// Model base rate: 5e-06
// Target subsystem base rate: 5e-06
// 
// -------------------------------------------------------------

// -------------------------------------------------------------
// 
// Module: PLL
// Source Path: pll_hdl_fixed/PLL
// Hierarchy Level: 0
// 
// -------------------------------------------------------------

`timescale 1 ns / 1 ns

module PLL (
  clk,
  rst,
  sin_in,
  cos_in,
  theta,
  omega,
  update
);

input clk;
  input rst;
  input update;
  input signed [15:0] sin_in;// sfix16_En15
  input signed [15:0] cos_in;// sfix16_En15
  output signed [31:0] theta;// sfix32_En32
  output signed [31:0] omega;// sfix32_En21

reg signed[15:0] epsilon;// sfix16_En15
  reg signed[31:0] p_e_kt;// sfix32_En21
  wire signed [31:0] p_e_kt_p;// sfix32_En21
  reg signed[31:0] Discrete_Time_Integrator1_x_reg;// sfix32_En32
  reg signed[16:0] sin_theta;// sfix17_En15
  wire signed [16:0] sin_theta_p;// sfix17_En15
  wire signed [32:0] Product1_mul_temp;// sfix33_En30
  reg signed[15:0] p_cos_sin;// sfix16_En15
  wire signed [15:0] p_cos_sin_p;// sfix16_En15
  reg signed[16:0] cos_theta;// sfix17_En15
  wire signed [16:0] cos_theta_p;// sfix17_En15
  wire signed [32:0] Product_mul_temp;// sfix33_En30
  reg signed[15:0] p_sin_cos;// sfix16_En15
  wire signed [15:0] p_sin_cos_p;// sfix16_En15
  wire signed [15:0] Discrete_Time_Integrator_indtc;// sfix16
  wire signed [31:0] gain_mul_temp;// sfix32_En8
  wire signed [31:0] Discrete_Time_Integrator_u_gain;// sfix32
  wire signed [31:0] Discrete_Time_Integrator_u_dtc;// sfix32_En21
  wire signed [31:0] omega_1;// sfix32_En21
  wire signed [31:0] Discrete_Time_Integrator_u_add;// sfix32_En21
  reg signed[31:0] Discrete_Time_Integrator_x_reg;// sfix32_En21
  reg signed[31:0] s_o_p;// sfix32_En21
  wire signed [31:0] Discrete_Time_Integrator1_indtc;// sfix32
  wire signed [63:0] gain1_mul_temp;// sfix64_En35
  wire signed [31:0] Discrete_Time_Integrator1_u_gain;// sfix32
  wire signed [31:0] Discrete_Time_Integrator1_u_dtc;// sfix32_En32
  wire signed [31:0] theta_1;// sfix32_En32
  wire signed [31:0] Discrete_Time_Integrator1_u_add;// sfix32_En32

reg   [ 7:0] cnt = 0;

Sine_HDL_Optimized u_Sine_HDL_Optimized (
  .u(Discrete_Time_Integrator1_x_reg),// sfix32_En32
  .x(sin_theta_p)       // sfix17_En15
  );
  assign Product1_mul_temp = cos_in * sin_theta;
  assign p_cos_sin_p   = Product1_mul_temp[30:15];

Cosine_HDL_Optimized u_Cosine_HDL_Optimized (
  .u(Discrete_Time_Integrator1_x_reg),// sfix32_En32
  .x(cos_theta_p)       // sfix17_En15
  );
  assign Product_mul_temp = cos_theta * sin_in;
  assign p_sin_cos_p  = Product_mul_temp[30:15];

always @(posedge clk) begin
  if (~rst) begin
    cnt <= 0;
  end else begin
    if (update) begin
      cnt <= 0;
    end else begin
      if (cnt == 8'd255) begin
      cnt <= cnt;
      end else begin
      cnt <= cnt + 1;
      end
    end
  end
  end

always @(posedge clk) begin
  if (~rst) begin
    sin_theta       <= 0;
    cos_theta       <= 0;
    p_sin_cos       <= 0;
    p_cos_sin       <= 0;
    epsilon         <= 0;
    Discrete_Time_Integrator_x_reg<= 0;
    Discrete_Time_Integrator1_x_reg <= 0;
  end
  case (cnt)
    8'd0: begin
      sin_theta <= sin_theta_p;
      cos_theta <= cos_theta_p;
    end
    8'd2: begin
      p_sin_cos <= p_sin_cos_p;
      p_cos_sin <= p_cos_sin_p;
    end
    8'd4: begin
      epsilon <= p_sin_cos - p_cos_sin;
    end
    8'd6: begin
      Discrete_Time_Integrator_x_reg <= Discrete_Time_Integrator_u_add;
      p_e_kt         <= p_e_kt_p;
    end
    8'd7: begin
      s_o_p <= omega_1 + p_e_kt;
    end
    8'd8: begin
      Discrete_Time_Integrator1_x_reg <= Discrete_Time_Integrator1_u_add;
    end
    default: begin
      sin_theta       <= sin_theta;
      cos_theta       <= cos_theta;
      p_sin_cos       <= p_sin_cos;
      p_cos_sin       <= p_cos_sin;
      epsilon         <= epsilon;
      Discrete_Time_Integrator_x_reg<= Discrete_Time_Integrator_x_reg;
      Discrete_Time_Integrator1_x_reg <= Discrete_Time_Integrator1_x_reg;
    end
  endcase
  end

assign p_e_kt_p         = 16'sb0100011010110011 * epsilon;

assign Discrete_Time_Integrator_indtc = epsilon;

assign gain_mul_temp      = 16'sb0101000001101101 * Discrete_Time_Integrator_indtc;
  assign Discrete_Time_Integrator_u_gain= {{8{gain_mul_temp[31]}}, gain_mul_temp[31:8]};

assign Discrete_Time_Integrator_u_dtc = Discrete_Time_Integrator_u_gain;

assign Discrete_Time_Integrator_u_add = omega_1 + Discrete_Time_Integrator_u_dtc;

assign omega_1        = Discrete_Time_Integrator_x_reg;

assign Discrete_Time_Integrator1_indtc= s_o_p;

assign gain1_mul_temp       = 32'sb00010100111110001011010110001001 * Discrete_Time_Integrator1_indtc;
  assign Discrete_Time_Integrator1_u_gain = {{3{gain1_mul_temp[63]}}, gain1_mul_temp[63:35]};

assign Discrete_Time_Integrator1_u_dtc= Discrete_Time_Integrator1_u_gain;

assign theta_1        = Discrete_Time_Integrator1_x_reg;

assign Discrete_Time_Integrator1_u_add= theta_1 + Discrete_Time_Integrator1_u_dtc;

assign theta          = Discrete_Time_Integrator1_x_reg;

assign omega          = Discrete_Time_Integrator_x_reg;

endmodule// PLL
```

使用modelsim进行仿真，测试激励如下：

```verilog
`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2022/10/15 23:49:43
// Design Name: 
// Module Name: ad7606_4_tb
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////

module ad7606_4_tb ();

localparam N = 2000;

reg clk = 0, rst = 0;
  reg [15:0] data_sin  [N - 1:0];
  reg [15:0] data_cos  [N - 1:0];
  wire [15:0] sin_in, cos_in;

wire trigger;
  wire [31:0] theta, omega;

integer i = 0;

initial begin
  $readmemh("../../matlab/sin.txt", data_sin);
  $readmemh("../../matlab/cos.txt", data_cos);
  #50 rst <= 1;
  #40000000 $stop;
  end

always begin
  #10 clk <= ~clk;
  end

assign sin_in = data_sin<i>;
  assign cos_in = data_cos<i>;

always @(posedge trigger) begin
  if (i == N-1) begin
    i <= 0;
  end else begin
    i <= i + 1;
  end
  end

PLL u_PLL(
  .clk  (clk  ),
  .rst  (rst  ),
  .update (trigger ),
  .sin_in (sin_in ),
  .cos_in (cos_in ),
  .theta(theta),
  .omega(omega)
  );
  
  counter u_counter(
  .clk (clk ),
  .rst (rst ),
  .trigger (trigger )
  );

endmodule
```

<ignore_js_op>

<a href="javascript:;" onclick="showWindow(this.id, this.getAttribute('url'), 'get', 0);" id="savephoto_678390" url="home.php?mod=spacecp&ac=album&op=saveforumphoto&aid=678390&handlekey=savephoto_678390">保存到相册</a>

</p>

</div>
<div class="tip_horn"></div>
</div>

</ignore_js_op>

>Modelsim仿真结果

仿真结果与Simulink相同，Verilog代码正确。

Jacktang

感觉可能在Simulink中搭建鉴相器模型这个比较重要，弄不好后面的代码就控制不了

【国产FPGA 正点原子DFPGL22G开发板】五、鉴相器代码编写 [复制链接]

最新回复