function [x1,y1,z1] = func_TOA(BS,time_dur,light_speed);

L = time_dur .* light_speed;
n = length(BS);

for i = 2:n
    P(i-1,1) = BS(i,1);
    P(i-1,2) = BS(i,2);
    P(i-1,3) = BS(i,3);
end

B(1,:) = (BS(2,1)^2 + BS(2,2)^2 + BS(2,3)^2) - L(2)^2 + L(1)^2;
B(2,:) = (BS(3,1)^2 + BS(3,2)^2 + BS(3,3)^2) - L(3)^2 + L(1)^2;
B(3,:) = (BS(4,1)^2 + BS(4,2)^2 + BS(4,3)^2) - L(4)^2 + L(1)^2;
 

k     = inv(P'*P)*P'*B/2;
 
x1    = k(1);
y1    = k(2);

z1    = k(3);

————————————————
 module position_function(
                         i_clk,
                                 i_rst,
                         time_dur1,
                                 time_dur2,
                                 time_dur3,
                                 time_dur4,
                                 px,
                                 py,
                                 pz,
                                 //test
                                 B1,
                                 B2,
                                 B3,
                                 X1,
                                 X2,
                                 X3,                                 
                                 Y1,
                                 Y2,
                                 Y3,                                 
                                 Z1,
                                 Z2,
                                 Z3                             
                                 
                          );
//根据实际BS的位置需要调整这几个参数
parameter BS0x = 32'd0;
parameter BS0y = 32'd0;
parameter BS0z = 32'd0;

parameter BS1x = 32'd500;
parameter BS1y = 32'd500;
parameter BS1z = 32'd0;

parameter BS2x = 32'd0;
parameter BS2y = 32'd500;
parameter BS2z = 32'd300;

parameter BS3x = 32'd500;
parameter BS3y = 32'd0;
parameter BS3z = 32'd300;

input i_clk;
input i_rst;
input signed[31:0]time_dur1;
input signed[31:0]time_dur2;
input signed[31:0]time_dur3;
input signed[31:0]time_dur4;
output signed[31:0]px;
output signed[31:0]py;
output signed[31:0]pz;
//test
output signed[31:0]B1;
output signed[31:0]B2;
output signed[31:0]B3;

output signed[63:0]X1;
output signed[63:0]X2;
output signed[63:0]X3;
output signed[63:0]Y1;
output signed[63:0]Y2;
output signed[63:0]Y3;
output signed[63:0]Z1;
output signed[63:0]Z2;
output signed[63:0]Z3;

//L = time_dur .* light_speed;
wire signed[63:0]L1tmp;
wire signed[63:0]L2tmp;
wire signed[63:0]L3tmp;
wire signed[63:0]L4tmp;

wire signed[31:0]L1;
wire signed[31:0]L2;
wire signed[31:0]L3;
wire signed[31:0]L4;
multi_tc multi_tc_u1(
  .clk (i_clk), 
  .a   (time_dur1), 
  .b   (32'd300000000), //light speed
  .sclr(i_rst),
  .p   (L1tmp) 
);

multi_tc multi_tc_u2(
  .clk (i_clk), 
  .a   (time_dur2), 
  .b   (32'd300000000), //light speed
  .sclr(i_rst),
  .p   (L2tmp) 
);

multi_tc multi_tc_u3(
  .clk (i_clk), 
  .a   (time_dur3), 
  .b   (32'd300000000), //light speed
  .sclr(i_rst),
  .p   (L3tmp) 
);

multi_tc multi_tc_u4(
  .clk (i_clk), 
  .a   (time_dur4), 
  .b   (32'd300000000), //light speed
  .sclr(i_rst),
  .p   (L4tmp) 
);
//L=[L1,L2,L3,L4]
assign L1 = L1tmp[63:32];
assign L2 = L2tmp[63:32];
assign L3 = L3tmp[63:32];
assign L4 = L4tmp[63:32];


reg signed[31:0]P1x_d1;
reg signed[31:0]P1y_d1;
reg signed[31:0]P1z_d1;
reg signed[31:0]P2x_d1;
reg signed[31:0]P2y_d1;
reg signed[31:0]P2z_d1;
reg signed[31:0]P3x_d1;
reg signed[31:0]P3y_d1;
reg signed[31:0]P3z_d1;


reg signed[31:0]P1x_d2;
reg signed[31:0]P1y_d2;
reg signed[31:0]P1z_d2;
reg signed[31:0]P2x_d2;
reg signed[31:0]P2y_d2;
reg signed[31:0]P2z_d2;
reg signed[31:0]P3x_d2;
reg signed[31:0]P3y_d2;
reg signed[31:0]P3z_d2;

reg signed[31:0]P1x_d3;
reg signed[31:0]P1y_d3;
reg signed[31:0]P1z_d3;
reg signed[31:0]P2x_d3;
reg signed[31:0]P2y_d3;
reg signed[31:0]P2z_d3;
reg signed[31:0]P3x_d3;
reg signed[31:0]P3y_d3;
reg signed[31:0]P3z_d3;

reg signed[31:0]P1x;
reg signed[31:0]P1y;
reg signed[31:0]P1z;
reg signed[31:0]P2x;
reg signed[31:0]P2y;
reg signed[31:0]P2z;
reg signed[31:0]P3x;
reg signed[31:0]P3y;
reg signed[31:0]P3z;

//for i = 2:n
//    P(i-1,1) = BS(i,1);
//    P(i-1,2) = BS(i,2);
//    P(i-1,3) = BS(i,3);
//end
//在matlab中计算完了直接用到FPGA中
//inv(P'*P)*P'//在matlab中计算完了直接用到FPGA中
//   1.0e+06 *
//   4.2950   -4.2950    4.2950
//   4.2950    4.2950   -4.2950
//  -7.1583    7.1583    7.1583

always @(posedge i_clk or posedge i_rst)
begin
     if(i_rst)
      begin
      P1x_d1 <= 32'd0;
      P1y_d1 <= 32'd0;
      P1z_d1 <= 32'd0;
      P2x_d1 <= 32'd0;
      P2y_d1 <= 32'd0;
      P2z_d1 <= 32'd0;
      P3x_d1 <= 32'd0;
      P3y_d1 <= 32'd0;
      P3z_d1 <= 32'd0;
      
      P1x_d2 <= 32'd0;
      P1y_d2 <= 32'd0;
      P1z_d2 <= 32'd0;
      P2x_d2 <= 32'd0;
      P2y_d2 <= 32'd0;
      P2z_d2 <= 32'd0;
      P3x_d2 <= 32'd0;
      P3y_d2 <= 32'd0;
      P3z_d2 <= 32'd0;      
      
      P1x_d3 <= 32'd0;
      P1y_d3 <= 32'd0;
      P1z_d3 <= 32'd0;
      P2x_d3 <= 32'd0;
      P2y_d3 <= 32'd0;
      P2z_d3 <= 32'd0;
      P3x_d3 <= 32'd0;
      P3y_d3 <= 32'd0;
      P3z_d3 <= 32'd0;
      
      P1x <= 32'd0;
      P1y <= 32'd0;
      P1z <= 32'd0;
      P2x <= 32'd0;
      P2y <= 32'd0;
      P2z <= 32'd0;
      P3x <= 32'd0;
      P3y <= 32'd0;
      P3z <= 32'd0;
      end
else begin
      P1x_d3 <= 32'd4295000;
      P1y_d3 <= 32'd4295000;
      P1z_d3 <= 32'd4295000;    
      P2x_d3 <= 32'd4295000;
      P2y_d3 <= 32'd4295000;
      P2z_d3 <= 32'd4295000;
      P3x_d3 <= 32'd7158300;
      P3y_d3 <= 32'd7158300;
      P3z_d3 <= 32'd7158300;
      
      P1x_d2 <= P1x_d3;
      P1y_d2 <= P1y_d3;
      P1z_d2 <= P1z_d3;
      P2x_d2 <= P2x_d3;
      P2y_d2 <= P2y_d3;
      P2z_d2 <= P2z_d3;
      P3x_d2 <= P3x_d3;
      P3y_d2 <= P3y_d3;
      P3z_d2 <= P3z_d3;      
      
      P1x_d1 <= P1x_d2;
      P1y_d1 <= P1y_d2;
      P1z_d1 <= P1z_d2;
      P2x_d1 <= P2x_d2;
      P2y_d1 <= P2y_d2;
      P2z_d1 <= P2z_d2;
      P3x_d1 <= P3x_d2;
      P3y_d1 <= P3y_d2;
      P3z_d1 <= P3z_d2;           
      
      P1x <= P1x_d1;
      P1y <= P1y_d1;
      P1z <= P1z_d1;
      P2x <= P2x_d1;
      P2y <= P2y_d1;
      P2z <= P2z_d1;
      P3x <= P3x_d1;
      P3y <= P3y_d1;
      P3z <= P3z_d1;          
     end
end
 

//parameter BS1x = 32'd500;
//parameter BS1y = 32'd500;
//parameter BS1z = 32'd0;
//
//parameter BS2x = 32'd0;
//parameter BS2y = 32'd500;
//parameter BS2z = 32'd300;
//
//parameter BS3x = 32'd500;
//parameter BS3y = 32'd0;
//parameter BS3z = 32'd300; 


//B(1,:) = (BS(2,1)^2 + BS(2,2)^2 + BS(2,3)^2) - L(2)^2 + L(1)^2;
wire signed[63:0]BS1x2;
wire signed[63:0]BS1y2;
wire signed[63:0]BS1z2;
wire signed[63:0]L22;
wire signed[63:0]L12;

multi_tc multi_tc_u11(
  .clk (i_clk), 
  .a   (BS1x), 
  .b   (BS1x), //light speed
  .sclr(i_rst),
  .p   (BS1x2) 
);
multi_tc multi_tc_u12(
  .clk (i_clk), 
  .a   (BS1y), 
  .b   (BS1y), //light speed
  .sclr(i_rst),
  .p   (BS1y2) 
);
multi_tc multi_tc_u13(
  .clk (i_clk), 
  .a   (BS1z), 
  .b   (BS1z), //light speed
  .sclr(i_rst),
  .p   (BS1z2) 
);
multi_tc multi_tc_u14(
  .clk (i_clk), 
  .a   (L2), 
  .b   (L2), //light speed
  .sclr(i_rst),
  .p   (L22) 
);
multi_tc multi_tc_u15(
  .clk (i_clk), 
  .a   (L1), 
  .b   (L1), //light speed
  .sclr(i_rst),
  .p   (L12) 
);

reg signed[31:0]r11;
reg signed[31:0]r12;
reg signed[31:0]B1;
always @(posedge i_clk or posedge i_rst)
begin
     if(i_rst)
      begin
      r11 <= 32'd0;
      r12 <= 32'd0;
      B1  <= 32'd0;
      end
else begin
      r11 <= BS1x2[31:0] + BS1y2[31:0] + BS1z2[31:0];
      r12 <= L12[31:0]   - L22[31:0];
      B1  <= r11 + r12;
     end
end

//B(2,:) = (BS(3,1)^2 + BS(3,2)^2 + BS(3,3)^2) - L(3)^2 + L(1)^2;
wire signed[63:0]BS2x2;
wire signed[63:0]BS2y2;
wire signed[63:0]BS2z2;
wire signed[63:0]L32;
 

multi_tc multi_tc_u21(
  .clk (i_clk), 
  .a   (BS2x), 
  .b   (BS2x), //light speed
  .sclr(i_rst),
  .p   (BS2x2) 
);
multi_tc multi_tc_u22(
  .clk (i_clk), 
  .a   (BS2y), 
  .b   (BS2y), //light speed
  .sclr(i_rst),
  .p   (BS2y2) 
);
multi_tc multi_tc_u23(
  .clk (i_clk), 
  .a   (BS2z), 
  .b   (BS2z), //light speed
  .sclr(i_rst),
  .p   (BS2z2) 
);
multi_tc multi_tc_u24(
  .clk (i_clk), 
  .a   (L3), 
  .b   (L3), //light speed
  .sclr(i_rst),
  .p   (L32) 
);
 
reg signed[31:0]r21;
reg signed[31:0]r22;
reg signed[31:0]B2;
always @(posedge i_clk or posedge i_rst)
begin
     if(i_rst)
      begin
      r21 <= 32'd0;
      r22 <= 32'd0;
      B2  <= 32'd0;
      end
else begin
      r21 <= BS2x2[31:0] + BS2y2[31:0] + BS2z2[31:0];
      r22 <= L12[31:0]   - L32[31:0];
      B2  <= r21 + r22;
     end
end


//B(3,:) = (BS(4,1)^2 + BS(4,2)^2 + BS(4,3)^2) - L(4)^2 + L(1)^2;
wire signed[63:0]BS3x2;
wire signed[63:0]BS3y2;
wire signed[63:0]BS3z2;
wire signed[63:0]L42;
 

multi_tc multi_tc_u31(
  .clk (i_clk), 
  .a   (BS3x), 
  .b   (BS3x), //light speed
  .sclr(i_rst),
  .p   (BS3x2) 
);
multi_tc multi_tc_u32(
  .clk (i_clk), 
  .a   (BS3y), 
  .b   (BS3y), //light speed
  .sclr(i_rst),
  .p   (BS3y2) 
);
multi_tc multi_tc_u33(
  .clk (i_clk), 
  .a   (BS3z), 
  .b   (BS3z), //light speed
  .sclr(i_rst),
  .p   (BS3z2) 
);
multi_tc multi_tc_u34(
  .clk (i_clk), 
  .a   (L4), 
  .b   (L4), //light speed
  .sclr(i_rst),
  .p   (L42) 
);
 

reg signed[31:0]r31;
reg signed[31:0]r32;
reg signed[31:0]B3;
always @(posedge i_clk or posedge i_rst)
begin
     if(i_rst)
      begin
      r31 <= 32'd0;
      r32 <= 32'd0;
      B3  <= 32'd0;
      end
else begin
      r31 <= BS3x2[31:0] + BS3y2[31:0] + BS3z2[31:0];
      r32 <= L12[31:0]   - L42[31:0];
      B3  <= r31 + r32;
     end
end


//k     = inv(P'*P)*P'*B/2;

//X = P1x*B1/2 + P1y*B2/2 + P1z*B3/2;
wire signed[63:0]X1;
wire signed[63:0]X2;
wire signed[63:0]X3;
multi_tc multi_tc_uX1(
  .clk (i_clk), 
  .a   (P1x), 
  .b   ({B1[31],B1[31:1]}), //light speed
  .sclr(i_rst),
  .p   (X1) 
);

multi_tc multi_tc_uX2(
  .clk (i_clk), 
  .a   (P1y), 
  .b   ({B2[31],B2[31:1]}), //light speed
  .sclr(i_rst),
  .p   (X2) 
);

multi_tc multi_tc_uX3(
  .clk (i_clk), 
  .a   (P1z), 
  .b   ({B3[31],B3[31:1]}), //light speed
  .sclr(i_rst),
  .p   (X3) 
);

//Y = P2x*B1/2 + P2y*B2/2 + P2z*B3/2;
wire signed[63:0]Y1;
wire signed[63:0]Y2;
wire signed[63:0]Y3;
multi_tc multi_tc_uY1(
  .clk (i_clk), 
  .a   (P2x), 
  .b   ({B1[31],B1[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Y1) 
);

multi_tc multi_tc_uY2(
  .clk (i_clk), 
  .a   (P2y), 
  .b   ({B2[31],B2[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Y2) 
);

multi_tc multi_tc_uY3(
  .clk (i_clk), 
  .a   (P2z), 
  .b   ({B3[31],B3[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Y3) 
);


//Z = P3x*B1/2 + P3y*B2/2 + P3z*B3/2;

wire signed[63:0]Z1;
wire signed[63:0]Z2;
wire signed[63:0]Z3;
multi_tc multi_tc_uZ1(
  .clk (i_clk), 
  .a   (P3x), 
  .b   ({B1[31],B1[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Z1) 
);

multi_tc multi_tc_uZ2(
  .clk (i_clk), 
  .a   (P3y), 
  .b   ({B2[31],B2[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Z2) 
);

multi_tc multi_tc_uZ3(
  .clk (i_clk), 
  .a   (P3z), 
  .b   ({B3[31],B3[31:1]}), //light speed
  .sclr(i_rst),
  .p   (Z3) 
);

reg signed[31:0]px;
reg signed[31:0]py;
reg signed[31:0]pz;
always @(posedge i_clk or posedge i_rst)
begin
     if(i_rst)
      begin
      px <= 32'd0;
      py <= 32'd0;
      pz <= 32'd0;
      end
else begin
      px <= X1[63:32] - X2[63:32] + X3[63:32];
      py <= Y1[63:32] + Y2[63:32] - Y3[63:32];
      pz <= Z2[63:32] + Z3[63:32] - Z1[63:32];
     end
end

endmodule
————————————————

联系：highspeedlogic

QQ ：1224848052

微信：HuangL1121

邮箱：1224848052@qq.com

网站：http://www.mat7lab.com/

网站：http://www.hslogic.com/

微信扫一扫：