/**
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");
}
/**
* Our working loop while when running.
*/
void infinite_deep_sleep(void) {
uint8_t has_logged = 0;
uint32_t left_em_acc = 0, right_em_acc = 0;
//uint16_t left_envelope = 0, right_envelope = 0;
uint32_t acc_counter = 0;
/* Configure all the calibration stuff first */
configure_calibration();
/* Start the first calibration running */
start_calibration();
/* Configure all the registers for deep sleep */
configure_deep_sleep();
/* Wait for the first calibration to finish */
wait_for_calibration();
while (1) {
/* Sleep for 500 milliseconds */
do_deep_sleep(1);
increment_us(500*1000);
if (is_time_valid()) {
/* Fire up the ADC */
prepare_sampling();
/* If we've taken at least one reading before */
if (has_logged > 1) {
/**
* Update the envelope values. NOTE: This must be done before
* the fft as the fft is in-place.
*/
// left_envelope = get_envelope_32(left_envelope, samples_left+4);
// right_envelope = get_envelope_32(right_envelope, samples_right+4);
/**
* Add our samples to the accumulators. We skip the first 4 points of each sample.
* TODO 48MHz clock?
*/
left_em_acc += fft_32(samples_left+2, get_left_tuned_bin()) >> 7;
right_em_acc += fft_32(samples_right+2, get_right_tuned_bin()) >> 7;
if (++acc_counter >= 128) { /* If we're ready to write to memory */
/* Write em to memory */
/* Middle of average is 32 seconds ago */
write_sample_to_mem(get_em_record_flags(), left_em_acc, right_em_acc, 32);
/* Clear accumulators */
acc_counter = left_em_acc = right_em_acc = 0;
/* Wait for the write to finish */
wait_for_write_complete();
/* Write envelope to memory */
// write_sample_to_mem(get_envelope_record_flags(),
// left_envelope, right_envelope, 32);
/* Clear envelope */
// left_envelope = right_envelope = 0;
/* Wait for the write to finish */
// wait_for_write_complete();
}
} else {
has_logged++;
LED_OFF();
}
/* Take a reading */
do_sampling();
/* Shutdown the ADC */
shutdown_sampling();
/* Take battery readings */
do_battery();
} else { /* Invalid time */
