Commit 3d7d6607 authored by Andrey Filippov's avatar Andrey Filippov

Adding vignetting correction module

parent aa331d14
......@@ -326,7 +326,7 @@
parameter SENSIO_STATUS_REG_REL = 1, // 4 locations" 'h21, 'h23, 'h25, 'h27
parameter SENSOR_NUM_HISTOGRAM= 3, // number of histogram channels
parameter HISTOGRAM_RAM_MODE = "NOBUF", // valid: "NOBUF" (32-bits, no buffering), "BUF18", "BUF32"
parameter SENS_GAMMA_NUM_CHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_NUM_SUBCHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_GAMMA_BUFFER = 0, // 1 - use "shadow" table for clean switching, 0 - single table per channel
// parameters defining address map
......@@ -381,9 +381,33 @@
parameter SENS_GAMMA_MODE_EN = 3,
parameter SENS_GAMMA_MODE_REPET = 4,
parameter SENS_GAMMA_MODE_TRIG = 5,
parameter SENSIO_RADDR = 8, //'h308 .. 'h30c
parameter SENSIO_ADDR_MASK = 'h7f8,
// Vignetting correction / pixel value scaling - controlled via single data word (same as in 252), some of bits [23:16]
// are used to select register, bits 25:24 - select sub-frame
parameter SENS_LENS_RADDR = 'h3c,
parameter SENS_LENS_ADDR_MASK = 'h7fc,
parameter SENS_LENS_COEFF = 'h3, // set vignetting/scale coefficients (
parameter SENS_LENS_AX = 'h00, // 00000...
parameter SENS_LENS_AX_MASK = 'hf8,
parameter SENS_LENS_AY = 'h08, // 00001...
parameter SENS_LENS_AY_MASK = 'hf8,
parameter SENS_LENS_C = 'h10, // 00010...
parameter SENS_LENS_C_MASK = 'hf8,
parameter SENS_LENS_BX = 'h20, // 001.....
parameter SENS_LENS_BX_MASK = 'he0,
parameter SENS_LENS_BY = 'h40, // 010.....
parameter SENS_LENS_BY_MASK = 'he0,
parameter SENS_LENS_SCALES = 'h60, // 01100...
parameter SENS_LENS_SCALES_MASK = 'hf8,
parameter SENS_LENS_FAT0_IN = 'h68, // 01101000
parameter SENS_LENS_FAT0_IN_MASK = 'hff,
parameter SENS_LENS_FAT0_OUT = 'h69, // 01101001
parameter SENS_LENS_FAT0_OUT_MASK = 'hff,
parameter SENS_LENS_POST_SCALE = 'h6a, // 01101010
parameter SENS_LENS_POST_SCALE_MASK = 'hff,
parameter SENSIO_RADDR = 8, //'h408 .. 'h40f
parameter SENSIO_ADDR_MASK = 'h7f8,
// sens_parallel12 registers
parameter SENSIO_CTRL = 'h0,
// SENSIO_CTRL register bits
......@@ -408,8 +432,8 @@
parameter SENSIO_DELAYS = 'h4, // 'h4..'h7
// 4 of 8-bit delays per register
// sensor_i2c_io command/data write registers s (relative to SENSOR_GROUP_ADDR)
parameter SENSI2C_ABS_RADDR = 'h10, // 'h310..'h31f
parameter SENSI2C_REL_RADDR = 'h20, // 'h320..'h32f
parameter SENSI2C_ABS_RADDR = 'h10, // 'h410..'h41f
parameter SENSI2C_REL_RADDR = 'h20, // 'h420..'h42f
parameter SENSI2C_ADDR_MASK = 'h7f0, // both for SENSI2C_ABS_ADDR and SENSI2C_REL_ADDR
// sens_hist registers (relative to SENSOR_GROUP_ADDR)
......
/*******************************************************************************
* Module: lens_flat393
* Date:2015-08-27
* Author: Andrey Filippov
* Description: Correction of lens+sensor vignetting. Initially it is just
* a quadratic function that can be improved later by a piece-linear table
* function T() of the calculated f(x,y)=p*(x-x0)^2 + q(y-yo)^2 + c.
* T(f(x,y)) can be used to approximate cos^4). or other vignetting functions
*
* This function - f(x,y) or T(f(x,y)) here deal with full sensor data before
* gamma-tables are applied and the data is compressed to 8 bits
*
* Copyright (c) 2008-2015 Elphel, Inc.
* lens_flat393.v is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* lens_flat393.v is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/> .
*******************************************************************************/
`timescale 1ns / 1ps
/*
F2(x,y)=p*(x-x0)^2 + q(y-yo)^2 + c=
p*x^2 - (2*p*x0) * x + p* (x0*x0) + q*y^2 - (2*q*y0) * y + q* (y0*y0) + c=
p* x^2 - (2*p*x0) * x + q* y^2 -(2*q)* y + (p* (x0*x0)+q* (y0*y0) + c)
Final:
F2(X,Y)=p* x^2 - (2*p*x0) * x + q* y^2 -(2*q)* y + (p* (x0*x0)+q* (y0*y0) + c):
Ax(Y)= p
Bx(Y)=-(2*p)
F(0,Y)= q*y^2 - (2*q*y0) * y + (q* (y0*y0) + c + p* (x0*x0))
C= (q* (y0*y0) + c + p* (x0*x0));
BY= - (2*q*y0)
AY= q
AX= p
BX= -2*p*x0
*/
module lens_flat393 #(
// Vignetting correction / pixel value scaling - controlled via single data word (same as in 252), some of bits [23:16]
// are used to select register, bits 25:24 - select sub-frame
parameter SENS_LENS_ADDR = 'h43c,
parameter SENS_LENS_ADDR_MASK = 'h7fc,
// parameter SENS_LENS_HEIGHTS = 'h0, // .. 'h2 set frame heights (all that is not SENS_LENS_COEFF)
parameter SENS_LENS_COEFF = 'h3, // set vignetting/scale coefficients (
parameter SENS_LENS_AX = 'h00, // 00000...
parameter SENS_LENS_AX_MASK = 'hf8,
parameter SENS_LENS_AY = 'h08, // 00001...
parameter SENS_LENS_AY_MASK = 'hf8,
parameter SENS_LENS_C = 'h10, // 00010...
parameter SENS_LENS_C_MASK = 'hf8,
parameter SENS_LENS_BX = 'h20, // 001.....
parameter SENS_LENS_BX_MASK = 'he0,
parameter SENS_LENS_BY = 'h40, // 010.....
parameter SENS_LENS_BY_MASK = 'he0,
parameter SENS_LENS_SCALES = 'h60, // 01100...
parameter SENS_LENS_SCALES_MASK = 'hf8,
parameter SENS_LENS_FAT0_IN = 'h68, // 01101000
parameter SENS_LENS_FAT0_IN_MASK = 'hff,
parameter SENS_LENS_FAT0_OUT = 'h69, // 01101001
parameter SENS_LENS_FAT0_OUT_MASK = 'hff,
parameter SENS_LENS_POST_SCALE = 'h6a, // 01101010
parameter SENS_LENS_POST_SCALE_MASK = 'hff,
parameter SENS_NUM_SUBCHN = 3, // number of subchannels on the same sensor port (<=4)
parameter SENS_LENS_F_WIDTH = 19, // AF2015 18, // number of bits in the output result
parameter SENS_LENS_F_SHIFT = 22, // shift ~2*log2(width/2), for 4K width
parameter SENS_LENS_B_SHIFT = 12, //(<=F_SHIFT) shift b- coeff (12 is 2^12 - good for lines <4096, 1 output count per width)
parameter SENS_LENS_A_WIDTH = 19, // AF2015 18, // number of bits in a-coefficient (unsigned). Just to match the caller - MSBs will be anyway discarded
parameter SENS_LENS_B_WIDTH = 21 // number of bits in b-coefficient (signed).
) (
input prst, // @pclk sync reset
input pclk, // global clock input, pixel rate (96MHz for MT9P006)
// programming interface
input mrst, // @mclk sync reset
input mclk, // global clock, half DDR3 clock, synchronizes all I/O through the command port
input [7:0] cmd_ad, // byte-serial command address/data (up to 6 bytes: AL-AH-D0-D1-D2-D3
input cmd_stb, // strobe (with first byte) for the command a/d
input [15:0] pxd_in, // @(posedge pclk)
input hact_in,
input sof_in, // start of frame, single pclk, input
input eof_in, // end of frame, single pclk, input
output reg [15:0] pxd_out, // pixel data out, 16 bit unsigned
output hact_out, //
output sof_out, // latency 8 from pxd_in;
output eof_out, //
input [1:0] bayer,
output [1:0] subchannel, // for gamma correction (valid before/at start of line, may be invalid at the end)
output last_in_sub // last line in subchannel (valid before/at start of line, may be invalid at the end)
);
// AF2015 new signals
wire [ 1:0] cmd_a;
wire [31:0] cmd_data;
reg [31:0] cmd_data_r; // holds data to cross clock boundary
wire cmd_we;
reg [15:0] heights_m1_ram[0:3]; // set @ posedge mclk, used at pclk, but should be OK (change before first hact)
reg [15:0] line_cntr; // count image lines to switch to next subchannels
reg [1:0] sub_frame_early; // valid before/at newline to provide coefficients to lens_flat393_line
reg [1:0] sub_frame;
reg [1:0] sub_frame_late; // valid @ hact_d[2]
reg [3:0] sub_frame_late_d; // add extra stages if needed
reg pre_first_line;
reg inc_sub_frame;
reg [ 8:0] hact_d; // lens_corr_out; /// lens correction out valid (first clock from column0 )
wire [15:0] pxd_d; // pxd_in delayed buy 4 clocks
reg [ 2:0] newline;
wire sof_d; // delayed sof_in by 5 clocks to init first lens_flat393_line
reg we_AX,we_BX,we_AY,we_BY,we_C;
reg we_scales;/// write additional individual per-color scales (17 bits each)
reg we_fatzero_in,we_fatzero_out; ///
reg we_post_scale;
//F(x,y)=Ax*x^2+Bx*x+Ay*y^2+By*y+C
// small rams to store per-subframe parameters, they will be registered at each subframe start
reg [18:0] AX_ram[0:3]; /// Ax
reg [18:0] AY_ram[0:3]; /// Ax
reg [20:0] BX_ram[0:3]; /// Bx
reg [20:0] BY_ram[0:3]; /// By
reg [18:0] C_ram[0:3]; /// C
reg [16:0] scales_ram[0:15]; // per-color coefficients (parallel-combined fro all colors)
// reg [16:0] scales_r;
reg [15:0] fatzero_in_ram[0:3]; /// zero level to subtract before multiplication
reg [15:0] fatzero_out_ram[0:3]; /// zero level to add after multiplication
reg [ 3:0] post_scale_ram[0:3]; /// shift product after first multiplier - maybe needed when using decimation
wire [18:0] FY; /// F(0,y)
wire [23:0] ERR_Y; /// running error for the first column
wire [18:0] FXY; /// F(x,y)
// reg [18:0] FXY_sat; // Not used, add extra cycle in calculations?
/// copied form sensorpix353.v
reg bayer_nset;
reg bayer0_latched;
reg [1:0] color;
wire [35:0] mult_first_res;
reg [17:0] mult_first_scaled; /// scaled multiplication result (to use with decimation to make parabola 'sharper')
wire [35:0] mult_second_res;
// Use sub_frame_late?
wire [20:0] pre_pixdo_with_zero= mult_second_res[35:15] + {{5{fatzero_out_ram[sub_frame][15]}},fatzero_out_ram[sub_frame][15:0]};
// wire sync_bayer=linerun && ~lens_corr_out[0];
wire sync_bayer=hact_d[2] && ~hact_d[3];
// Has to be valid @ hact_d[4], using sub_frame_late_d (valid at hact_d[3] to ease timing (there is always >=1 hact gap)
wire [17:0] pix_zero = {2'b0,pxd_d[15:0]}-{{2{fatzero_in_ram[sub_frame_late_d[3:2]][15]}},fatzero_in_ram[sub_frame_late_d[3:2]][15:0]};
// Writing to register files @mclk (4 per-subframe registers for coefficients, 4x4 - for per-subframe per-color scales)
// these registers will be read out at other clock (pclk)
wire set_lens_w = cmd_we && (cmd_a == SENS_LENS_COEFF );
wire set_heights_w = cmd_we && (cmd_a != SENS_LENS_COEFF );
assign subchannel = sub_frame ;
assign last_in_sub = inc_sub_frame;
assign hact_out = hact_d[8];
always @(posedge mclk) begin
cmd_data_r <= cmd_data;
if (set_heights_w) heights_m1_ram[cmd_a] <= cmd_data[15:0];
we_AX <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_AX, SENS_LENS_AX_MASK);
we_AY <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_AY, SENS_LENS_AY_MASK);
we_C <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_C, SENS_LENS_C_MASK);
we_BX <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_BX, SENS_LENS_BX_MASK);
we_BY <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_BY, SENS_LENS_BY_MASK);
we_scales <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_SCALES, SENS_LENS_SCALES_MASK);
we_fatzero_in <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_FAT0_IN, SENS_LENS_FAT0_IN_MASK);
we_fatzero_out <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_FAT0_OUT, SENS_LENS_FAT0_OUT_MASK);
we_post_scale <= set_lens_w && func_cmd_we (cmd_data, SENS_LENS_POST_SCALE, SENS_LENS_POST_SCALE_MASK);
// Write to RAM
if (we_AX) AX_ram [func_chn(cmd_data_r)] <= cmd_data_r[18:0];
if (we_AY) AY_ram [func_chn(cmd_data_r)] <= cmd_data_r[18:0];
if (we_BX) BX_ram [func_chn(cmd_data_r)] <= cmd_data_r[20:0];
if (we_BY) BY_ram [func_chn(cmd_data_r)] <= cmd_data_r[20:0];
if (we_C) C_ram [func_chn(cmd_data_r)] <= cmd_data_r[18:0];
if (we_scales) scales_ram [{func_chn(cmd_data_r), cmd_data_r[18:17]}] <= cmd_data_r[16:0];
if (we_fatzero_in) fatzero_in_ram [func_chn(cmd_data_r)] <= cmd_data_r[15:0];
if (we_fatzero_out) fatzero_out_ram[func_chn(cmd_data_r)] <= cmd_data_r[15:0];
if (we_post_scale) post_scale_ram [func_chn(cmd_data_r)] <= cmd_data_r[ 3:0];
end
always @ (posedge pclk) begin
hact_d <= {hact_d[7:0],hact_in};
newline <= {newline[1:0], hact_in && !hact_d[0]};
// line_start <= newline; // make it SR?
if (sof_in) pre_first_line <= 1;
else if (newline) pre_first_line <= 0;
if (pre_first_line || newline) inc_sub_frame <= (sub_frame != (SENS_NUM_SUBCHN - 1)) && (line_cntr == 0);
sub_frame_early <= sub_frame + inc_sub_frame;
if (pre_first_line) sub_frame <= 0;
else if (newline) sub_frame <= sub_frame_early;
if (pre_first_line || (newline[1] && inc_sub_frame)) line_cntr <= heights_m1_ram[sub_frame];
if (newline[2]) sub_frame_late <= sub_frame;
sub_frame_late_d <= {sub_frame_late_d[1:0],sub_frame_late}; // valid @ hact_d[3], use @hact_d[4] as there is always >= 1 clock HACT gap
end
//reg color[1:0]
always @ (posedge pclk) begin
bayer_nset <= !sof_in && (bayer_nset || hact_d[1]);
bayer0_latched<= bayer_nset? bayer0_latched:bayer[0];
color[1:0] <= { bayer_nset? (sync_bayer ^ color[1]):bayer[1] ,
(bayer_nset &&(~sync_bayer))?~color[0]:bayer0_latched };
/// now scale the result (normally post_scale[2:0] ==1)
case (post_scale_ram[sub_frame][2:0])
3'h0:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:33]) ? 18'h1ffff:mult_first_res[33:16]; /// only limit positive overflow
3'h1:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:32]) ? 18'h1ffff:mult_first_res[32:15];
3'h2:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:31]) ? 18'h1ffff:mult_first_res[31:14];
3'h3:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:30]) ? 18'h1ffff:mult_first_res[30:13];
3'h4:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:29]) ? 18'h1ffff:mult_first_res[29:12];
3'h5:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:28]) ? 18'h1ffff:mult_first_res[28:11];
3'h6:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:27]) ? 18'h1ffff:mult_first_res[27:10];
3'h7:mult_first_scaled[17:0]<= (~mult_first_res[35] & |mult_first_res[34:26]) ? 18'h1ffff:mult_first_res[26: 9];
endcase
if (hact_d[6]) pxd_out[15:0] <= pre_pixdo_with_zero[20]? 16'h0: /// negative - use 0
((|pre_pixdo_with_zero[19:16])?16'hffff: ///>0xffff - limit by 0xffff
pre_pixdo_with_zero[15:0]);
end
// Replacing MULT18X18SIO of x353, registers on both inputs, outputs
reg [17:0] mul1_a;
reg [17:0] mul1_b;
reg [35:0] mul1_p;
reg [17:0] mul2_a;
reg [17:0] mul2_b;
// wire [17:0] mul2_b = mult_first_scaled[17:0]; // TODO - delay to have a register!
reg [35:0] mul2_p;
always @ (posedge pclk) begin
if (hact_d[2]) mul1_a <= (FXY[18]==FXY[17])?FXY[17:0]:(FXY[18]?18'h20000:18'h1ffff);
if (hact_d[2]) mul1_b <= {1'b0,scales_ram[{sub_frame_late,~color[1:0]}]};
if (hact_d[3]) mul1_p <= mul1_a * mul1_b;
if (hact_d[4]) mul2_a <= pix_zero[17:0]; // adjust sub_frame delay
if (hact_d[4]) mul2_b <= mult_first_scaled[17:0]; // 18-bit multiplier input - always positive
if (hact_d[5]) mul2_p <= mul2_a * mul2_b;
end
assign mult_first_res = mul1_p;
assign mult_second_res = mul2_p;
cmd_deser #(
.ADDR (SENS_LENS_ADDR),
.ADDR_MASK (SENS_LENS_ADDR_MASK),
.NUM_CYCLES (6),
.ADDR_WIDTH (2),
.DATA_WIDTH (32)
) cmd_deser_lens_i (
.rst (mrst), // rst), // input
.clk (mclk), // input
.srst (mrst), // input
.ad (cmd_ad), // input[7:0]
.stb (cmd_stb), // input
.addr (cmd_a), // output[15:0]
.data (cmd_data), // output[31:0]
.we (cmd_we) // output
);
dly_16 #(
.WIDTH(2)
) dly_16_sof_eof_i (
.clk (pclk), // input
.rst (prst), // input
.dly (4'd7), // input[3:0]
.din ({sof_in,eof_in}), // input[0:0]
.dout ({sof_out,eof_out}) // output[0:0]
);
dly_16 #(
.WIDTH(16)
) dly_16_pxd_i (
.clk (pclk), // input
.rst (prst), // input
.dly (4'd4), // input[3:0]
.din (pxd_in), // input[0:0]
.dout (pxd_d) // output[0:0]
);
dly_16 #(
.WIDTH(1)
) dly_16_sof_d_i (
.clk (pclk), // input
.rst (prst), // input
.dly (4'd4), // input[3:0]
.din (sof_in), // input[0:0]
.dout (sof_d) // output[0:0]
);
lens_flat393_line #(
.F_WIDTH (SENS_LENS_F_WIDTH), // number of bits in the output result (signed)
.F_SHIFT (SENS_LENS_F_SHIFT), // shift ~2*log2(width/2), for 4K width
.B_SHIFT (SENS_LENS_B_SHIFT), //(<=F_SHIFT) shift b- coeff (12 is 2^12 - good for lines <4096, 1 output count per width)
.A_WIDTH (SENS_LENS_A_WIDTH), // number of bits in a-coefficient (signed). Just to match the caller - MSBs will be anyway discarded
.B_WIDTH (SENS_LENS_B_WIDTH)) // number of bits in b-coefficient (signed).
i_fy(
.pclk (pclk), // pixel clock
.first (sof_d), // initialize running parameters from the inputs (first column). Should be at least 1-cycle gap between "first" and first "next"
.next (newline[0]), // calcualte next pixel
.F0 (C_ram[sub_frame_early]), // value of the output in the first column (before saturation), 18 bit, unsigned
.ERR0 (24'b0), // initial value of the running error (-2.0<err<+2.0), scaled by 2^22, so 24 bits
.A0 (AY_ram[sub_frame_early]), // Ay
.B0 (BY_ram[sub_frame_early]), // By, signed
.F (FY),
.ERR (ERR_Y));
lens_flat393_line #(
.F_WIDTH (SENS_LENS_F_WIDTH), // number of bits in the output result (signed)
.F_SHIFT (SENS_LENS_F_SHIFT), // shift ~2*log2(width/2), for 4K width
.B_SHIFT (SENS_LENS_B_SHIFT), // (<=F_SHIFT) shift b- coeff (12 is 2^12 - good for lines <4096, 1 output count per width)
.A_WIDTH (SENS_LENS_A_WIDTH), // number of bits in a-coefficient (signed). Just to match the caller - MSBs will be anyway discarded
.B_WIDTH (SENS_LENS_B_WIDTH)) // number of bits in b-coefficient (signed).
i_fxy(
.pclk (pclk), // pixel clock
.first (newline[0]), // initialize running parameters from the inputs (first column). Should be at least 1-cycle gap between "first" and first "next"
.next (hact_d[1]), // calcualte next pixel
.F0 (FY), // value of the output in the first column (before saturation), 18 bit, unsigned
.ERR0 (ERR_Y), // initial value of the running error (-2.0<err<+2.0), scaled by 2^22, so 24 bits
.A0 (AX_ram[sub_frame_early]), // Ax(Y), signed
.B0 (BX_ram[sub_frame_early]), // Bx(Y), signed
.F (FXY),
.ERR());
function func_cmd_we;
input [31:0] cmd_data;
input [7:0] pattern;
input [7:0] mask;
begin
func_cmd_we = ((cmd_data[23:16] ^ pattern) & mask) == 0;
end
endfunction
function [1:0] func_chn;
input [31:0] cmd_data;
begin
func_chn = cmd_data[25:24];
end
endfunction
endmodule
module lens_flat393_line#(
parameter F_WIDTH = 19, // AF2015 18, /// number of bits in the output result
parameter F_SHIFT = 22, /// shift ~2*log2(width/2), for 4K width
parameter B_SHIFT = 12, ///(<=F_SHIFT) shift b- coeff (12 is 2^12 - good for lines <4096, 1 output count per width)
parameter A_WIDTH = 19, // AF2015 18, /// number of bits in a-coefficient (unsigned). Just to match the caller - MSBs will be anyway discarded
parameter B_WIDTH = 21 // number of bits in b-coefficient (signed).
)(
input pclk, /// pixel clock
input first, /// initialize running parameters from the inputs (first column). Should be at least 1-cycle gap between "first" and first "next"
input next, /// calcualte next pixel
input [F_WIDTH-1:0] F0, /// value of the output in the first column (before saturation), 18 bit, unsigned
input [F_SHIFT+1:0] ERR0, /// initial value of the running error (-2.0<err<+2.0), scaled by 2^22, so 24 bits
input [A_WIDTH-1:0] A0, /// a - fixed for negative values
input [B_WIDTH-1:0] B0,
output [F_WIDTH-1:0] F,
output reg [F_SHIFT+1:0] ERR); /// running difference between ax^2+bx+c and y, scaled by 2^22, signed, should never overflow
// output - 18 bits, unsigned (not saturated)
localparam DF_WIDTH = B_WIDTH - F_SHIFT + B_SHIFT; //21-22+12 11; /// number of bits in step of F between (df/dx), signed
reg [F_SHIFT+1:0] ApB; // a+b, scaled by 2 ^22, high bits ignored (not really needed - can use ApB0
reg [F_SHIFT+1:1] A2X; // running value for 2*a*x, scaled by 2^22, high bits ignored
reg [(DF_WIDTH)-1:0] dF; // or [9:0] - anyway only lower bits will be used in comparison operations
reg [F_WIDTH-1:0] F_r; // Running value of the output
reg next_d, first_d; // delayed by 1 cycle
reg [F_WIDTH-1:0] F1;
reg [A_WIDTH-1:0] A;
wire [F_SHIFT+1:0] preERR={A2X[F_SHIFT+1:1],1'b0}+ApB[F_SHIFT+1:0]-{dF[1:0],{F_SHIFT{1'b0}}};
assign F = F_r;
/// Increment can be 0 or +/-1, depending on the required correction
/// It relies on the facts that:
/// - the output F(x) is integer
/// - dF/dx does not chnage by more than +/-1 when x is incremented (abs (d2f/dx2)<1), so the algorithm to get
/// y=round(F(x)) is simple :
/// At each step x, try to chnage y by the same amount as was done at the previous step, adding/subtracting 1 if needed
/// and updating the new running error (difference between the current (integer) value of y and the precise value of F(x)
/// This error is calculated here with the 22 binary digits after the point.
///f=ax^2+bx+c
///
///1) f <= f+ df +1
/// df <= df+1;
/// err+= (2ax+a+b-df) -1
///2) f <= f+ df
/// err+= (2ax+a+b-df)
///3) f <= f+ df -1
/// df <= df-1;
/// err+= (2ax+a+b-df) +1
///preERR->inc:
/// 100 -> 11
/// 101 -> 11
/// 110 -> 11
/// 111 -> 00
/// 000 -> 00
/// 001 -> 01
/// 010 -> 01
/// 011 -> 01
wire [1:0] inc= {preERR[F_SHIFT+1] & (~preERR[F_SHIFT] | ~preERR[F_SHIFT-1]),
(preERR[F_SHIFT+1:F_SHIFT-1] != 3'h0) &
(preERR[F_SHIFT+1:F_SHIFT-1] != 3'h7)};
always @(posedge pclk) begin
first_d <=first;
next_d <=next;
if (first) begin
F1 [F_WIDTH-1:0] <= F0[ F_WIDTH-1:0];
dF[(DF_WIDTH)-1:0] <= B0[B_WIDTH-1: (F_SHIFT-B_SHIFT)];
ERR[F_SHIFT+1:0] <= ERR0[F_SHIFT+1:0];
ApB[F_SHIFT+1:0] <= {{F_SHIFT + 2 - A_WIDTH{A0[A_WIDTH-1]}},A0[A_WIDTH-1:0]} + // width correct
//AF2015 {B0[B_WIDTH-1:0],{F_SHIFT-B_SHIFT{1'b0}}}; /// high bits from B will be discarded
{B0[B_SHIFT-1:0],{F_SHIFT-B_SHIFT{1'b0}}}; /// high bits from B are discarded
A [A_WIDTH-1:0] <= A0[A_WIDTH-1:0];
end else if (next) begin
//AF2015 dF[(DF_WIDTH)-1:0] <= dF[(DF_WIDTH)-1:0]+{{((DF_WIDTH)-1){inc[1]}},inc[1:0]};
dF[(DF_WIDTH)-1:0] <= dF[(DF_WIDTH)-1:0] + {{((DF_WIDTH)-2){inc[1]}},inc[1:0]};
ERR[F_SHIFT-1:0]<= preERR[F_SHIFT-1:0];
ERR[F_SHIFT+1:F_SHIFT]<= preERR[F_SHIFT+1:F_SHIFT]-inc[1:0];
end
if (first_d) F_r[F_WIDTH-1:0] <= F1[ F_WIDTH-1:0];
else if (next_d) F_r[F_WIDTH-1:0] <= F_r[F_WIDTH-1:0]+{{(F_WIDTH-(DF_WIDTH)){dF[(DF_WIDTH)-1]}},dF[(DF_WIDTH)-1:0]};
//AF2015 if (first_d) A2X[F_SHIFT+1:1] <= {{F_SHIFT+2-A_WIDTH{A[A_WIDTH-1]}},A[A_WIDTH-1:0]};
//AF2015 else if (next) A2X[F_SHIFT+1:1] <= A2X[F_SHIFT+1:1] + {{F_SHIFT+2-A_WIDTH{A[A_WIDTH-1]}},A[A_WIDTH-1:0]};
if (first_d) A2X[F_SHIFT+1:1] <= {{F_SHIFT+1-A_WIDTH{A[A_WIDTH-1]}},A[A_WIDTH-1:0]};
else if (next) A2X[F_SHIFT+1:1] <= A2X[F_SHIFT+1:1] + {{F_SHIFT+1-A_WIDTH{A[A_WIDTH-1]}},A[A_WIDTH-1:0]};
end
endmodule
......@@ -21,9 +21,9 @@
`timescale 1ns/1ps
module sens_gamma #(
parameter SENS_GAMMA_NUM_CHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_NUM_SUBCHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_GAMMA_BUFFER = 0, // 1 - use "shadow" table for clean switching, 0 - single table per channel
parameter SENS_GAMMA_ADDR = 'h338,
parameter SENS_GAMMA_ADDR = 'h438,
parameter SENS_GAMMA_ADDR_MASK = 'h7fc,
parameter SENS_GAMMA_CTRL = 'h0,
parameter SENS_GAMMA_ADDR_DATA = 'h1, // bit 20 ==1 - table address, bit 20==0 - table data (18 bits)
......@@ -65,7 +65,9 @@ module sens_gamma #(
// programming interface
input mclk, // global clock, half DDR3 clock, synchronizes all I/O through the command port
input [7:0] cmd_ad, // byte-serial command address/data (up to 6 bytes: AL-AH-D0-D1-D2-D3
input cmd_stb // strobe (with first byte) for the command a/d
input cmd_stb, // strobe (with first byte) for the command a/d
output [1:0] bayer_out // for lens_flat module - separate them?
);
wire [1:0] cmd_a;
wire [31:0] cmd_data;
......@@ -182,9 +184,13 @@ module sens_gamma #(
assign sof_masked= sof_in && (pend_trig || repet_mode) && en_input;
assign trig = trig_in || trig_soft;
assign bayer_out = bayer;
always @ (posedge mclk) begin
if (mrst) tdata <= 0;
else if (set_taddr_w) tdata <= cmd_data[17:0];
// else if (set_taddr_w) tdata <= cmd_data[17:0];
else if (set_tdata_w) tdata <= cmd_data[17:0];
if (mrst) set_tdata_r <= 0;
else set_tdata_r <= set_tdata_w;
......@@ -256,7 +262,7 @@ module sens_gamma #(
ram_chn_d <= ram_chn;
ram_chn_d2 <= ram_chn_d;
inc_line <= hact_d[0] && !hact_in && (sensor_subchn < SENS_GAMMA_NUM_CHN);
inc_line <= hact_d[0] && !hact_in && (sensor_subchn < SENS_NUM_SUBCHN);
sof_r <= sof_in;
......@@ -265,6 +271,7 @@ module sens_gamma #(
if (sof_r) line_cntr <= height0_m1;
else if (inc_line && (line_cntr == 0)) line_cntr <= (sensor_subchn ==0)? height1_m1: height2_m1;
else if (inc_line) line_cntr <= line_cntr - 1;
end
......@@ -275,8 +282,8 @@ module sens_gamma #(
.NUM_CYCLES (6),
.ADDR_WIDTH (2),
.DATA_WIDTH (32)
) cmd_deser_sens_io_i (
.rst (1'b0), // rst), // input
) cmd_deser_sens_gamma_i (
.rst (mrst), // rst), // input
.clk (mclk), // input
.srst (mrst), // input
.ad (cmd_ad), // input[7:0]
......@@ -336,7 +343,7 @@ module sens_gamma #(
.REGISTERS (1), // try to delay i2c_byte_start by one more cycle
.LOG2WIDTH_WR (4),
.LOG2WIDTH_RD (4),
.DUMMY (!((SENS_GAMMA_NUM_CHN > 1) && ((SENS_GAMMA_NUM_CHN > 2) || SENS_GAMMA_BUFFER)))
.DUMMY (!((SENS_NUM_SUBCHN > 1) && ((SENS_NUM_SUBCHN > 2) || SENS_GAMMA_BUFFER)))
) gamma_table1_i (
.rclk (pclk), // input
.raddr (table_raddr), // input[11:0]
......@@ -354,7 +361,7 @@ module sens_gamma #(
.REGISTERS (1), // try to delay i2c_byte_start by one more cycle
.LOG2WIDTH_WR (4),
.LOG2WIDTH_RD (4),
.DUMMY (!(SENS_GAMMA_BUFFER && (SENS_GAMMA_NUM_CHN > 2)))
.DUMMY (!(SENS_GAMMA_BUFFER && (SENS_NUM_SUBCHN > 2)))
) gamma_table2_i (
.rclk (pclk), // input
.raddr (table_raddr), // input[11:0]
......@@ -372,7 +379,7 @@ module sens_gamma #(
.REGISTERS (1), // try to delay i2c_byte_start by one more cycle
.LOG2WIDTH_WR (4),
.LOG2WIDTH_RD (4),
.DUMMY (!(SENS_GAMMA_BUFFER && (SENS_GAMMA_NUM_CHN > 3)))
.DUMMY (!(SENS_GAMMA_BUFFER && (SENS_NUM_SUBCHN > 3)))
) gamma_table3_i (
.rclk (pclk), // input
.raddr (table_raddr), // input[11:0]
......
......@@ -44,7 +44,7 @@ module sensor_channel#(
parameter SENSOR_NUM_HISTOGRAM= 3, // number of histogram channels
parameter HISTOGRAM_RAM_MODE = "NOBUF", // valid: "NOBUF" (32-bits, no buffering), "BUF18", "BUF32"
parameter SENS_GAMMA_NUM_CHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_NUM_SUBCHN = 3, // number of subchannels for his sensor ports (1..4)
parameter SENS_GAMMA_BUFFER = 0, // 1 - use "shadow" table for clean switching, 0 - single table per channel
// parameters defining address map
......@@ -91,8 +91,31 @@ module sensor_channel#(
parameter SENS_GAMMA_MODE_EN = 3,
parameter SENS_GAMMA_MODE_REPET = 4,
parameter SENS_GAMMA_MODE_TRIG = 5,
// Vignetting correction / pixel value scaling - controlled via single data word (same as in 252), some of bits [23:16]
// are used to select register, bits 25:24 - select sub-frame
parameter SENS_LENS_RADDR = 'h3c,
parameter SENS_LENS_ADDR_MASK = 'h7fc,
parameter SENS_LENS_COEFF = 'h3, // set vignetting/scale coefficients (
parameter SENS_LENS_AX = 'h00, // 00000...
parameter SENS_LENS_AX_MASK = 'hf8,
parameter SENS_LENS_AY = 'h08, // 00001...
parameter SENS_LENS_AY_MASK = 'hf8,
parameter SENS_LENS_C = 'h10, // 00010...
parameter SENS_LENS_C_MASK = 'hf8,
parameter SENS_LENS_BX = 'h20, // 001.....
parameter SENS_LENS_BX_MASK = 'he0,
parameter SENS_LENS_BY = 'h40, // 010.....
parameter SENS_LENS_BY_MASK = 'he0,
parameter SENS_LENS_SCALES = 'h60, // 01100...
parameter SENS_LENS_SCALES_MASK = 'hf8,
parameter SENS_LENS_FAT0_IN = 'h68, // 01101000
parameter SENS_LENS_FAT0_IN_MASK = 'hff,
parameter SENS_LENS_FAT0_OUT = 'h69, // 01101001
parameter SENS_LENS_FAT0_OUT_MASK = 'hff,
parameter SENS_LENS_POST_SCALE = 'h6a, // 01101010
parameter SENS_LENS_POST_SCALE_MASK = 'hff,
parameter SENSIO_RADDR = 8, //'h308 .. 'h30f
parameter SENSIO_RADDR = 8, //'h408 .. 'h40f
parameter SENSIO_ADDR_MASK = 'h7f8,
// sens_parallel12 registers
parameter SENSIO_CTRL = 'h0,
......@@ -118,8 +141,8 @@ module sensor_channel#(
parameter SENSIO_DELAYS = 'h4, // 'h4..'h7
// 4 of 8-bit delays per register
// sensor_i2c_io command/data write registers s (relative to SENSOR_BASE_ADDR)
parameter SENSI2C_ABS_RADDR = 'h10, // 'h310..'h31f
parameter SENSI2C_REL_RADDR = 'h20, // 'h320..'h32f
parameter SENSI2C_ABS_RADDR = 'h10, // 'h410..'h41f
parameter SENSI2C_REL_RADDR = 'h20, // 'h420..'h42f
parameter SENSI2C_ADDR_MASK = 'h7f0, // both for SENSI2C_ABS_ADDR and SENSI2C_REL_ADDR
// sens_hist registers (relative to SENSOR_BASE_ADDR)
......@@ -228,6 +251,7 @@ module sensor_channel#(
localparam SENSI2C_CTRL_ADDR = SENSOR_BASE_ADDR + SENSI2C_CTRL_RADDR;
localparam SENS_GAMMA_ADDR = SENSOR_BASE_ADDR + SENS_GAMMA_RADDR;
localparam SENSIO_ADDR = SENSOR_BASE_ADDR + SENSIO_RADDR;
localparam SENS_LENS_ADDR = SENSOR_BASE_ADDR + SENS_LENS_RADDR;
localparam SENSI2C_ABS_ADDR = SENSOR_BASE_ADDR + SENSI2C_ABS_RADDR;
localparam SENSI2C_REL_ADDR = SENSOR_BASE_ADDR + SENSI2C_REL_RADDR;
localparam HISTOGRAM_ADDR0 = (SENSOR_NUM_HISTOGRAM > 0)?(SENSOR_BASE_ADDR + HISTOGRAM_RADDR0):-1; //
......@@ -261,11 +285,18 @@ module sensor_channel#(
wire sof_out_sync; // sof filtetred, optionally decimated (for linescan mode)
wire [15:0] lens_pxd_in;
wire lens_hact_in;
wire lens_sof_in;
wire lens_eof_in;
wire [15:0] gamma_pxd_in;
wire gamma_hact_in;
wire gamma_sof_in;
wire gamma_eof_in;
wire [1:0] gamma_bayer; // gamma module mode register bits -> lens_flat module
wire [7:0] gamma_pxd_out;
......@@ -299,10 +330,10 @@ module sensor_channel#(
reg eof_out_r;
// TODO: insert vignetting and/or flat field, pixel defects before gamma_*_in
assign gamma_pxd_in = {pxd[11:0],4'b0};
assign gamma_hact_in = hact;
assign gamma_sof_in = sof_out_sync; // sof;
assign gamma_eof_in = eof;
assign lens_pxd_in = {pxd[11:0],4'b0};
assign lens_hact_in = hact;
assign lens_sof_in = sof_out_sync; // sof;
assign lens_eof_in = eof;
assign dout = dout_r;
assign dout_valid = dav_r;
......@@ -538,8 +569,56 @@ module sensor_channel#(
.cmd_stb (cmd_stb) // input
);
lens_flat393 #(
.SENS_LENS_ADDR (SENS_LENS_ADDR),
.SENS_LENS_ADDR_MASK (SENS_LENS_ADDR_MASK),
.SENS_LENS_COEFF (SENS_LENS_COEFF),
.SENS_LENS_AX (SENS_LENS_AX),
.SENS_LENS_AX_MASK (SENS_LENS_AX_MASK),
.SENS_LENS_AY (SENS_LENS_AY),
.SENS_LENS_AY_MASK (SENS_LENS_AY_MASK),
.SENS_LENS_C (SENS_LENS_C),
.SENS_LENS_C_MASK (SENS_LENS_C_MASK),
.SENS_LENS_BX (SENS_LENS_BX),
.SENS_LENS_BX_MASK (SENS_LENS_BX_MASK),
.SENS_LENS_BY (SENS_LENS_BY),
.SENS_LENS_BY_MASK (SENS_LENS_BY_MASK),
.SENS_LENS_SCALES (SENS_LENS_SCALES),
.SENS_LENS_SCALES_MASK (SENS_LENS_SCALES_MASK),