int saradc_ts_service(int cmd)
{
int value = -1;
switch (cmd) {
case CMD_GET_X:
//set_sample_sw(CHAN_YP, X_SW);
value = get_adc_sample(CHAN_YP);
set_sample_sw(CHAN_XP, Y_SW); // preset for y
break;
case CMD_GET_Y:
//set_sample_sw(CHAN_XP, Y_SW);
value = get_adc_sample(CHAN_XP);
break;
case CMD_GET_Z1:
set_sample_sw(CHAN_XP, Z1_SW);
value = get_adc_sample(CHAN_XP);
break;
case CMD_GET_Z2:
set_sample_sw(CHAN_YN, Z2_SW);
value = get_adc_sample(CHAN_YN);
break;
case CMD_GET_PENDOWN:
value = !detect_level();
set_sample_sw(CHAN_YP, X_SW); // preset for x
break;
case CMD_INIT_PENIRQ:
enable_detect_pullup();
enable_detect_sw();
value = 0;
printk(KERN_INFO "init penirq ok\n");
break;
case CMD_SET_PENIRQ:
enable_detect_pullup();
enable_detect_sw();
value = 0;
break;
case CMD_CLEAR_PENIRQ:
disable_detect_pullup();
disable_detect_sw();
value = 0;
break;
default:
break;
}
return value;
}
void saradc_enable(void)
{
int i;
enable_bandgap();
set_clock_src(0); //0-xtal, 1-clk81
//set adc clock as 1.28Mhz @sys=27MHz
set_clock_divider(20);
enable_clock();
enable_adc();
set_sample_mode(DIFF_MODE);
set_tempsen(0);
disable_fifo_irq();
disable_continuous_sample();
disable_chan0_delta();
disable_chan1_delta();
set_input_delay(10, INPUT_DELAY_TB_1US);
set_sample_delay(10, SAMPLE_DELAY_TB_1US);
set_block_delay(10, BLOCK_DELAY_TB_1US);
// channels sampling mode setting
for(i=0; i<AML_ADC_SARADC_CHAN_NUM; i++) {
set_sample_sw(i, IDLE_SW);
set_sample_mux(i, g_chan_mux[i]);
}
// idle mode setting
set_idle_sw(IDLE_SW);
set_idle_mux(g_chan_mux[AML_ADC_CHAN_0]);
// detect mode setting
set_detect_sw(DETECT_SW);
set_detect_mux(g_chan_mux[AML_ADC_CHAN_0]);
disable_detect_sw();
disable_detect_pullup();
set_detect_irq_pol(0);
disable_detect_irq();
set_cal_voltage(7);
enable_sample_engine();
#if AML_ADC_SAMPLE_DEBUG
printf("ADCREG reg0 =%x\n", get_reg(PP_SAR_ADC_REG0));
printf("ADCREG ch list =%x\n",get_reg(PP_SAR_ADC_CHAN_LIST));
printf("ADCREG avg =%x\n", get_reg(PP_SAR_ADC_AVG_CNTL));
printf("ADCREG reg3 =%x\n", get_reg(PP_SAR_ADC_REG3));
printf("ADCREG ch72 sw =%x\n",get_reg(PP_SAR_ADC_AUX_SW));
printf("ADCREG ch10 sw =%x\n",get_reg(PP_SAR_ADC_CHAN_10_SW));
printf("ADCREG detect&idle=%x\n",get_reg(PP_SAR_ADC_DETECT_IDLE_SW));
#endif //AML_ADC_SAMPLE_DEBUG
select_temp();
enable_temp();
enable_temp__();
udelay(1000);
}
static void saradc_reset(void)
{
int i;
#if defined(CONFIG_ARCH_MESONG9TV) || defined(CONFIG_ARCH_MESONG9BB)
set_sar_adc_clk();
#endif
//set adc clock as 1.28Mhz
set_clock_divider(20);
saradc_power_control(1);
set_sample_mode(DIFF_MODE);
set_tempsen(0);
disable_fifo_irq();
disable_continuous_sample();
disable_chan0_delta();
disable_chan1_delta();
set_input_delay(10, INPUT_DELAY_TB_1US);
set_sample_delay(10, SAMPLE_DELAY_TB_1US);
set_block_delay(10, BLOCK_DELAY_TB_1US);
// channels sampling mode setting
for(i=0; i<SARADC_CHAN_NUM; i++) {
set_sample_sw(i, IDLE_SW);
set_sample_mux(i, chan_mux[i]);
}
// idle mode setting
set_idle_sw(IDLE_SW);
set_idle_mux(chan_mux[CHAN_0]);
// detect mode setting
set_detect_sw(DETECT_SW);
set_detect_mux(chan_mux[CHAN_0]);
disable_detect_sw();
disable_detect_pullup();
set_detect_irq_pol(0);
disable_detect_irq();
// set_sc_phase();
enable_sample_engine();
// printk("ADCREG reg0 =%x\n", get_reg(PP_SAR_ADC_REG0));
// printk("ADCREG ch list =%x\n", get_reg(PP_SAR_ADC_CHAN_LIST));
// printk("ADCREG avg =%x\n", get_reg(PP_SAR_ADC_AVG_CNTL));
// printk("ADCREG reg3=%x\n", get_reg(PP_SAR_ADC_REG3));
// printk("ADCREG ch72 sw=%x\n", get_reg(PP_SAR_ADC_AUX_SW));
// printk("ADCREG ch10 sw=%x\n", get_reg(PP_SAR_ADC_CHAN_10_SW));
// printk("ADCREG detect&idle=%x\n", get_reg(PP_SAR_ADC_DETECT_IDLE_SW));
}
static void saradc_init(void)
{
int i;
enable_bandgap();
//low speed, set to clk81 without division
set_clock_src(1); //0-xtal, 1-clk81
set_clock_divider(0);
enable_clock();
enable_adc();
set_sample_mode(DIFF_MODE);
set_tempsen(0);
disable_fifo_irq();
disable_continuous_sample();
disable_chan0_delta();
disable_chan1_delta();
set_input_delay(10, INPUT_DELAY_TB_1US);
set_sample_delay(10, SAMPLE_DELAY_TB_1US);
set_block_delay(10, BLOCK_DELAY_TB_1US);
aml_set_reg32_bits(P_AO_SAR_ADC_DELAY, 3, 27, 2);
// channels sampling mode setting
for(i=0; i<AML_ADC_SARADC_CHAN_NUM; i++) {
set_sample_sw(i, IDLE_SW);
set_sample_mux(i, g_chan_mux[i]);
}
// idle mode setting
set_idle_sw(IDLE_SW);
set_idle_mux(g_chan_mux[AML_ADC_CHAN_0]);
// detect mode setting
set_detect_sw(DETECT_SW);
set_detect_mux(g_chan_mux[AML_ADC_CHAN_0]);
disable_detect_sw();
disable_detect_pullup();
set_detect_irq_pol(0);
disable_detect_irq();
set_cal_voltage(7);
set_sc_phase();
enable_sample_engine();
udelay(1000);
while (get_fifo_cnt()) {
i = get_fifo_sample() & 0x3ff;
}
}
//void main_function(cyg_addrword_t data) {
void * main_function(void * data) {
int tmp = 1;
region_information = (int *) malloc (4 * sizeof(int));
// create particles
create_particle_filter(100, 10);
/*slots = (int *) malloc (6 * sizeof(int));
slots[0] = 0;
slots[1] = 1;
slots[2] = 2;
slots[3] = 3;
slots[4] = 4;
slots[5] = 5;
set_importance_hw_static(1, &slots[0]);*/
/*rthread_attr_init(&hwthread_sorter_attr);
rthread_attr_setslotnum(&hwthread_sorter_attr, 0);
rthread_attr_setresources(&hwthread_sorter_attr, hwthread_sorter_resources, 3);
reconos_hwthread_create( 15, // priority
&hwthread_sorter_attr, // hardware thread attributes
0, // entry data (not needed)
"MT_HW_SORT", // thread name
hwthread_sorter_stack, // stack
STACK_SIZE, // stack size
&hwthread_sorter_handle, // thread handle
&hwthread_sorter // thread object
);
cyg_thread_resume( hwthread_sorter_handle );*/
//cyg_thread_delay(50);
#ifndef NO_ETHERNET
init_all_network_interfaces();
if(!eth0_up){
printf("failed to initialize eth0\naborting\n");
return NULL;
}
else{
printf(" eth0 up\n");
}
diag_printf( "initializing ECAP interface..." );
ecap_init();
diag_printf( "done\n" );
// establish connection
while (tmp == 1){
tmp = establish_connection(6666, region_information);
}
#endif
// start read_new_frame
cyg_semaphore_post(sem_read_new_frame_start);
srand(1);
// set region information for equal time measurements
#define VIDEO 1
printf("\n#################################################");
printf("\n#################################################");
#ifdef VIDEO
#if VIDEO==1
printf("\n########## S O C C E R V I D E O ############");
region_information[0] = 286;
region_information[1] = 247;
region_information[2] = 117;
region_information[3] = 171;
#else
#if VIDEO==2
printf("\n######## F O O T B A L L V I D E O ##########");
region_information[0] = 255;
region_information[1] = 252;
region_information[2] = 23;
region_information[3] = 41;
#else
#if VIDEO==3
printf("\n########## H O C K E Y V I D E O ############");
region_information[0] = 152;
region_information[1] = 95;
region_information[2] = 19;
region_information[3] = 39;
#else
#if VIDEO==4
printf("\n########## H O C K E Y V I D E O (FULL PICTURE) ############");
region_information[0] = 159;
region_information[1] = 119;
region_information[2] = 320;
region_information[3] = 240;
#endif
#endif
#endif
#endif
#else
printf("\n###### N O V I D E O D E F I N E D ########");
#endif
printf("\n#################################################");
printf("\n#################################################\n");
// init particles
init_particles(region_information, 4);
// Output Object Region
printf("\n\nx0 = %d\ny0 = %d\nwidth = %d\nheight = %d\n\n", region_information[0], region_information[1], region_information[2], region_information[3]);
particle p;
p.x = region_information[0]*PF_GRANULARITY;
p.y = region_information[1]*PF_GRANULARITY;
p.x0 = p.x;
p.y0 = p.y;
p.xp = p.x;
p.yp = p.y;
p.s = PF_GRANULARITY;
p.sp = p.s;
p.width = region_information[2];
p.height = region_information[3];
// get reference data
get_reference_data(&p, &reference_data);
init_reference_data (&reference_data);
print_histogram(&reference_data);
//print_histogram2(&reference_data);
#ifdef STORE_VIDEO
int i;
printf("\n\nThe first %d Frames will be stored into Main Memory. Again this will take some time.\n", (int)MAX_FRAMES);
// load first frames
for(i=0; i<MAX_FRAMES-1; i++){
//switch_framebuffer();
read_frame();
}
printf("\nFinished: The first %d Frames are stored in the Main Memory.\n", (int)MAX_FRAMES);
#endif
parameter_s = (int *) malloc (5 * sizeof(int));
parameter_s[0] = SIZE_X;
parameter_s[1] = SIZE_Y;
parameter_s[2] = 16384; // GRANULARITY / TRANS_STD_X
parameter_s[3] = 8192; // GRANULARITY / TRANS_STD_Y
parameter_s[4] = 16; // GRANULARITY / TRANS_STD_S
parameter_o = (int *) malloc (2 * sizeof(int));
parameter_o[0] = SIZE_X;
parameter_o[1] = SIZE_Y;
slots = (int *) malloc (6 * sizeof(int));
slots[0] = 0;
slots[1] = 1;
slots[2] = 2;
slots[3] = 3;
slots[4] = 4;
slots[5] = 5;
// create sampling, importance, resampling thread
printf("\n#################################################");
printf("\n#################################################");
#ifdef PARTITIONING
#if PARTITIONING==1
printf("\n###### P A R T I T I O N I N G S W #######");
set_sample_sw(1);
set_observe_sw(1);
set_importance_sw(1);
set_resample_sw(1);
#else
#if PARTITIONING==2
printf("\n#### P A R T I T I O N I N G H W I #####");
set_sample_sw(1);
set_observe_sw(1);
set_importance_sw(1);
set_importance_hw_static(1, &slots[2]);
set_resample_sw(1);
#else
#if PARTITIONING==3
printf("\n#### P A R T I T I O N I N G H W II #####");
set_sample_sw(1);
set_observe_sw(1);
set_importance_sw(1);
set_importance_hw_static(2, &slots[2]);
set_resample_sw(1);
#else
#if PARTITIONING==4
printf("\n#### P A R T I T I O N I N G H W O #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(1, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_resample_sw(1);
#else
#if PARTITIONING==5
printf("\n#### P A R T I T I O N I N G H W OO #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(1, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_resample_sw(1);
#else
#if PARTITIONING==6
printf("\n#### P A R T I T I O N I N G H W IO #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(1, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_importance_hw_static(1, &slots[2]);
set_resample_sw(1);
#else
#if PARTITIONING==7
printf("\n#### P A R T I T I O N I N G H W IIO #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(2, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_importance_hw_static(1, &slots[2]);
set_resample_sw(1);
#else
#if PARTITIONING==8
printf("\n#### P A R T I T I O N I N G H W IOO #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(1, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_importance_hw_static(2, &slots[2]);
set_resample_sw(1);
#else
#if PARTITIONING==9
printf("\n#### P A R T I T I O N I N G H W IIOO #####");
set_sample_sw(1);
set_observe_sw(1);
set_observe_hw_static(2, &slots[0], parameter_o, 2);
set_importance_sw(1);
set_importance_hw_static(2, &slots[2]);
set_resample_sw(1);
#endif
#endif
#endif
#endif
#endif
#endif
#endif
#endif
#endif
#else
//printf("\n# N O P A R T I T I O N I N G D E F I N E D #");
printf("\n#### P A R T I T I O N I N G S W O #####");
reconf_mode_observation_on = TRUE;
reconf_mode_observation_last_slot_on = FALSE;
// I. SAMPLING ////////////////////////////////////////
set_sample_sw(1);
//set_sample_hw_dynamic(1, &hw_thread_s_circuit, parameter_s, 5);
// II. OBSERVATION ////////////////////////////////////
//set_observe_sw(1);
set_observe_hw_dynamic(2, &hw_thread_o_circuit, parameter_o, 2);
//set_observe_hw_dynamic(2, &hw_thread_o_circuit, parameter_o, 2);
//set_observe_hw_static(1, &slots[2], parameter_o, 2);
// III. IMPORTANCE ////////////////////////////////////
set_importance_sw(1);
//set_importance_hw_dynamic(1, &hw_thread_i_circuit);
//set_importance_hw_dynamic(2, &hw_thread_i_circuit);
set_importance_hw_static(1, &slots[2]); //[2]
// IV. RESAMPLING /////////////////////////////////////
set_resample_sw(1);
#endif
printf("\n#################################################");
printf("\n#################################################\n");
/*
// create and start sorting thread
init_all_network_interfaces();
if(!eth0_up){
printf("failed to initialize eth0\naborting\n");
return NULL;
}
else{
printf(" eth0 up\n");
}
diag_printf( "initializing ECAP interface..." );
ecap_init();
diag_printf( "done\n" );*/
//create_particle_filter(100, 10);
//set_observe_hw_dynamic(2, &hw_thread_o_circuit, 0, 0);
//sw_test_thread = (cyg_thread *) malloc (sizeof(cyg_thread));
//sw_test_thread_stack = (char *) malloc (sizeof(char) * STACK_SIZE);
//sw_test_thread_handle = (cyg_handle_t *) malloc(sizeof(cyg_handle_t));
/*cyg_thread_create(PRIO, // scheduling info (eg pri)
test_thread, // entry point function
0, // entry data
"TEST", // optional thread name
//sw_test_thread_stack, // stack base
sw_measurement_thread_4_stack, // stack base
STACK_SIZE, // stack size,
//sw_test_thread_handle, // returned thread handle
&sw_measurement_thread_4_handle, // returned thread handle
//sw_test_thread // put thread here
&sw_measurement_thread_4 // put thread here
);
// resume thread
cyg_thread_resume(sw_measurement_thread_4_handle);*/
//cyg_thread_resume(*sw_test_thread_handle);
/*cyg_thread_create(PRIO, // scheduling info (eg pri)
test_thread, // entry point function
0, // entry data
"READ_MEASUREMENTS_4", // optional thread name
sw_measurement_thread_4_stack, // stack base
STACK_SIZE, // stack size,
&sw_measurement_thread_4_handle, // returned thread handle
&sw_measurement_thread_4 // put thread here
);
// resume thread
cyg_thread_resume(sw_measurement_thread_4_handle);*/
//cyg_thread_delay(1000);
//diag_printf( "Reconfigure!\n" );
/*old_number_of_sortings = 0;
start_sorting();
set_sort8k_hw_dynamicB(2);
set_sort8k_hw_dynamic(2);
cyg_thread_delay(1000);*/
start_particle_filter();
printf("\nstart particle filter");
return NULL;
}
