void DNS_shutdown(void) { if(dns_running) dns_cmd = DNS_SHUTDOWN; else return; cyg_mbox_tryput(dns_mbox_id, (void*)dns_cmd); }
/** terminates and deletes all HW threads for Sampling (dynamic) */ void sample_hw_delete_dynamic(int number){ //diag_printf("\nSAMPLING: send message 1 to delete hw thread -------------------------"); int i; for (i = 0; i<number; i++) { cyg_mbox_tryput( hw_mb_sampling_exit_handle[0], (void *) 1 ); } }
void DNS_update(void) { int val=0; CFG_get(CFG_DNS_EN, &val); if(val) { if(dns_running) dns_cmd = DNS_SIGHUP; else { DNS_init(); return; } } else { if(dns_running) dns_cmd = DNS_SHUTDOWN; else return; } cyg_mbox_tryput(dns_mbox_id, (void*)dns_cmd); }
/** 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); } } } }
/** starts particle filter by getting all particles sampled. */ void start_particle_filter(){ int message, message_delivered, done; //printf("\nStarte Particle Filter\n"); // get first measurement get_new_measurement(); #ifndef ONLYPC // send messages to sampling hw/sw message box for (message=1; message<=number_of_blocks; message++){ message_delivered = FALSE; while (message_delivered == FALSE){ done = cyg_mbox_tryput( mb_sampling_handle[0], ( void * ) message ); if (done > 0){ //printf("\n[Sampling Switch] Nachricht %d an Sampling weitergeleitet", message); message_delivered = TRUE; } } } #else int i; int iteration = 0; while (42){ iteration++; //printf("\n--------------------------------"); //printf("\n-- I T E R A T I O N %d --", iteration); //printf("\n--------------------------------"); // (1) [SAMPLING]: sample particles for (i=0; i<N; i++) prediction(&particles[i]); //printf("\n(1) sampling"); // (2) receive new measurement get_new_measurement(); if (end_of_particle_filter==TRUE) return; //printf("\n(2) get new measurement"); // (3) observation: for (i=0; i<N; i++) extract_observation(&particles[i], &observations[i]); //printf("\n(3) observation"); // (4) [IMPORTANCE]: weight particle according to likelihood for (i=0; i<N; i++) particles[i].w = likelihood (&particles[i], &observations[i], ref_data); //printf("\n(4) importance"); // (5) iteration done: estimate, output, etc. iteration_done(particles, observations, ref_data, N); //printf("\n(5) iteration done"); if (iteration%1000 == 999){ // (6) [RESAMPLING]: resample particle according to weights resampling_pc(); //printf("\n(6) resampling"); // (7) presampling prepare_sampling(); //printf("\n(7) prepare sampling\n"); } } #endif // printf("\nParticle Filter gestartet\n"); }
static void lcd_panel_server(cyg_addrword_t p) { int iX, iY, newX, newY, diffX, diffY, timeout, samples; cyg_uint32 event; diag_printf("LCD panel server here\n"); while (TRUE) { cyg_semaphore_wait(&lcd_panel_sem); samples = 0; iX = 0; iY = 0; // Wait for press to go away (no drag support) timeout = 0; while (timeout < LCD_PANEL_TIMEOUT) { *(volatile cyg_uint8 *)TOUCH_CTL = 0x00; // Disable drives while (*(volatile cyg_uint32 *)INTSR1 & INTSR1_EINT2) ; *(volatile cyg_uint8 *)TOUCH_CTL = 0x70; // Idle state (so interrupt works) cyg_thread_delay(2); // Wait 20 ms if (*(volatile cyg_uint32 *)INTSR1 & INTSR1_EINT2) { // Still pressed // Drive TSPY, ground TSMY, and disconnect TSPX and TSMX *(volatile cyg_uint8 *)TOUCH_CTL = 0x50; panel_delay(); newY = adc_sample(2); // Drive TSPX, ground TSMX, and disconnect TSPY and TSMY *(volatile cyg_uint8 *)TOUCH_CTL = 0xA0; panel_delay(); newX = adc_sample(7); #if 0 diag_printf("timeout: %d, ISR: %x, newX: %d, newY: %d\n", timeout, *(volatile cyg_uint32 *)INTSR1, newX, newY); #endif // See if this sample makes any sense if (samples) { diffX = abs(iX/samples - newX); diffY = abs(iY/samples - newY); if ((diffX <= ((iX/samples)/4)) && (diffY <= ((iY/samples)/4))) { samples++; iX += newX; iY += newY; } else { #if 0 diag_printf("Discard - newX: %d, X: %d, newY: %d, Y: %d\n", newX, iX/samples, newY, iY/samples); #endif break; } } else { iX = newX; iY = newY; samples = 1; } timeout = 0; } else { timeout++; } } if (samples) { // Send event to user level event = (iX/samples)<<16 | (iY/samples); if (!cyg_mbox_tryput(lcd_panel_events_mbox_handle, (void *)event)) { diag_printf("LCD event lost: %x\n", event); } } *(volatile cyg_uint8 *)TOUCH_CTL = 0x00; // Disable drives while (*(volatile cyg_uint32 *)INTSR1 & INTSR1_EINT2) ; *(volatile cyg_uint8 *)TOUCH_CTL = 0x70; // Idle state (so interrupt works) cyg_thread_delay(10); cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_EINT2); } }
/** 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); } } } }
/** get new measurement !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! !!!!! U S E R F U N C T I O N !!!!! !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */ void get_new_measurement(void){ int i, j, k, l; int16 re, im; //unsigned char byte_buffer[1]; //int counter = 0; // fill measurement buffer with next bytes of audio input file // (a) shift old values //memcpy(&measurement[MEASUREMENT_BUFFER/2], &measurement[0], (MEASUREMENT_BUFFER/2)); // (b) get new sound data //receive_sound_frame(measurement, (MEASUREMENT_BUFFER/2)); #ifndef STORE_AUDIO receive_sound_frame(measurement, MEASUREMENT_BUFFER); //printf("\nload next frame"); #else memcpy(measurement, &sound_frames[current_audio_frame*MEASUREMENT_BUFFER], MEASUREMENT_BUFFER); current_audio_frame++; if (MAX_FRAMES <= current_audio_frame) { //printf("\nload %d frames", MAX_FRAMES); current_audio_frame = 0; // load first frames for(i=0; i<MAX_FRAMES-1; i++) { receive_sound_frame(&sound_frames[i*MEASUREMENT_BUFFER], MEASUREMENT_BUFFER); } } #endif memcpy(output, measurement, MEASUREMENT_BUFFER); event_found = FALSE; // spectral center (sc) //complex_number sc, sc_old; //sc_old.re = 0; sc_old.im = 0; sc.re = 0; sc.im = 0; // find first two beats //printf("\ninterval min: %ld ", interval_min); //for(i=0;i<NUM_INITIAL_BEATS;i++) printf("\n%d. initial beat: %ld", i, initial_beats[i]); if (initial_phase == TRUE ) { //printf("\ninitial phase"); int16 samples[OBSERVATION_LENGTH]; //observation obs; complex_number obs[OBSERVATION_LENGTH]; int max_amplitude = 0, max_frequency = 0, current_amplitude; int max_amplitude_total = 0, max_frequency_total = 0; int16 max_sample_value = 0, max_sample_value_total = 0; long int position = 0; event_found = FALSE; // check current measurement frame for (i=0; i<MEASUREMENT_BUFFER/(OBSERVATION_LENGTH); i++) { memcpy(samples, &measurement[i*OBSERVATION_LENGTH], (OBSERVATION_LENGTH*sizeof(int16))); //change Little Endian <-> Big Endian max_sample_value = 0; for (j=0; j<OBSERVATION_LENGTH; j++) { samples[j] = ntohl(samples[j]); if (ABS(samples[j]) > max_sample_value) { max_sample_value = ABS(samples[j]); } } for (j=0; j<OBSERVATION_LENGTH; j++) { obs[j].re = 111; obs[j].im = 222; } #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif // fft //fft_analysis_try (samples, OBSERVATION_LENGTH, obs, sample_rate); // -a: send message: sample address (input) while (cyg_mbox_tryput( *mb_fft_start_handle, (void *) samples ) == 0) { } // -b: send message: observations address (output) while (cyg_mbox_tryput( *mb_fft_start_handle, (void *) obs ) == 0) { } // -c: receive message (fft done) while (cyg_mbox_get( *mb_fft_done_handle ) == 0) { } #ifdef USE_CACHE XCache_EnableDCache( 0xF0000000 ); #endif // find spectral center (sc) /*sc.re = 32000; sc.im = 32000; for (j=1;j<((OBSERVATION_LENGTH/2)-1);j++) { }*/ // max amplitude, frequency //for (j=0; j<OBSERVATION_LENGTH; j++) /*printf("\n\nSAMPLES\n"); for (j=0; j<OBSERVATION_LENGTH; j++) { printf("\n%d", samples[j]); } printf("\n\nFFT RESULTS\n");*/ for (j=0; j<OBSERVATION_LENGTH; j++) { // i. calculate current amplitude re = (int16) obs[j].re; im = (int16) obs[j].im; current_amplitude = (re*re) + (im*im); current_amplitude = sqrt(current_amplitude); // ii. max amplitude if (current_amplitude > max_amplitude) { max_amplitude = current_amplitude; // calculate max frequency max_frequency = j*(sample_rate/2); max_frequency /= (OBSERVATION_LENGTH); } //printf("\n%d", current_amplitude); } //printf("\nmax frequency %d, \tmax amplitude: %d", max_frequency, max_amplitude); // likelihood for 'beat' (frequency > 100 => amplitude) if (max_frequency>100 && max_frequency<5000 && max_amplitude_total<max_amplitude)//<5000 { max_amplitude_total = max_amplitude; max_frequency_total = max_frequency; max_sample_value_total = max_sample_value; position = interval_min + i*OBSERVATION_LENGTH - (OBSERVATION_LENGTH/2); } } //printf("\n!!!!!!!max frequency %d, \tmax amplitude: %d", max_frequency_total, max_amplitude_total); // analyse if event was found if (max_frequency_total>100 && max_amplitude_total>50 && max_frequency_total<5000 && max_sample_value_total>5000){ //printf("\nEVENT FOUND at position %ld", position); event_found = TRUE; event_position = position; event_salience = max_amplitude_total; } // init particle filter if needed; if (event_found == TRUE) // new beat { int beat_ind = 0; while (initial_beats[beat_ind]>0 && beat_ind<(NUM_INITIAL_BEATS-1)) { beat_ind++; } // 'beat_ind' is the current initial beat initial_beats[beat_ind] = event_position; if ((beat_ind>0 && initial_beats[beat_ind] - initial_beats[beat_ind-1] < bytespersecond/5)) { event_found = FALSE; initial_beats[beat_ind] = 0; return; } //printf("\n%d. beat at position: %ld", beat_ind, event_position); // if last initial beat found: extract hypothesises if (beat_ind == (NUM_INITIAL_BEATS-1)) { // if last initial beat => initialize particles with extracted hypothesis // number of possibilities to create hypothesis for beats // mathematical reasoning to calcualate the number of possibilities int num_hypothesis = (NUM_INITIAL_BEATS*NUM_INITIAL_BEATS) - (((NUM_INITIAL_BEATS+1)*NUM_INITIAL_BEATS)/2); // generic algorithm to calcualte all possibilities for tempo hypothesises // i: current initial beat // j: other initial beats before current initial beat // k: current hypothesis [0,...,num_hypothesis-1] // l: current particle position: (N/num_hypothesis) particles per hypothesis k = 0; for (i=NUM_INITIAL_BEATS-1; i>=0;i--) { for (j=i-1; j>=0;j--) { for(l=(k*N)/num_hypothesis; l<MIN((((k+1)*N)/num_hypothesis),N);l++) { // init particle according to hypothesis particles[l].tempo = (int)initial_beats[i] - initial_beats[j]; if((5*particles[l].tempo)<bytespersecond) { //printf("\ndifference short, set to default value"); particles[l].tempo = bytespersecond/5; } particles[l].last_beat = initial_beats[i]; particles[l].next_beat = initial_beats[i] + particles[l].tempo; particles[l].w = 100; particles[l].likelihood = 100; } //printf("\n%d. hypothesis: tempo = %d,\tlast beat: %ld", // (k+1),(int)(initial_beats[i]-initial_beats[j]), // initial_beats[i]); k++; } } } /*if (first_beat_pos <= 0) { first_beat_pos = event_position; printf("\n1st beat at position: %ld", first_beat_pos); } else { if (second_beat_pos <= 0 && (event_position - first_beat_pos > (bytespersecond/5))) { second_beat_pos = event_position; printf("\n2nd beat at position: %ld (init particles, tempo: %ld)", second_beat_pos, (second_beat_pos - first_beat_pos)); // init particles for (i=0; i<N; i++) { particles[i].tempo = second_beat_pos - first_beat_pos; particles[i].last_beat = second_beat_pos; particles[i].next_beat = second_beat_pos + particles[i].tempo; particles[i].last_event_time = second_beat_pos; particles[i].w = PF_GRANULARITY/N; particles[i].likelihood = 100; } } }*/ //last_event_found = TRUE; } /*else { last_event_found = FALSE; }*/ } //int16 sample_value = 0; // make short-time Fourier transformation for the incoming sound data /*fft_analysis (measurement, (MEASUREMENT_BUFFER/2), fft_window, sample_rate); // real, and imaginary component // debug: find max frequency double amplitude_max = 0; int max_freq = 0; for(i = 0; i < (sample_rate/(2*FREQUENCY_HOPPING)); i++){ if (amplitude_max < fft_window[i]){ max_freq = i*FREQUENCY_HOPPING; amplitude_max = fft_window[i]; } } event_found = FALSE; // extract events if (max_freq > 100 && amplitude_max > 60){ //if (last_event_found == FALSE){ printf("\nevent found!!!!!!!!!!!"); // event found event_found = TRUE; event_position = interval_min + (MEASUREMENT_BUFFER/4); event_salience = amplitude_max; //} } else { last_event_found = FALSE; } // init particle filter if needed; if (first_beat_pos <= 0 || second_beat_pos <= 0 ){ // init still needed if (event_found == TRUE && last_event_found == FALSE){ // new beat if (first_beat_pos <= 0){ first_beat_pos = event_position; printf("\n1st beat at position: %d", (int)(first_beat_pos/10000)); } else { if (second_beat_pos <= 0){ second_beat_pos = event_position; printf("\n2nd beat at position: %d (init particles, tempo: %d)", (int)(second_beat_pos/10000), (int)((second_beat_pos - first_beat_pos)/10000)); // init particles for (i=0; i<N; i++){ particles[i].tempo = second_beat_pos - first_beat_pos; particles[i].last_beat = second_beat_pos; particles[i].next_beat = second_beat_pos + particles[i].tempo; particles[i].last_event_time = second_beat_pos; particles[i].w = PF_GRANULARITY/N; particles[i].likelihood = 100; } } } } } if (event_found == TRUE) {last_event_found = TRUE; } */ // calculate 'energy' in buffer //int16 output_value = max_ind; //for(i = 0; i < MEASUREMENT_BUFFER/2; i++) memcpy(&output[2*i], &output_value, 2); /* // debug: print sample values to screen for(i = 0; i < MEASUREMENT_BUFFER/2; i++){ double result = fft_window[i]; memcpy(&sample_value, &measurement[2*i], 2); if (interval_min > bytespersecond && interval_min < bytespersecond+1000) printf("\n%f", 1.0*sample_value); //if (i>0) result *= 8; if (result > 32767){printf("\nFFT value to high ( < 32768), but it is %f", result); result = 32767;} if (result<-32768){printf("\nFFT value to high ( >= -32768), but it is %f", result); result=-32768;} sample_value = (signed short int)result; //printf("\n sample value: %d", sample_value); //memcpy(&measurement[2*i], &sample_value, 2); //printf("\nA[%d] = %d", i, sample_value]); }*/ // TRY START: CHANGE CHANGE CHANGE /*event_found = FALSE; long int energy = 0; char current_sample[2]; // calculate 'energy' in buffer int16 max_value = 0; for (i=0; i<MEASUREMENT_BUFFER/2; i++){ current_sample[1] = measurement[(2*i) + 1]; current_sample[0] = measurement[(i*2)]; memcpy(&sample_value, current_sample, 2); if (ABS(sample_value) > max_value) max_value = ABS(sample_value); energy += (sample_value * sample_value); } max_amplitude = max_value; // adjust energy change long int value = sqrt(sqrt(energy-last_energy));// / 2; //long int value = (energy-last_energy)/80000; sample_value = value; if (value > 32767){ sample_value = 32768; printf("\nenergy change too high (%d > 32767)", value); } else if (value < -32768) { sample_value = -32768; printf("\nenergy change too high (%d < -32768)", value); } if (max_value < 15000 || sample_value == 0) { sample_value = 0; last_event_found = FALSE; //energy = 0; } else { if (last_event_found == FALSE){ event_found = TRUE; last_event_found = TRUE; event_position = interval_min + (MEASUREMENT_BUFFER/2); event_salience = ABS(sample_value); //printf("\nEvent found: position = %d, salience = %d", event_position, event_salience); } } last_energy = energy;*/ // debug: write energy into file /*for (i=0; i<MEASUREMENT_BUFFER/2; i++){ memcpy(current_sample, &sample_value, 2); measurement[(2*i) + 1] = current_sample[0]; measurement[(i*2)] = current_sample[1]; }*/ // TRY TRY TRY //fft_calc(measurement, MEASUREMENT_BUFFER/2); // TRY END: CHANGE CHANGE CHANGE /* char current_sample[2]; int16 sample_value; long int left_max = 0, right_max=0; int s; event_found = FALSE; // calcualte left sum for (s=0; s<MEASUREMENT_BUFFER/4; s++){ current_sample[0] = measurement[(2*s) + 1]; current_sample[1] = measurement[(2*s)]; memcpy(&sample_value, current_sample, 2); if (ABS(sample_value) > left_max) left_max = ABS(sample_value); } // calculate right sum for (s=MEASUREMENT_BUFFER/4; s<MEASUREMENT_BUFFER/2; s++){ current_sample[0] = measurement[(2*s) + 1]; current_sample[1] = measurement[(2*s)]; memcpy(&sample_value, current_sample, 2); if (ABS(sample_value) > right_max) right_max = ABS(sample_value); } if ((left_max > 0) && (((4.0 * right_max) / (1.0 * left_max)) > 4.5) && ((right_max + left_max) > 40000)){ event_found = TRUE; event_position = interval_min + (MEASUREMENT_BUFFER/2); if (left_max > 0) event_salience = (4*right_max) / left_max; else event_salience = 2; printf("\nEvent found: position = %d, salience = %d", event_position, event_salience); } }*/ //for (i=0; i<MEASUREMENT_BUFFER; i++) fwrite(&measurement[i],1,(BUFFSIZE-1),outputstream); }
static void kbd_server(cyg_addrword_t p) { int col, bit, key, event, timeout; diag_printf("KBD server here\n"); while (TRUE) { cyg_semaphore_wait(&kbd_sem); // As long as keys are pressed, scan and update timeout = 0; while (TRUE) { // Wait for 20ms - time to debounce keyboard cyg_thread_delay(2); // Scan keyboard kbd_scan(); // Reset state for (key = 0; key < sizeof(kbd_new_state); key++) { kbd_new_state[key] = 0; } // Check state of all keys and send events for those that change for (col = 0; col < 8; col++) { for (bit = 0; bit < 8; bit++) { if (col_state[col] & (1<<bit)) { key = kbd_map[col][bit]; if (key != KBD_Empty) { kbd_new_state[key] = 1; } } if (ext_state[col] & (1<<bit)) { key = kbd_map[col][bit+8]; if (key != KBD_Empty) { kbd_new_state[key] = 1; } } } } // Compare new and old (known) states, generate events for changes // Send events for modifier keys first. for (key = 0; key < sizeof(kbd_new_state); key++) { if (kbd_state[key] != kbd_new_state[key]) { event = 0xFF; switch (key) { case KBD_LeftShift: case KBD_RightShift: case KBD_Ctrl: case KBD_LeftAlt: case KBD_RightAlt: case KBD_Function: case KBD_CapsLock: if (kbd_state[key]) { // Key going up event = key; } else { // Key going down event = key + KBD_Press; } kbd_state[key] = kbd_new_state[key]; } if (event != 0xFF) { if (!cyg_mbox_tryput(kbd_events_mbox_handle, (void *)event)) { diag_printf("KBD event lost: %x\n", event); } } } } // First key up events for (key = 0; key < sizeof(kbd_new_state); key++) { if (kbd_state[key] != kbd_new_state[key]) { if (kbd_state[key]) { // Key going up event = key; } else { // Key going down event = key + KBD_Press; } if (!cyg_mbox_tryput(kbd_events_mbox_handle, (void *)event)) { diag_printf("KBD event lost: %x\n", event); } } kbd_state[key] = kbd_new_state[key]; } // Clear interrupt (true when keys are pressed) cyg_drv_interrupt_acknowledge(CYGNUM_HAL_INTERRUPT_KBDINT); #if 0 if (*(volatile cyg_uint32 *)INTSR2 & INTSR2_KBDINT) { timeout = 0; } else if (++timeout == 5) { // No keys for 100ms break; } #endif } // Allow interrupts to happen again cyg_drv_interrupt_acknowledge(CYGNUM_HAL_INTERRUPT_KBDINT); cyg_drv_interrupt_unmask(CYGNUM_HAL_INTERRUPT_KBDINT); } }
int main( int argc, char *argv[] ) { unsigned int i, start_count = 0, done_count = 0, j; timing_t t_start = 0, t_stop = 0, t_gen = 0, t_sort = 0, t_merge = 0, t_check = 0, t_tmp; printf( "-------------------------------------------------------\n" "ReconOS hardware multithreading case study (sort)\n" "(c) Computer Engineering Group, University of Paderborn\n\n" "eCos, single-threaded hardware version (" __FILE__ ")\n" "Compiled on " __DATE__ ", " __TIME__ ".\n" "-------------------------------------------------------\n\n" ); #ifdef USE_CACHE printf( "enabling data cache for external ram\n" ); XCache_EnableDCache( 0x80000000 ); #else printf( "data cache disabled\n" ); XCache_DisableDCache( ); #endif data = buf_a; //---------------------------------- //-- GENERATE DATA //---------------------------------- printf( "Generating data..." ); t_start = gettime( ); generate_data( data, SIZE ); t_stop = gettime( ); t_gen = calc_timediff_ms( t_start, t_stop ); printf( "done\n" ); #ifdef USE_CACHE // flush cache contents - the hardware can only read from main memory // TODO: storing could be more efficient printf( "Flushing cache..." ); XCache_EnableDCache( 0x80000000 ); printf( "done\n" ); #endif //---------------------------------- //-- SORT DATA //---------------------------------- // create mail boxes for 'start' and 'complete' messages cyg_mbox_create( &mb_start_handle, &mb_start ); cyg_mbox_create( &mb_done_handle, &mb_done ); // create sorting thread reconos_hwthread_create( 16, // priority 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 (void*)UPBHWR_OSIF_0_BASEADDR, XPAR_OPB_INTC_0_OSIF_0_INTERRUPT_INTR+1, // ( void * ) XPAR_PLB_RECONOS_SLOT_0_BASEADDR, // base address // XPAR_OPB_INTC_0_PLB_RECONOS_SLOT_0_INTERRUPT_INTR + 1, // interrupt hwthread_sorter_resources, // resource array 2, // number of resources 0xFFFFFFFF, 0xFFFFFFFF ); cyg_thread_resume( hwthread_sorter_handle ); printf( "Sorting data..." ); i = 0; while ( done_count < SIZE / N ) { t_start = gettime( ); // if we have something to distribute, // put as many as possile into the start mailbox while ( start_count < SIZE / N ) { if ( cyg_mbox_tryput( mb_start_handle, ( void * ) &data[i] ) == true ) { start_count++; i += N; } else { // mailbox full break; } } t_stop = gettime( ); t_sort += calc_timediff_ms( t_start, t_stop ); // see whether anybody's done t_start = gettime( ); if ( ( t_tmp = ( timing_t ) cyg_mbox_get( mb_done_handle ) ) != 0 ) { done_count++; } else { printf( "cyg_mbox_get returned NULL!\n" ); } t_stop = gettime( ); t_sort += calc_timediff_ms( t_start, t_stop ); } printf( "done\n" ); #ifdef USE_CACHE // flush cache contents // TODO: invalidating would suffice printf( "Flushing cache..." ); XCache_EnableDCache( 0x80000000 ); printf( "done\n" ); #endif //---------------------------------- //-- MERGE DATA //---------------------------------- printf( "Merging data..." ); t_start = gettime( ); data = recursive_merge( data, buf_b, SIZE, N, simple_merge ); t_stop = gettime( ); t_merge = calc_timediff_ms( t_start, t_stop ); printf( "done\n" ); //---------------------------------- //-- CHECK DATA //---------------------------------- printf( "Checking sorted data..." ); t_start = gettime( ); if ( check_data( data, SIZE ) != 0 ) printf( "CHECK FAILED!\n" ); else printf( "check successful.\n" ); t_stop = gettime( ); t_check = calc_timediff_ms( t_start, t_stop ); printf( "\nRunning times (size: %d words):\n" "\tGenerate data: %d ms\n" "\tSort data : %d ms\n" "\tMerge data : %d ms\n" "\tCheck data : %d ms\n" "\nTotal computation time (sort & merge): %d ms\n", SIZE, t_gen, t_sort, t_merge, t_check, t_sort + t_merge ); return 0; }
/** 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"); } }