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");
}
}
