| Line 18... |
Line 18... |
[15:0]mr,md; not accessable through Wishbone interface
|
[15:0]mr,md; not accessable through Wishbone interface
|
[31:0]p,b; not accessable through Wishbone interface
|
[31:0]p,b; not accessable through Wishbone interface
|
[31:0]un,sigma; read only through Wishbone interface, address: 0x20, 0x24
|
[31:0]un,sigma; read only through Wishbone interface, address: 0x20, 0x24
|
|
|
|
|
[4:0]of; overflow register, read only through Wishbone interface, address: 0x28
|
[4:0]OF; overflow register, read only through Wishbone interface, address: 0x28
|
of[0]==1 : kpd overflow
|
OF[0]==1 : kpd overflow
|
of[1]==1 : err[0] overflow
|
OF[1]==1 : err[0] overflow
|
of[2]==1 : err[1] overflow
|
OF[2]==1 : err[1] overflow
|
of[3]==1 : un overflow
|
OF[3]==1 : un overflow
|
of[4]==1 : sigma overflow
|
OF[4]==1 : sigma overflow
|
[0:15]rl; read lock, when asserted corelated reagister can not be read through Wishbone interface
|
[0:15]rl; read lock, when asserted corelated reagister can not be read through Wishbone interface
|
[0:7]wl; write lock, when asserted corelated reagister can not be written through Wishbone interface
|
[0:7]wl; write lock, when asserted corelated reagister can not be written through Wishbone interface
|
|
|
|
|
|
|
| Line 71... |
Line 71... |
kpd_adr = 5,
|
kpd_adr = 5,
|
err_0_adr = 6,
|
err_0_adr = 6,
|
err_1_adr = 7,
|
err_1_adr = 7,
|
un_adr = 8,
|
un_adr = 8,
|
sigma_adr = 9,
|
sigma_adr = 9,
|
of_adr = 10;
|
OF_adr = 10;
|
|
|
wire rst;
|
wire rst;
|
assign rst=~i_rst;
|
assign rst=~i_rst;
|
|
|
reg [15:0]kp,ki,kd,sp,pv;
|
reg [15:0]kp,ki,kd,sp,pv;
|
| Line 202... |
Line 202... |
reg rlkpd;
|
reg rlkpd;
|
reg rlerr_0;
|
reg rlerr_0;
|
reg rlerr_1;
|
reg rlerr_1;
|
reg rla;
|
reg rla;
|
reg rlsigma;
|
reg rlsigma;
|
reg rlof;
|
reg rlOF;
|
|
|
reg [4:0]of;
|
reg [4:0]OF;
|
reg [15:0]kpd;
|
reg [15:0]kpd;
|
reg [15:0]err[0:1];
|
reg [15:0]err[0:1];
|
|
|
wire [15:0]mr,md;
|
wire [15:0]mr,md;
|
|
|
| Line 226... |
Line 226... |
reg cout;
|
reg cout;
|
wire cin;
|
wire cin;
|
wire [31:0]sum;
|
wire [31:0]sum;
|
wire [31:0]product;
|
wire [31:0]product;
|
|
|
wire of_addition[0:1];
|
wire OF_addition[0:1];
|
assign of_addition[0]=(p[15]&&a[15]&&(!sum[15]))||((!p[15])&&(!a[15])&&sum[15]);
|
assign OF_addition[0]=(p[15]&&a[15]&&(!sum[15]))||((!p[15])&&(!a[15])&&sum[15]);
|
assign of_addition[1]=(p[31]&&a[31]&&(!sum[31]))||((!p[31])&&(!a[31])&&sum[31]);
|
assign OF_addition[1]=(p[31]&&a[31]&&(!sum[31]))||((!p[31])&&(!a[31])&&sum[31]);
|
|
|
|
|
|
|
reg [31:0]reg_sum;
|
reg [31:0]reg_sum;
|
reg [31:0]reg_product;
|
reg [31:0]reg_product;
|
reg reg_of_addition[0:1];
|
reg reg_OF_addition[0:1];
|
|
|
always@(posedge i_clk)begin
|
always@(posedge i_clk)begin
|
reg_sum<=sum;
|
reg_sum<=sum;
|
reg_of_addition[0]<=of_addition[0];
|
reg_OF_addition[0]<=OF_addition[0];
|
reg_of_addition[1]<=of_addition[1];
|
reg_OF_addition[1]<=OF_addition[1];
|
reg_product<=product;
|
reg_product<=product;
|
end
|
end
|
|
|
always@(posedge i_clk or negedge rst)
|
always@(posedge i_clk or negedge rst)
|
if(!rst)begin
|
if(!rst)begin
|
| Line 257... |
Line 257... |
rlkpd<=0;
|
rlkpd<=0;
|
rlerr_0<=0;
|
rlerr_0<=0;
|
rlerr_1<=0;
|
rlerr_1<=0;
|
rla<=0;
|
rla<=0;
|
rlsigma<=0;
|
rlsigma<=0;
|
rlof<=0;
|
rlOF<=0;
|
of<=0;
|
OF<=0;
|
kpd<=0;
|
kpd<=0;
|
err[0]<=0;
|
err[0]<=0;
|
err[1]<=0;
|
err[1]<=0;
|
p<=0;
|
p<=0;
|
a<=0;
|
a<=0;
|
| Line 283... |
Line 283... |
state_2<=15'b000000000000010;
|
state_2<=15'b000000000000010;
|
wlkp<=1;
|
wlkp<=1;
|
wlkd<=1;
|
wlkd<=1;
|
wlpv<=1;
|
wlpv<=1;
|
rlkpd<=1;
|
rlkpd<=1;
|
rlof<=1;
|
rlOF<=1;
|
end
|
end
|
else if(update_esu)begin
|
else if(update_esu)begin
|
state_2<=15'b00000000001000;
|
state_2<=15'b00000000001000;
|
wlkp<=1;
|
wlkp<=1;
|
wlki<=1;
|
wlki<=1;
|
| Line 297... |
Line 297... |
rlkpd<=1;
|
rlkpd<=1;
|
rlerr_0<=1;
|
rlerr_0<=1;
|
rlerr_1<=1;
|
rlerr_1<=1;
|
rla<=1;
|
rla<=1;
|
rlsigma<=1;
|
rlsigma<=1;
|
rlof<=1;
|
rlOF<=1;
|
end
|
end
|
end
|
end
|
15'b000000000000010: begin
|
15'b000000000000010: begin
|
p<={{16{kp[15]}},kp};
|
p<={{16{kp[15]}},kp};
|
a<={{16{kd[15]}},kd};
|
a<={{16{kd[15]}},kd};
|
| Line 312... |
Line 312... |
kpd<=reg_sum[15:0];
|
kpd<=reg_sum[15:0];
|
wlkp<=0;
|
wlkp<=0;
|
wlkd<=0;
|
wlkd<=0;
|
wlpv<=0;
|
wlpv<=0;
|
rlkpd<=0;
|
rlkpd<=0;
|
rlof<=0;
|
rlOF<=0;
|
of[0]<=reg_of_addition[0];
|
OF[0]<=reg_OF_addition[0];
|
state_2<=15'b000000000000001;
|
state_2<=15'b000000000000001;
|
end
|
end
|
15'b000000000001000: begin
|
15'b000000000001000: begin
|
p<={{16{~err[0][15]}},~err[0]};
|
p<={{16{~err[0][15]}},~err[0]};
|
a<={31'b0,1'b1};
|
a<={31'b0,1'b1};
|
| Line 329... |
Line 329... |
a<={{16{~pv[15]}},~pv};
|
a<={{16{~pv[15]}},~pv};
|
cout<=1;
|
cout<=1;
|
end
|
end
|
15'b000000000100000: begin
|
15'b000000000100000: begin
|
err[1]<=reg_sum[15:0];
|
err[1]<=reg_sum[15:0];
|
of[2]<=of[1];
|
OF[2]<=OF[1];
|
|
|
state_2<=15'b000000001000000;
|
state_2<=15'b000000001000000;
|
end
|
end
|
15'b000000001000000: begin
|
15'b000000001000000: begin
|
err[0]<=reg_sum[15:0];
|
err[0]<=reg_sum[15:0];
|
of[1]<=reg_of_addition[0];
|
OF[1]<=reg_OF_addition[0];
|
cout<=0;
|
cout<=0;
|
start<=1;
|
start<=1;
|
state_2<=15'b000000010000000;
|
state_2<=15'b000000010000000;
|
end
|
end
|
15'b000000010000000: begin
|
15'b000000010000000: begin
|
| Line 372... |
Line 372... |
end
|
end
|
15'b001000000000000: begin
|
15'b001000000000000: begin
|
|
|
a<=reg_product;
|
a<=reg_product;
|
sigma<=reg_sum;
|
sigma<=reg_sum;
|
of[3]<=of[4]|reg_of_addition[1];
|
OF[3]<=OF[4]|reg_OF_addition[1];
|
of[4]<=of[4]|reg_of_addition[1];
|
OF[4]<=OF[4]|reg_OF_addition[1];
|
state_1<=1;
|
state_1<=1;
|
|
|
state_2<=15'b010000000000000;
|
state_2<=15'b010000000000000;
|
|
|
end
|
end
|
15'b010000000000000: begin
|
15'b010000000000000: begin
|
a<=reg_sum; //Kpd*err0-Kd*err1
|
a<=reg_sum; //Kpd*err0-Kd*err1
|
p<=sigma;
|
p<=sigma;
|
of[3]<=of[3]|reg_of_addition[1];
|
OF[3]<=OF[3]|reg_OF_addition[1];
|
state_1<=1;
|
state_1<=1;
|
state_2<=15'b100000000000000;
|
state_2<=15'b100000000000000;
|
end
|
end
|
15'b100000000000000: begin
|
15'b100000000000000: begin
|
un<=reg_sum;
|
un<=reg_sum;
|
of[3]<=of[3]|reg_of_addition[1];
|
OF[3]<=OF[3]|reg_OF_addition[1];
|
state_2<=15'b000000000000001;
|
state_2<=15'b000000000000001;
|
wlkp<=0;
|
wlkp<=0;
|
wlki<=0;
|
wlki<=0;
|
wlkd<=0;
|
wlkd<=0;
|
wlsp<=0;
|
wlsp<=0;
|
| Line 400... |
Line 400... |
rlkpd<=0;
|
rlkpd<=0;
|
rlerr_0<=0;
|
rlerr_0<=0;
|
rlerr_1<=0;
|
rlerr_1<=0;
|
rla<=0;
|
rla<=0;
|
rlsigma<=0;
|
rlsigma<=0;
|
rlof<=0;
|
rlOF<=0;
|
end
|
end
|
endcase
|
endcase
|
end
|
end
|
endcase
|
endcase
|
end
|
end
|
| Line 440... |
Line 440... |
assign rdata[5]=kpd;
|
assign rdata[5]=kpd;
|
assign rdata[6]=err[0];
|
assign rdata[6]=err[0];
|
assign rdata[7]=err[1];
|
assign rdata[7]=err[1];
|
assign rdata[8]=un[15:0];
|
assign rdata[8]=un[15:0];
|
assign rdata[9]=sigma[15:0];
|
assign rdata[9]=sigma[15:0];
|
assign rdata[10]={11'b0,of};
|
assign rdata[10]={11'b0,OF};
|
`endif
|
`endif
|
|
|
`ifdef wb_32bit
|
`ifdef wb_32bit
|
assign rdata[0]={{16{kp[15]}},kp};
|
assign rdata[0]={{16{kp[15]}},kp};
|
assign rdata[1]={{16{ki[15]}},ki};
|
assign rdata[1]={{16{ki[15]}},ki};
|
| Line 454... |
Line 454... |
assign rdata[5]={{16{kpd[15]}},kpd};
|
assign rdata[5]={{16{kpd[15]}},kpd};
|
assign rdata[6]={{16{err[0][15]}},err[0]};
|
assign rdata[6]={{16{err[0][15]}},err[0]};
|
assign rdata[7]={{16{err[1][15]}},err[1]};
|
assign rdata[7]={{16{err[1][15]}},err[1]};
|
assign rdata[8]=un;
|
assign rdata[8]=un;
|
assign rdata[9]=sigma;
|
assign rdata[9]=sigma;
|
assign rdata[10]={27'b0,of};
|
assign rdata[10]={27'b0,OF};
|
`endif
|
`endif
|
|
|
`ifdef wb_64bit
|
`ifdef wb_64bit
|
assign rdata[0]={{48{kp[15]}},kp};
|
assign rdata[0]={{48{kp[15]}},kp};
|
assign rdata[1]={{48{ki[15]}},ki};
|
assign rdata[1]={{48{ki[15]}},ki};
|
| Line 468... |
Line 468... |
assign rdata[5]={{48{kpd[15]}},kpd};
|
assign rdata[5]={{48{kpd[15]}},kpd};
|
assign rdata[6]={{48{err[0][15]}},err[0]};
|
assign rdata[6]={{48{err[0][15]}},err[0]};
|
assign rdata[7]={{48{err[1][15]}},err[1]};
|
assign rdata[7]={{48{err[1][15]}},err[1]};
|
assign rdata[8]={{32{un[31]}},un};
|
assign rdata[8]={{32{un[31]}},un};
|
assign rdata[9]={{32{sigma[31]}},sigma};
|
assign rdata[9]={{32{sigma[31]}},sigma};
|
assign rdata[10]={59'b0,of};
|
assign rdata[10]={59'b0,OF};
|
`endif
|
`endif
|
|
|
assign rdata[11]=0;
|
assign rdata[11]=0;
|
assign rdata[12]=0;
|
assign rdata[12]=0;
|
assign rdata[13]=0;
|
assign rdata[13]=0;
|
assign rdata[14]=0;
|
assign rdata[14]=0;
|
assign rdata[15]=0;
|
assign rdata[15]=0;
|
|
|
|
|
wire [0:15]rl;
|
wire [0:15]rl;
|
assign rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlof,5'b0};
|
assign rl={5'b0,rlkpd,rlerr_0,rlerr_1,rla,rlsigma,rlOF,5'b0};
|
|
|
wire rack; // wishbone read acknowledged
|
wire rack; // wishbone read acknowledged
|
assign rack=(re&adr_check_1&(~rl[adr_1]))|(re&(~adr_check_1));
|
assign rack=(re&adr_check_1&(~rl[adr_1]))|(re&(~adr_check_1));
|
|
|
assign o_wb_ack=(wack|rack)&i_wb_stb;
|
assign o_wb_ack=(wack|rack)&i_wb_stb;
|
