//! waits until the temperature does not change more than 'temp' degree Celsius in 'dt' seconds void wait_for_stable_temperature(float temp, int dt){ unsigned int time_in_sec = 0, t_start = 0, t_stop; float diode_test_1 = 0, diode_test_2 = 0, diode_difference; int milli_temp = (int)(1000*temp); milli_temp %= 1000; diode_test_2 = sysmon_temp_reg() * TEMPERATURE_FACTOR; diode_difference = diode_test_2 - diode_test_1; if (diode_difference<0) diode_difference = -diode_difference; #ifdef UPBDBG_RECONOS_DEBUG diag_printf("\nwait until temporal temperature difference is below %d.%03d degree Celsius inside %d seconds interval \n", (int) temp, milli_temp, dt); #endif // temperature is considered stable when difference in x seconds is less than defined temperature difference while (diode_difference > temp ){ diode_test_1 = diode_test_2; // 1. wait for 20 seconds time_in_sec = 0; // busy waiting t_start = *timebase; while (time_in_sec < dt){ t_stop = *timebase; time_in_sec = (calc_timediff(t_start, t_stop) / 100000000); } #ifdef UPBDBG_RECONOS_DEBUG diag_printf("."); #endif diode_test_2 = sysmon_temp_reg() * TEMPERATURE_FACTOR; diode_difference = diode_test_2 - diode_test_1; if (diode_difference<0) diode_difference = -diode_difference; } #ifdef UPBDBG_RECONOS_DEBUG diag_printf("done\n"); #endif }
/** sample sw thread @param data: input data for sw thread */ void sampling_sw_thread (cyg_addrword_t data){ //unsigned int thread_number = (unsigned int) data; int from, to; int done; int i; int new_message = FALSE; int message = 1; int message_to_deliver = FALSE; timing_t time_start_s = 0, time_stop_s = 0, time_sampling_sw = 0; int number_of_measurements_s = 0; while (42) { // 1) if there is no message to delivered, check for new message while (message_to_deliver == FALSE && new_message == FALSE){ message = (int) cyg_mbox_get( mb_sampling_handle[0] ); if (message > 0 && message <= number_of_blocks){ //printf("\n[Sampling Thread No. %d] Nachricht %d erhalten", (int) thread_number, message); new_message = TRUE; time_start_s = gettime(); } } #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif // 2) if a new message has arrived, sample the particles while (new_message == TRUE){ new_message = FALSE; from = (message - 1) * block_size; to = from + block_size - 1; if ((N - 1) < to) to = N - 1; // sample particles for (i=from; i<=to; i++){ // predict particle prediction (&particles[i]); } message_to_deliver = TRUE; } #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif time_stop_s = gettime(); // 3) if a message should be delivered, deliver it while ( message_to_deliver == TRUE){ done = cyg_mbox_tryput( mb_sampling_done_handle[0], ( void * ) message ); if (done > 0){ //printf("\n[Sampling Thread No. %d] Nachricht %d geschickt", (int) thread_number, message); message_to_deliver = FALSE; time_sampling_sw = calc_timediff( time_start_s, time_stop_s ); number_of_measurements_s++; //diag_printf("\nSampling SW: %d, \tmessage %d, \ttime: %d", number_of_measurements_s, message, time_sampling_sw); } } } }
/** observation sw thread @param data: input data for sw thread */ void observation_sw_thread (cyg_addrword_t data){ //unsigned int thread_number = (unsigned int) data; int from, to; int done; int i; int new_message = FALSE; int message = 1; int message_to_deliver = FALSE; int number_of_measurements_o = 0; //int sum_of_measurements_o = 0; //int average_of_measurements_o = 0; timing_t time_start_o = 0, time_stop_o = 0, time_observation_sw = 0; while (42) { // 1) if there is no message to delivered, check for new message while (message_to_deliver == FALSE && new_message == FALSE){ message = (int) cyg_mbox_get( mb_sampling_done_handle[0] ); if (message > 0 && message <= (N/block_size)){ new_message = TRUE; time_start_o = gettime(); //printf("\n[Observation Thread No. %d] Nachricht %d erhalten", (int) thread_number, message); //diag_printf("\n[Observation] Nachricht %d erhalten", message); } } // 2) if a new message has arrived, sample the particles new_message = FALSE; from = (message - 1) * block_size; to = from + block_size - 1; if ((N - 1) < to) to = N - 1; #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif //printf("\nOBSERVATION"); // extract observations for (i=from; i<=to; i++){ extract_observation(&particles[i], &observations[i]); } message_to_deliver = TRUE; #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif time_stop_o = gettime(); // 3) if a message should be delivered, deliver it while ( message_to_deliver == TRUE){ done = cyg_mbox_tryput( mb_importance_handle[0], ( void * ) message ); if (done > 0){ message_to_deliver = FALSE; time_observation_sw = calc_timediff( time_start_o, time_stop_o ); number_of_measurements_o++; //sum_of_measurements_o += time_observation_sw; //average_of_measurements_o = sum_of_measurements_o / number_of_measurements_o; //diag_printf("\nObservation SW: %d, \tmessage %d, \ttime: %d", // number_of_measurements_o, (message-1), time_observation_sw); //printf("\n[Observation Thread No. %d] Nachricht %d geschickt", (int) thread_number, message); } } } }
/** preResampling sw thread. Waits for all importance_done messages, normalizes the particle weights, calls iteration_done function and finally sends messages to the resampling message box. */ void preResampling_thread (cyg_addrword_t data){ int message = 1; int i, done; int message_delivered = FALSE; int * messages = (int *) malloc(sizeof(int)*(number_of_blocks)); //int * messages = (int *) malloc(sizeof(int)*N); timing_t time_start = 0, time_stop = 0, time_particle_filter = 0; int number_of_measurements = 0; timing_t t_start = 0, t_stop = 0, t_result_1 = 0, t_result; for (i=0; i<number_of_blocks; i++){ messages[i] = FALSE; } while (42){ // 1) check, if all messages are received to message box // If this is not true, get the next message while (!all_messages_received(messages, number_of_blocks) ){ message = (int) cyg_mbox_get( mb_importance_done_handle[0] ); if (message > 0){ messages[message-1] = TRUE; } } //printf("\n[Resampling Switch] alle Nachrichten erhalten"); t_start = gettime(); // set messages back to 'not received' for (i=0; i<number_of_blocks; i++){ messages[i] = FALSE; } #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif #ifdef OBSERVATION_DEBUG // debug: check, if observation is correct i = 0; observation * obs1 = &observations[i]; observation obs2; extract_observation(&particles[i], &obs2); if (obs1->no_tracking_needed != obs2.no_tracking_needed) { diag_printf("\nparticle[%d] \tno_tracking_needed - hw=%d sw=%d", i, obs1->no_tracking_needed, obs2.no_tracking_needed); diag_printf("\nparticle[%d] \told_likelihood - hw=%d sw=%d", i, obs1->old_likelihood, obs2.old_likelihood); } else if (obs1->no_tracking_needed == FALSE) { int j; for(j=0;j<OBSERVATION_LENGTH;j++) { if ((ABS(obs2.fft[j].re-obs1->fft[j].re)>1)||(ABS(obs2.fft[j].im-obs1->fft[j].im)>1)) { diag_printf("\nobservation[%d].fft[%d].re - hw=%d sw=%d", i,j,(int)obs1->fft[j].re,(int)obs2.fft[j].re); diag_printf("\nobservation[%d].fft[%d].im - hw=%d sw=%d", i,j,(int)obs1->fft[j].im,(int)obs2.fft[j].im); } } } // debug end #endif #ifdef IMPORTANCE_DEBUG2 // TODO: remove debug // debug: check if likelihood is calculated correctly int sw_likelihood, difference; for (i=0; i<N; i++){ if (observations[i].no_tracking_needed == FALSE) { //printf("\nreceived hw results. get sw results"); sw_likelihood = likelihood(&particles[i], &observations[i], NULL); // difference between likelihood calculated in sw and likelihood calculated in hw difference = particles[i].w - sw_likelihood; //if (difference > 1 || difference < -1) diag_printf("\nlikelihood[%d]: sw = %d; hw = %d", i, sw_likelihood, particles[i].w); } } // debug end if (particles[i].w <= 0) particles[i].w = 1; #endif // 2) calls iteration_done user function iteration_done(particles, observations, ref_data, N); // 3) normalize particle weights normalize_particle_weights(); t_stop = gettime(); t_result_1 = calc_timediff(t_start, t_stop); //printf("\nNormalize weights + iteration done: %d", t_result_1);; // calculate time for one particle filter loop time_stop = gettime(); time_particle_filter = calc_timediff( time_start, time_stop ); if (number_of_measurements > 0) //printf("\n%d \t%d", number_of_measurements, time_particle_filter); printf("\n%d", time_particle_filter); //diag_printf("\n%d \t%d", number_of_measurements, time_particle_filter); time_start = gettime(); number_of_measurements++; t_start = gettime(); // 4) calculate Resampling Function U calculate_resampling_function(); t_stop = gettime(); t_result = calc_timediff(t_start, t_stop); t_result += t_result_1; //printf("\nPreresampling: %d", t_result); #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif // 5) send all messages to sw/hw message box for (message=1; message<=number_of_blocks; message++){ message_delivered = FALSE; while (message_delivered == FALSE){ done = cyg_mbox_tryput( mb_resampling_handle[0], ( void * ) message ); if (done > 0){ //diag_printf("\n[Resampling Switch] Nachricht %d an Reampling weitergeleitet", message); message_delivered = TRUE; } } } //printf("\n[Sampling Switch] alle Nachrichten geschickt"); } }
int main( int argc, char *argv[] ) { unsigned int start, stop; int i = 0; int test = 0; int NUM_LOAD_THREADS = 0; thread_arg_t arg; hthread_mutexattr_t block_attr; hthread_mutexattr_t data_attr; benchmark_t my_benchmark[TESTS]; // Turn on the Xilinx Timer enableTimer(); for (test = 0; test < TESTS; test++) { // Set the # of load threads NUM_LOAD_THREADS = test * 10; // Init. benchmark structure my_benchmark[test].num_threads_in_test = NUM_LOAD_THREADS; *my_benchmark[test].name = "lock/unlock"; for (i = 0; i < RUNS; i++) { // Initialize thread argument fields // * Setup mutexes // * Initialize counter block_attr.num = 0; block_attr.type = HTHREAD_MUTEX_DEFAULT; data_attr.num = 1; data_attr.type = HTHREAD_MUTEX_DEFAULT; hthread_mutex_init(&arg.block_mutex, &block_attr); hthread_mutex_init(&arg.data_mutex, &data_attr); arg.counter = 0; arg.num_load_threads = NUM_LOAD_THREADS; // Run the test start = readTimer(); run_A(0,NUM_LOAD_THREADS,&arg); stop = readTimer(); // Gather results my_benchmark[test].results1[i] = calc_timediff(arg.lock_start, arg.lock_stop); my_benchmark[test].results2[i] = calc_timediff(arg.unlock_start, arg.unlock_stop); /* printf( "*** start = %u\n" "*** stop = %u\n" "*** elapsed = %u\n", start, stop, stop - start ); // Calculate timing results printf( "\t*********************************\n" "\t lock Start Time = %llu ticks\n" "\t lock Stop Time = %llu ticks\n" "\t diff = %llu ticks\n" "\t lock Elapsed Time = %llu us\n", arg.lock_start, arg.lock_stop, arg.lock_stop - arg.lock_start, calc_timediff_us(arg.lock_start, arg.lock_stop) ); printf( "\t*********************************\n" "\t unlock Start Time = %llu ticks\n" "\t unlock Stop Time = %llu ticks\n" "\t diff = %llu ticks\n" "\t unlock Elapsed Time = %llu us\n", arg.unlock_start, arg.unlock_stop, arg.unlock_stop - arg.unlock_start, calc_timediff_us(arg.unlock_start, arg.unlock_stop) ); */ } } report_benchmark_results(my_benchmark); for (test = 0; test < TESTS; test++) { // Set the # of load threads NUM_LOAD_THREADS = test * 10; // Init. benchmark structure my_benchmark[test].num_threads_in_test = NUM_LOAD_THREADS; *my_benchmark[test].name = "turn_around"; for (i = 0; i < RUNS; i++) { // Initialize thread argument fields // * Setup mutexes // * Initialize counter block_attr.num = 0; block_attr.type = HTHREAD_MUTEX_DEFAULT; data_attr.num = 1; data_attr.type = HTHREAD_MUTEX_DEFAULT; hthread_mutex_init(&arg.block_mutex, &block_attr); hthread_mutex_init(&arg.data_mutex, &data_attr); arg.counter = 0; // Num loads threads + 1 b/c the 2nd measurement thread is a load thread and needs to increment the counter barrier // before the main thread releases the blocking mutex arg.num_load_threads = NUM_LOAD_THREADS+1; // Run the test start = readTimer(); run_B(0,NUM_LOAD_THREADS,&arg); stop = readTimer(); // Gather results my_benchmark[test].results1[i] = calc_timediff(arg.unlock_start, arg.measure_lock_stop); my_benchmark[test].results2[i] = calc_timediff(arg.unlock_start, arg.measure_lock_stop); /* printf( "*** start = %u\n" "*** stop = %u\n" "*** elapsed = %u\n", start, stop, stop - start ); // Calculate timing results printf( "\t*********************************\n" "\t lock Start Time = %llu ticks\n" "\t lock Stop Time = %llu ticks\n" "\t diff = %llu ticks\n" "\t lock Elapsed Time = %llu us\n", arg.lock_start, arg.lock_stop, arg.lock_stop - arg.lock_start, calc_timediff_us(arg.lock_start, arg.lock_stop) ); printf( "\t*********************************\n" "\t unlock Start Time = %llu ticks\n" "\t unlock Stop Time = %llu ticks\n" "\t diff = %llu ticks\n" "\t unlock Elapsed Time = %llu us\n" "\t m_lock Start Time = %llu ticks\n" "\t m_lock Stop Time = %llu ticks\n" "\t turn around = %llu us\n", arg.unlock_start, arg.unlock_stop, arg.unlock_stop - arg.unlock_start, calc_timediff_us(arg.unlock_start, arg.unlock_stop), arg.measure_lock_start, arg.measure_lock_stop, calc_timediff_us(arg.unlock_start, arg.measure_lock_stop)); */ } } report_benchmark_results(my_benchmark); return 0; }
/** * This SW thread which reads the new frame (if it is not allready stored in Main Memory) and sends particle data back to the PV via TCP/IP packages. * * @param data: entry data for thread (e.g. an address) */ void read_new_frame(cyg_addrword_t data) { int frame_counter = 0; #ifndef STORE_VIDEO timing_t t_start = 0, t_stop = 0, t_result = 0; #else int i; #endif while (42){ // 1) wait for semaphore //diag_printf("\n+++++++++++++++wait for sem_read_new_frame_start semaphore"); cyg_semaphore_wait(sem_read_new_frame_start); //diag_printf("\n+++++++++++++++received sem_read_new_frame_start semaphore"); frame_counter++; //printf("\nFrame %d", frame_counter); // 2) read new frame #ifdef STORE_VIDEO // all frames are read framecounter++; //printf("\nframe counter: %d", frame_counter); if ((frame_counter % MAX_FRAMES) == 0 && frame_counter > 0){ //diag_printf("\nload next %d Frames", MAX_FRAMES); #ifndef NO_ETHERNET #ifdef NO_VGA_FRAMEBUFFER send_particles_back(particles, N); #endif #endif reset_the_framebuffer(); set_observations_input(tft_editing.fb); // load first frames for(i=0; i<MAX_FRAMES; i++){ //switch_framebuffer(); //diag_printf("\n+++++++++++++++read frame"); #ifndef NO_ETHERNET read_frame(); #endif } set_observations_input(tft_editing.fb); } //send_best_particle_back(particles, N); #else // send particle data to pc program t_start = gettime(); #ifndef NO_ETHERNET #ifdef NO_VGA_FRAMEBUFFER send_particles_back(particles, N); #endif #endif //send_best_particle_back(particles, N); t_stop = gettime(); t_result = calc_timediff(t_start, t_stop); //printf("\nSend Particles back: %d", t_result); //send_particles_back(); t_start = gettime(); // read next frame #ifndef NO_ETHERNET read_frame(); #endif t_stop = gettime(); t_result = calc_timediff(t_start, t_stop); //printf("\nRead Frame: %d", t_result); #endif //diag_printf("\n+++++++++++++++send sem_read_new_frame_stop semaphore"); // 3) post semaphore cyg_semaphore_post(sem_read_new_frame_stop); } }