af_alg_common.c

dm270 source code

/* Auto-focus common routines, variables for AF algo 1, AF algo 2   */

#include <system/uart270.h>
#include <system/h3a270.h>

#include <user/af_ctrl.h>
#include <user/ccdm_3d3_lens_ctrl.h>

#include <h3a/af_alg_externs.h>

/* global variables */

/* IIR filter coeffcients */
char *iir_filter_str[IIR_FILTER_ID_MAX] = {
    "none",
    "butter_wide",
    "butter_med",
    "butter_narrow",
    "ellip_narrow",
    "cheby_narrow",
    "FIR_highpass"  
};

Int16 iir_filter[IIR_FILTER_ID_MAX][11]={
    {  0,     0,    0,     0,     0,     0,     0,     0,     0,     0,     0 },
    { 64,    55,   -13,  -16,    32,   -16,    71,   -37,   -16,    32,   -16 },
    { 64,     0,    -3,   -16,    32,   -16,     0,   -29,   -16,    32,   -16},
    { 64,   -80,   -26,   -16,    32,   -16,   -97,   -45,   -16,    32,   -16},
    { 64,   -96,   -40,   -16,   -18,   -16,  -110,   -60,   -16,   -27,   -16},
    { 64,  -102,   -43,   -16,    32,   -16,  -105,   -55,   -16,    32,   -16},
    { 64,     0,    0,   -16,    32,   -16,     0,     0,    64,     0,     0 }

};

int  fine_step_size=1;
char out_buff[100];
int af_hstart=3, af_vstart=0, af_hend=5, af_vend=3;

Uint16 MIN_MOTOR_VALUE=240;
Uint16 MAX_MOTOR_VALUE=280;

int cur_motor_value;

STATUS AFAlgSharpCalcReset() {
    #if AF_OUT_DATA
    memset( (void*)focus_value, 0, 360*50*4);
    #endif
    return E_PASS;
}

int AFAlgSharpCalc(char *data_addr) {
    int v, h;
    Uint32 cur_val, sum;
    
    sum=0;
    for(v=0;v<4;v++) {  
        for(h=0;h<10;h++) { 
            cur_val= *(Uint32 *)(data_addr+4);

            if(  h>=af_hstart && h <=af_hend &&
                 v>=af_vstart && v <=af_vend
              ) {
                sum+=cur_val;
            }
            #if AF_OUT_DATA
            focus_value[cur_motor_value][v*10+h] = cur_val;
            #endif
            data_addr += 16;    
        }
    }
    #if AF_OUT_DATA
    focus_value[cur_motor_value][40]=sum;
    #endif
        
    return sum;
}

STATUS AFAlgFocusMotorInit() {
    CCDM_3D3_fxFocusMotor(320-MIN_MOTOR_VALUE+16);
    CCDM_3D3_rxFocusMotor(320-MAX_MOTOR_VALUE);
    cur_motor_value=MAX_MOTOR_VALUE;
    return E_PASS;
}


STATUS CCDM_MotorCtrlReset() {
    CCDM_3D3_fxFocusMotor(MAX_MOTOR_VALUE-cur_motor_value+16);
    CCDM_3D3_rxFocusMotor(16);
    cur_motor_value=MAX_MOTOR_VALUE;
    return E_PASS;
}

STATUS CCDM_MotorCtrl(int motor_value) {
    if(motor_value > 0 ) {
        CCDM_3D3_fxFocusMotor(motor_value);
    }

    if(motor_value < 0 ) {
        motor_value = -motor_value;
        CCDM_3D3_rxFocusMotor(motor_value);
    }
    return E_PASS;
}


STATUS AFAlgSetFilter(AF_CTRL *af_ctrl, Uint16 filter_id_0, Uint16 filter_id_1) {
	Uint8 i;
	
    if( filter_id_0 >= IIR_FILTER_ID_MAX )
        return E_INVALID_INPUT;

	for(i=0; i<11; i++) 
		af_ctrl->afConfig->iircoeff_0[i]= iir_filter[filter_id_0][i];
    
    if( filter_id_1 >= IIR_FILTER_ID_MAX )
        return E_INVALID_INPUT;    

	for(i=0; i<11; i++) 
		af_ctrl->afConfig->iircoeff_1[i]= iir_filter[filter_id_1][i];
    
    return E_PASS;
}

#if AF_OUT_DATA
BOOL dump_data=FALSE;
Uint32 focus_value[360][50];

static int cur_file_index=0;
static char dump_buff[1024];

int DumpDataToFile() {
    FILE *fp;
    int i, j;

    UART_sendString( UART0, "\r\nWriting data to file ...");
    UART_sendString( UART0, "\r\n[0........9]");
    UART_sendString( UART0, "\r\n[");
    sprintf(out_buff,"D:\\ked\\DM270\\DM270_CCD_Preview\\auto_focus\\dump\\af_%d%d.dat", cur_file_index/10, cur_file_index%10); 
    fp=fopen(out_buff,"w");

    if(fp==NULL) 
        return E_INVALID_INPUT;
    
    for(i=MIN_MOTOR_VALUE; i<=MAX_MOTOR_VALUE; i++) {
        dump_buff[0]=0;
        for(j=0;j<41; j++) { 
            if(j!=40)
                sprintf( out_buff, "%ld , ", focus_value[i][j]);
            else
                sprintf( out_buff, "%ld\n", focus_value[i][j]);
            strcat(dump_buff, out_buff);
        }
        fprintf(fp, dump_buff );
        if( (i-MIN_MOTOR_VALUE+1)%((MAX_MOTOR_VALUE-MIN_MOTOR_VALUE)/10) == 0)
            UART_sendString( UART0, ".");
    }
    
    fclose(fp);
    cur_file_index++;
    UART_sendString( UART0, "]   DONE.\r\n");
    return E_PASS;
}
#endif