//! 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);
}
}
