/*************************************************
********************FOR GUI***********************
*************************************************/
QVector<QVector<double>> sleep_apnea::sleep_apnea_plots(QVector<unsigned int> tab_R_peaks)
{
//getting data
QVector<QVector<double>>tab_RR,tab_RR_new,tab_res;
QVector<QVector<double>>h_amp(2);
QVector<QVector<double>>h_freq(2);
QVector<QVector<double>> apnea_plots(3);
tab_RR=RR_intervals(tab_R_peaks);
tab_RR_new=averange_filter(tab_RR);
tab_res=resample(tab_RR_new);
HP_LP_filter(tab_res);
hilbert(tab_res,h_amp,h_freq);
freq_amp_filter(h_freq,h_amp);
median_filter(h_freq,h_amp);
//resizing output
apnea_plots[0].resize(h_amp[0].size());
apnea_plots[1].resize(h_amp[1].size());
apnea_plots[2].resize(h_freq[1].size());
//writing output
int i;
for(i=0;i<apnea_plots[0].size();i++)apnea_plots[0][i]=h_amp[0][i];
for(i=0;i<apnea_plots[1].size();i++)apnea_plots[1][i]=h_amp[1][i];
for(i=0;i<apnea_plots[2].size();i++)apnea_plots[2][i]=h_freq[1][i];
return apnea_plots;
}
int glass(t_bunny_pixelarray **pix,
const int value)
{
t_bunny_pixelarray *save;
(void)value;
if (!(save = bunny_new_pixelarray((*pix)->clipable.clip_width,
(*pix)->clipable.clip_height)))
return (1);
apply_shatter(*pix, save, (*pix)->clipable.clip_width,
(*pix)->clipable.clip_height);
bunny_delete_clipable(&(*pix)->clipable);
*pix = save;
if (median_filter(pix, value) || median_filter(pix, value) ||
median_filter(pix, value))
return (1);
return (0);
}
QVector<double> sleep_apnea::gui_output(QVector<unsigned int> tab_R_peaks)
{
//getting data
QVector<QVector<double>>tab_RR,tab_RR_new,tab_res;
QVector<QVector<double>>h_amp(2);
QVector<QVector<double>>h_freq(2);
QVector<BeginEndPair> apnea_output;
tab_RR=RR_intervals(tab_R_peaks);
tab_RR_new=averange_filter(tab_RR);
tab_res=resample(tab_RR_new);
HP_LP_filter(tab_res);
hilbert(tab_res,h_amp,h_freq);
freq_amp_filter(h_freq,h_amp);
median_filter(h_freq,h_amp);
apnea_output=apnea_detection(h_amp,h_freq);
//treshold_amp value for amplitude
int i; double a,b,mini_amp,mini_freq,maxi_amp,maxi_freq,mid,treshold_amp,treshold_freq;
mini_amp=99999;mini_freq=99999;
maxi_amp=0;maxi_freq=0;
for(i=0;i<h_amp[0].size();i++)
{
if(h_amp[1][i]>maxi_amp)maxi_amp=h_amp[1][i];
if(h_amp[1][i]<mini_amp)mini_amp=h_amp[1][i];
if(h_freq[1][i]>maxi_freq)maxi_freq=h_freq[1][i];
if(h_freq[1][i]<mini_freq)mini_freq=h_freq[1][i];
}
a=-0.18;b=1;mid=(maxi_amp+mini_amp)*0.5;
treshold_amp=a+b*(mid+1)*0.5;
//treshold_freq value for frequency
treshold_freq=(maxi_freq+mini_freq)*0.4;
//assessment_amp
int n_samples=0,n_apnea=0;double assessment_amp;
n_samples=double(h_amp[0][h_amp[0].size()-1]-h_amp[0][0])/data_freq;
for(int i=0;i<apnea_output.size();i++)
{n_apnea+=apnea_output[i].second-apnea_output[i].first;}
n_apnea=double(n_apnea)/data_freq;
assessment_amp=(double(n_apnea)/n_samples)*100; //*100 -> value in percent
//assessment_freq
double assessment_freq=assessment_amp;
//writing data to output
QVector<double> gui_out;
gui_out.append(treshold_amp);
gui_out.append(treshold_freq);
gui_out.append(assessment_amp);
gui_out.append(assessment_freq);
return gui_out;
}
QVector<BeginEndPair> sleep_apnea::sleep_apnea_output(QVector<unsigned int> tab_R_peaks)
{
//getting data
QVector<QVector<double>>tab_RR,tab_RR_new,tab_res;
QVector<QVector<double>>h_amp(2);
QVector<QVector<double>>h_freq(2);
QVector<BeginEndPair> apnea_output;
tab_RR=RR_intervals(tab_R_peaks);
tab_RR_new=averange_filter(tab_RR);
tab_res=resample(tab_RR_new);
HP_LP_filter(tab_res);
hilbert(tab_res,h_amp,h_freq);
freq_amp_filter(h_freq,h_amp);
median_filter(h_freq,h_amp);
apnea_output=apnea_detection(h_amp,h_freq);
//writing output
return apnea_output;
}
/* simulate processing for one complete frame cycle */
int main(int argc, char* argv[]) {
unsigned char rgb_buf[WIDTH*HEIGHT*3];
float hsv_buf[WIDTH*HEIGHT*3];
unsigned char mask_buf[WIDTH*HEIGHT];
int mass, x, y;
if (argc < 2) {
printf("Usage: exe filename.ppm\n");
return 1;
}
// read frame
read_ppm(rgb_buf, argv[1]);
// blur
gauss_blur(rgb_buf);
// convert to hue, saturation, value
rgb2hsv(rgb_buf, hsv_buf);
// convert to mask with orange threshold
orange_thresh(hsv_buf, mask_buf);
// apply 3x3 median filter
median_filter(mask_buf);
// compute mass and centroid
mass = centroid(mask_buf, &x, &y);
printf("Mass: %d; ", mass);
if (mass) {
printf("Centroid: (%d, %d)\n", x, y);
} else {
printf("No centroid\n");
}
// output mask for verification
write_ppm1(mask_buf, "testdata/full_frame_test.ppm");
printf("Wrote mask to 'testdata/full_frame_test.ppm' for verification.\n");
return 0;
}
int main(int argc, char *argv []) {
if(populate_env_variable(REF_ERROR_CODES_FILE, "L2_ERROR_CODES_FILE")) {
printf("\nUnable to populate [REF_ERROR_CODES_FILE] variable with corresponding environment variable. Routine will proceed without error handling\n");
}
if (argc != 15) {
if(populate_env_variable(SPF_BLURB_FILE, "L2_SPF_BLURB_FILE")) {
RETURN_FLAG = 1;
} else {
print_file(SPF_BLURB_FILE);
}
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -1, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
return 1;
} else {
// ***********************************************************************
// Redefine routine input parameters
char *target_f = strdup(argv[1]);
int bin_size_px = strtol(argv[2], NULL, 0);
double bg_percentile = strtod(argv[3], NULL);
double clip_sigma = strtod(argv[4], NULL);
int median_filter_width_px = strtol(argv[5], NULL, 0);
double min_SNR = strtod(argv[6], NULL);
int min_spatial_width_px = strtol(argv[7], NULL, 0);
int finding_window_lo_px = strtol(argv[8], NULL, 0);
int finding_window_hi_px = strtol(argv[9], NULL, 0);
int max_centering_num_px = strtol(argv[10], NULL, 0);
int centroid_half_window_size_px = strtol(argv[11], NULL, 0);
int min_used_bins = strtol(argv[12], NULL, 0);
int window_x_lo = strtol(argv[13], NULL, 0);
int window_x_hi = strtol(argv[14], NULL, 0);
// ***********************************************************************
// Open target file (ARG 1), get parameters and perform any data format
// checks
fitsfile *target_f_ptr;
int target_f_maxdim = 2, target_f_status = 0, target_f_bitpix, target_f_naxis;
long target_f_naxes [2] = {1,1};
if(!fits_open_file(&target_f_ptr, target_f, IMG_READ_ACCURACY, &target_f_status)) {
if(!populate_img_parameters(target_f, target_f_ptr, target_f_maxdim, &target_f_bitpix, &target_f_naxis, target_f_naxes, &target_f_status, "TARGET FRAME")) {
if (target_f_naxis != 2) { // any data format checks here
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -2, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
} else {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -3, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
fits_report_error(stdout, target_f_status);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
} else {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -4, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
fits_report_error(stdout, target_f_status);
free(target_f);
return 1;
}
// ***********************************************************************
// Set the range limits
int cut_x [2] = {window_x_lo, window_x_hi};
int cut_y [2] = {1, target_f_naxes[1]};
// ***********************************************************************
// Set parameters used when reading data from target file (ARG 1)
long fpixel [2] = {cut_x[0], cut_y[0]};
long nxelements = (cut_x[1] - cut_x[0]) + 1;
long nyelements = (cut_y[1] - cut_y[0]) + 1;
// ***********************************************************************
// Create arrays to store pixel values from target fits file (ARG 1)
double target_f_pixels [nxelements];
// ***********************************************************************
// Get target fits file (ARG 1) values and store in 2D array
int ii;
double target_frame_values [nyelements][nxelements];
memset(target_frame_values, 0, sizeof(double)*nxelements*nyelements);
for (fpixel[1] = cut_y[0]; fpixel[1] <= cut_y[1]; fpixel[1]++) {
memset(target_f_pixels, 0, sizeof(double)*nxelements);
if(!fits_read_pix(target_f_ptr, TDOUBLE, fpixel, nxelements, NULL, target_f_pixels, NULL, &target_f_status)) {
for (ii=0; ii<nxelements; ii++) {
target_frame_values[fpixel[1]-1][ii] = target_f_pixels[ii];
}
} else {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -5, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
fits_report_error(stdout, target_f_status);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
}
// FIND VALUES OF PEAK CENTROID ALONG DISPERSION AXIS
// ***********************************************************************
// 1. Bin array according to bin width given by [bin_size_px]
int disp_nelements = nxelements, spat_nelements = nyelements;
int disp_nelements_binned = (int)floor(disp_nelements/bin_size_px);
double this_frame_values_binned[spat_nelements][disp_nelements_binned];
memset(this_frame_values_binned, 0, sizeof(double)*spat_nelements*disp_nelements_binned);
double this_bin_value;
int bin_number = 0;
int jj;
for (jj=0; jj<spat_nelements; jj++) {
this_bin_value = 0;
bin_number = 0;
for (ii=0; ii<disp_nelements; ii++) {
if (ii % bin_size_px == 0 && ii != 0) {
this_frame_values_binned[jj][bin_number] = this_bin_value;
bin_number++;
this_bin_value = 0;
}
this_bin_value += target_frame_values[jj][ii];
}
}
printf("\nFinding peaks");
printf("\n-------------------------------------\n");
double peaks[disp_nelements_binned];
int num_bins_used = 0;
for (ii=0; ii<disp_nelements_binned; ii++) {
// 1a. Establish if any target flux is in this bin
// First find the mean/sd of the [bg_percentile]th lowest valued pixels as an initial parameters for sigma clip
double this_spat_values[spat_nelements];
double this_spat_values_sorted[spat_nelements];
for (jj=0; jj<spat_nelements; jj++) {
this_spat_values[jj] = this_frame_values_binned[jj][ii];
}
memcpy(this_spat_values_sorted, this_spat_values, sizeof(double)*spat_nelements);
gsl_sort(this_spat_values_sorted, 1, spat_nelements);
int bg_nelements = (int)floor(spat_nelements*bg_percentile);
double bg_values [bg_nelements];
int idx = 0;
for (jj=0; jj<spat_nelements; jj++) {
if (this_spat_values_sorted[jj] != 0) { // avoid 0s set from median filter edges
bg_values[idx] = this_spat_values_sorted[jj];
idx++;
if (idx == bg_nelements)
break;
}
}
if (idx != bg_nelements) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -6, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
double start_mean = gsl_stats_mean(bg_values, 1, bg_nelements);
double start_sd = gsl_stats_sd(bg_values, 1, bg_nelements);
// 1b. Iterative sigma clip around dataset with the initial guess
int retain_indexes[spat_nelements];
double final_mean, final_sd;
int final_num_retained_indexes;
printf("\nBin:\t\t\t\t%d", ii);
printf("\nStart mean:\t\t\t%f", start_mean);
printf("\nStart SD:\t\t\t%f", start_sd);
iterative_sigma_clip(this_spat_values, spat_nelements, clip_sigma, retain_indexes, start_mean, start_sd, &final_mean, &final_sd, &final_num_retained_indexes, FALSE);
printf("\nFinal mean:\t\t\t%f", final_mean);
printf("\nFinal SD:\t\t\t%f", final_sd);
// 2. Smooth array with median filter
double this_spat_values_smoothed[spat_nelements];
memset(this_spat_values_smoothed, 0, sizeof(double)*spat_nelements);
median_filter(this_spat_values, this_spat_values_smoothed, spat_nelements, median_filter_width_px);
// 3. Ascertain if this bin contains target flux
int num_pixels_contain_target_flux = 0;
for (jj=0; jj<spat_nelements-1; jj++) {
if (this_spat_values_smoothed[jj] > final_mean + final_sd*min_SNR) {
num_pixels_contain_target_flux++;
}
}
printf("\nNum pixels (target):\t\t%d", num_pixels_contain_target_flux);
printf("\nDoes bin contain target flux?\t");
if (num_pixels_contain_target_flux >= min_spatial_width_px) {
printf("Yes\n");
} else {
printf("No\n");
peaks[ii] = -1;
continue;
}
// 3. Take derivatives
double this_spat_values_der[spat_nelements-1];
memset(this_spat_values_der, 0, sizeof(double)*spat_nelements-1);
for (jj=1; jj<spat_nelements; jj++) {
this_spat_values_der[jj-1] = this_frame_values_binned[jj][ii] - this_frame_values_binned[jj-1][ii];
}
// 4. Smooth derivatives
double this_spat_values_der_smoothed[spat_nelements-1];
memcpy(this_spat_values_der_smoothed, this_spat_values_der, sizeof(double)*spat_nelements-1);
median_filter(this_spat_values_der, this_spat_values_der_smoothed, spat_nelements-1, median_filter_width_px);
// 5. Pick most positive gradient from window, retaining proper index
double this_spat_values_der_smoothed_windowed[spat_nelements-1];
memcpy(this_spat_values_der_smoothed_windowed, this_spat_values_der_smoothed, sizeof(double)*spat_nelements-1);
for (jj=0; jj<spat_nelements; jj++) {
if (jj >= finding_window_lo_px && jj <= finding_window_hi_px) {
this_spat_values_der_smoothed_windowed[jj] = this_spat_values_der_smoothed_windowed[jj];
} else {
this_spat_values_der_smoothed_windowed[jj] = -1;
}
}
size_t this_pk_idx = gsl_stats_max_index(this_spat_values_der_smoothed_windowed, 1, spat_nelements-1);
printf("Start peak index:\t\t%d\n", this_pk_idx);
// 6. Using this index, walk through centering window [max_centering_num_px] and find derivative turnover point
printf("Found turnover:\t\t\t");
bool found_turnover = FALSE;
for (jj=this_pk_idx; jj<this_pk_idx+max_centering_num_px; jj++) {
if (this_spat_values_der_smoothed[jj] < 0.) {
this_pk_idx = jj;
found_turnover = TRUE;
break;
}
}
if (found_turnover) {
printf("Yes\n");
printf("End peak index:\t\t\t%d\n", this_pk_idx);
} else {
printf("No\n");
peaks[ii] = -1;
continue;
}
// 7. Get parabolic centroid using the full centroid window [centroid_half_window_size_px]
double this_pk_window_idxs[1 + (2*centroid_half_window_size_px)];
double this_pk_window_vals[1 + (2*centroid_half_window_size_px)];
memset(this_pk_window_idxs, 0, sizeof(double)*(1 + (2*centroid_half_window_size_px)));
memset(this_pk_window_vals, 0, sizeof(double)*(1 + (2*centroid_half_window_size_px)));
idx = 0;
for (jj=this_pk_idx-centroid_half_window_size_px; jj<=this_pk_idx+centroid_half_window_size_px; jj++) {
this_pk_window_idxs[idx] = jj;
this_pk_window_vals[idx] = this_frame_values_binned[jj][ii];
idx++;
}
int order = 2;
double coeffs [order+1];
memset(coeffs, 0, sizeof(double)*order+1);
double chi_squared;
if (calc_least_sq_fit(2, 1 + (2*centroid_half_window_size_px), this_pk_window_idxs, this_pk_window_vals, coeffs, &chi_squared)) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -7, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
double fitted_peak_idx = -coeffs[1]/(2*coeffs[2]);
printf("Fitted peak index:\t\t%f\n", fitted_peak_idx);
// 8. Ensure fitted peak location is within finding window
printf("Is fitted peak within window?\t");
if (fitted_peak_idx > finding_window_lo_px && fitted_peak_idx < finding_window_hi_px) {
printf("Yes\n");
num_bins_used++;
} else {
printf("No\n");
peaks[ii] = -1;
continue;
}
peaks[ii] = fitted_peak_idx;
/*for (jj=0; jj<spat_nelements-1; jj++) {
printf("%d\t%f\t%f\n", jj, this_spat_values_der_smoothed[jj], this_spat_values_der[jj]);
}*/ // DEBUG
}
printf("\nNum bins used:\t%d\n", num_bins_used);
if (num_bins_used < min_used_bins) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -8, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
// ***********************************************************************
// Create [FRFIND_OUTPUTF_PEAKS_FILE] output file and print a few
// parameters
FILE *outputfile;
outputfile = fopen(SPFIND_OUTPUTF_PEAKS_FILE, FILE_WRITE_ACCESS);
if (!outputfile) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -9, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
free(target_f);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
char timestr [80];
memset(timestr, '\0', sizeof(char)*80);
find_time(timestr);
fprintf(outputfile, "#### %s ####\n\n", SPFIND_OUTPUTF_PEAKS_FILE);
fprintf(outputfile, "# Lists the coordinates of the peaks found using the spfind routine.\n\n");
fprintf(outputfile, "# Run filename:\t%s\n", target_f);
fprintf(outputfile, "# Run datetime:\t%s\n\n", timestr);
for (ii=0; ii<disp_nelements_binned; ii++) {
if (peaks[ii] == -1)
continue;
fprintf(outputfile, "%f\t%f\n", cut_x[0]+(ii*bin_size_px) + (double)bin_size_px/2., peaks[ii]);
}
fprintf(outputfile, "%d", EOF);
// ***********************************************************************
// Clean up heap memory
free(target_f);
// ***********************************************************************
// Close [FRFIND_OUTPUTF_PEAKS_FILE] output file and target file (ARG 1)
if (fclose(outputfile)) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -10, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
if(fits_close_file(target_f_ptr, &target_f_status)) fits_report_error (stdout, target_f_status);
return 1;
}
if(fits_close_file(target_f_ptr, &target_f_status)) {
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", -11, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
fits_report_error (stdout, target_f_status);
return 1;
}
// ***********************************************************************
// Write success to [ERROR_CODES_FILE]
write_key_to_file(ERROR_CODES_FILE, REF_ERROR_CODES_FILE, "L2STATFI", RETURN_FLAG, "Status flag for L2 spfind routine", ERROR_CODES_FILE_WRITE_ACCESS);
return 0;
}
}
void read_controls(Slider_type* sliders, Calibration_slider_type* cal) {
uint16_t ADC_value;
uint8_t slider_number;
uint16_t ODR_tmp;
uint16_t tmp;
uint16_t adc_res;
uint8_t mux3;
uint8_t i;
switch (controls_read_status) {
case CHECK_VALUE:
mux3=mux_pin*3;
for (i=0; i<3; i++)
ADC_sourse[mux3+i][ADC_channel]= ADC_DMA_buffer[i] & 0x0FFF;
slider_number = mux3 + ADC_channel;
adc_res = median(&ADC_sourse[slider_number][0]);
ADC_value = median_filter(adc_res, &filter_storage[slider_number]); //big window median filter
switch (sliders_state) { // SLIDERS_WORK is for ordinary work, other values are for calibration only
case SLIDERS_WORK:
//Calculate change comparing with old value.
if (check_integral_delta(&ADC_value, slider_number, cal[slider_number].delta)) { //Change a result only if difference exceeds SLIDERS_DELTA.
ADC_old_values[slider_number] = ADC_value;
if (sliders[slider_number].active) //only active sliders work send fifo
slider_FIFO_send(slider_number, ADC_value,
&sliders[slider_number], &cal[slider_number]);
}
break;
case SLIDERS_MENU_SEARCH:
//Calculate change comparing with old value.
if (check_delta(&ADC_value, slider_number, SLIDERS_DELTA_SEARCH)) { //Change a result only if difference exceeds SLIDERS_DELTA.
ADC_old_values[slider_number] = ADC_value;
slider_calibrate_number = slider_number;
send_message(MES_SLIDER_MENU_FOUND);
}
break;
case SLIDERS_CALIBRATE:
if (slider_number == slider_calibrate_number) {
sliders_state = SLIDERS_EDGE;
slider_calibrate_store = ADC_value;
send_message(MES_SLIDER_EDGE);
}
break;
default:
break;
}
ADC_channel++;
if (ADC_channel>=3){
ADC_channel=0;
controls_read_status = NEXT_MUX;
}
break;
case NEXT_MUX:
//Switch multiplexors to next state
mux_pin++;
if (mux_pin > 7) {
mux_pin = 0;
}
ODR_tmp = GPIOB->ODR & 0xFF8F; //PB4, PB5, PB6
tmp = ODR_tmp | (mux_pin << 4); //next value to multiplexors PB4, PB5, PB6
GPIOB->ODR = tmp;
controls_read_status = WAIT_MUX;
break;
case WAIT_MUX:
controls_read_status = CHECK_VALUE;
break;
}
}
int main(int argc, char *argv[]) {
SDL_Surface *screen;
static struct option long_options[] = {
{"no-kinect", no_argument, 0, 'k'},
{"fullscreen", optional_argument, 0, 'f'},
{"help", no_argument, 0, 'h'},
{0, 0, 0, 0}
};
int option_index = 0, opt;
bool init_kinect = true;
bool fullscreen_mode = false;
char *fullscreen_resolution = NULL;
while ((opt = getopt_long(argc, argv, "khf:", long_options, &option_index)) != -1) {
switch (opt) {
case 'k':
init_kinect = false;
printf("Not initializing kinect (-k passed)\n");
break;
case 'f':
printf("Starting in fullscreen mode\n");
fullscreen_mode = true;
if (optarg)
fullscreen_resolution = strdup(optarg);
break;
case 'h':
printf("Syntax: %s [-k] [-h]\n", argv[0]);
printf("\t--no-kinect\tDisables initializing kinect\n");
printf("\t--fullscreen\tEnable fullscreen mode (default is windowed)\n");
printf("\t\t\t(--fullscreen=1024x768 to overwrite the resolution)\n");
exit(0);
break;
}
}
median_filter_init();
glow_filter_init();
if (init_kinect)
kinect_init();
mask_rgb_init();
loadimg_init();
/* Initialize SDL */
SDL_Init(SDL_INIT_VIDEO);
TTF_Init();
SDL_GL_SetAttribute(SDL_GL_RED_SIZE, 8);
SDL_GL_SetAttribute(SDL_GL_GREEN_SIZE, 8);
SDL_GL_SetAttribute(SDL_GL_BLUE_SIZE, 8);
SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 16);
SDL_GL_SetAttribute(SDL_GL_BUFFER_SIZE, 32);
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
SDL_GL_SetAttribute(SDL_GL_SWAP_CONTROL, 1);
/* Initialize the screen / window */
if (fullscreen_mode && fullscreen_resolution != NULL) {
if (sscanf(fullscreen_resolution, "%dx", &SCREEN_WIDTH) != 1) {
fprintf(stderr, "Invalid resolution specified: %s (needs to be WxH, e.g. 1024x768)\n", fullscreen_resolution);
exit(1);
}
printf("Setting width to %d\n", SCREEN_WIDTH);
}
int flags = SDL_OPENGL | SDL_HWSURFACE | SDL_NOFRAME | SDL_DOUBLEBUF;
if (fullscreen_mode)
flags |= SDL_FULLSCREEN;
screen = SDL_SetVideoMode(SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_DEPTH, flags);
if (screen == 0) {
printf("set failed: %s\n", SDL_GetError());
return 1;
}
SDL_WM_SetCaption("kinectboard", "");
glewInit();
/* Setup viewport */
glEnable(GL_TEXTURE_2D);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glViewport(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
glClear(GL_COLOR_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
kb_ui_init();
// Register callbacks
kb_ui_register_void_callback("Exit",exit_callback);
kb_ui_register_void_callback("Calibrate",run_calibration_callback);
kb_ui_register_void_callback("StartCalibration",start_calibration_callback);
kb_ui_register_void_callback("EndCalibration",end_calibration_callback);
kb_ui_register_void_callback("ImageRight",kb_images_scroll_right);
kb_ui_register_void_callback("ImageLeft",kb_images_scroll_left);
kb_ui_register_value_callback("SetDistanceThreshold", set_distance_threshold_callback);
kb_ui_register_value_callback("SetDepthMultiplier", set_depth_multiplier_callback);
kb_ui_register_value_callback("SetDepthDifferenceThreshold", set_depth_difference_threshold_callback);
kb_ui_register_value_callback("SetGlowAreaStart", set_glow_area_start_callback);
kb_ui_register_value_callback("SetGlowAreaEnd", set_glow_area_end_callback);
kb_ui_call_javascript("SetRGB", "142,51,19");
// The CUDA Device Info requires a valid UI since the info is displayed there
print_cuda_device_info();
/* Allocate textures and buffers to draw into (from the GPU) */
allocateGLTexture(&rawDepthBufferID, &rawDepthTextureID);
allocateGLTexture(&medianBufferID, &medianTextureID);
allocateGLTexture(&maskedMedianBufferID, &maskedMedianTextureID);
allocateGLTexture(&glowBufferID, &glowTextureID);
allocateGLTexture(&rawRgbBufferID, &rawRgbTextureID);
allocateGLTexture(&maskRgbBufferID, &maskRgbTextureID);
allocateGLTexture(&contRgbBufferID, &contRgbTextureID);
kb_image_create("Raw depth image", rawDepthBufferID, rawDepthTextureID);
kb_image_create("Median-filtered depth image", medianBufferID, medianTextureID);
kb_image_create("Masked depth image", maskedMedianBufferID, maskedMedianTextureID);
kb_image_create("Glowing depth", glowBufferID, glowTextureID);
kb_image_create("Raw RGB image", rawRgbBufferID, rawRgbTextureID);
kb_image_create("Masked kinect RGB image", maskRgbBufferID, maskRgbTextureID);
kb_image_create("Cont RGB image", contRgbBufferID, contRgbTextureID);
// Load a Texture
//loadTextureFromFile("../data/calibration.bmp", &calibrationBufferID, &calibrationTextureID);
//kb_image_create("Calibration", calibrationBufferID, calibrationTextureID);
SDL_Surface* surface = SDL_LoadBMP("../data/calibration.bmp");
cudaMalloc((void**)&(backgrounds[1]), 640 * 480 * 3 * sizeof(uint8_t));
loadimg_convert((uint8_t*)surface->pixels, backgrounds[1]);
surface = SDL_LoadBMP("../data/malen_haus.bmp");
cudaMalloc((void**)&(backgrounds[2]), 640 * 480 * 3 * sizeof(uint8_t));
loadimg_convert((uint8_t*)surface->pixels, backgrounds[2]);
surface = SDL_LoadBMP("../data/malen_stern.bmp");
cudaMalloc((void**)&(backgrounds[3]), 640 * 480 * 3 * sizeof(uint8_t));
loadimg_convert((uint8_t*)surface->pixels, backgrounds[3]);
surface = SDL_LoadBMP("../data/empty.bmp");
cudaMalloc((void**)&(backgrounds[4]), 640 * 480 * 3 * sizeof(uint8_t));
loadimg_convert((uint8_t*)surface->pixels, backgrounds[4]);
printf("gl set up.\n");
uchar4 *gpu_median_output,
*gpu_masked_median_output,
*gpu_glow_output,
*gpu_mask_rgb_output,
*gpu_raw_depth_output,
*gpu_raw_rgb_output,
*gpu_cont_rgb_output;
int fps = 0;
int last_time = 0;
int current_time;
while (1) {
/* FPS counter */
current_time = SDL_GetTicks();
if ((current_time - last_time) >= 1000) {
static char buffer[20] = {0};
sprintf(buffer, "%d FPS", fps);
SDL_WM_SetCaption(buffer, 0);
kb_ui_call_javascript("SetFPS",buffer);
fps = 0;
last_time = current_time;
}
//kb_poll_events(list);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
/* Reset viewport for rendering our images, it was modified by
* kb_ui_render(). */
glViewport(0, 0, SCREEN_WIDTH, SCREEN_HEIGHT);
glClear(GL_COLOR_BUFFER_BIT);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
kb_poll_events();
gpu_median_output = NULL;
gpu_masked_median_output = NULL;
gpu_glow_output = NULL;
gpu_mask_rgb_output = NULL;
gpu_raw_depth_output = NULL;
gpu_raw_rgb_output = NULL;
gpu_cont_rgb_output = NULL;
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_raw_depth_output, rawDepthBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_median_output, medianBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_masked_median_output, maskedMedianBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_glow_output, glowBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_mask_rgb_output, maskRgbBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_raw_rgb_output, rawRgbBufferID));
cutilSafeCall(cudaGLMapBufferObject((void**)&gpu_cont_rgb_output, contRgbBufferID));
// XXX: Potential for optimization: We currently call functions like
// median_filter(), median_mask() and mask_rgb() which are all
// blocking. However, we could launch the kernel and perform more work
// on the CPU while waiting for the kernel to complete (or maybe even
// launch some in parallel and/or use async events).
median_filter(take_depth_image(), gpu_median_output, gpu_raw_depth_output);
done_depth_image();
median_mask(calibration, gpu_median_output, gpu_masked_median_output);
glow_filter(gpu_masked_median_output, gpu_glow_output, glow_start, glow_end);
mask_rgb(gpu_glow_output, take_rgb_image(), gpu_mask_rgb_output, gpu_raw_rgb_output, gpu_cont_rgb_output, reference_color, FILTER_DISTANCE, backgrounds[current_background], calibrated_offset);
done_rgb_image();
cutilSafeCall(cudaGLUnmapBufferObject(maskedMedianBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(medianBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(glowBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(maskRgbBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(rawDepthBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(rawRgbBufferID));
cutilSafeCall(cudaGLUnmapBufferObject(contRgbBufferID));
if(fullscreen_canvas) {
kb_images_render_canvas_only();
} else {
kb_images_render();
kb_ui_update();
kb_ui_render();
}
SDL_GL_SwapBuffers();
fps++;
}
}
CCM_FUNC static THD_FUNCTION(ThreadADC, arg)
{
(void)arg;
chRegSetThreadName("Sensors");
adcsample_t * sensorsDataPtr;
size_t n;
uint16_t i, pos;
uint32_t an[3] = {0, 0, 0};
median_t an1, an2, an3;
uint8_t row, col;
chThdSleepMilliseconds(250);
timcapEnable(&TIMCAPD3);
chVTSet(&vt_freqin, FREQIN_INTERVAL, freqinVTHandler, NULL);
adcStartConversion(&ADCD1, &adcgrpcfg_sensors, samples_sensors, ADC_GRP1_BUF_DEPTH);
median_init(&an1, 0 , an1_buffer, ADC_GRP1_BUF_DEPTH/2);
median_init(&an2, 0 , an2_buffer, ADC_GRP1_BUF_DEPTH/2);
median_init(&an3, 0 , an3_buffer, ADC_GRP1_BUF_DEPTH/2);
while (TRUE)
{
while (!recvFreeSamples(&sensorsMb, (void*)&sensorsDataPtr, &n))
chThdSleepMilliseconds(5);
an[0]= 0;
an[1]= 0;
an[2]= 0;
/* Filtering and adding */
for (i = 0; i < (n/ADC_GRP1_NUM_CHANNELS); i++)
{
pos = i * ADC_GRP1_NUM_CHANNELS;
an[0] += median_filter(&an1, sensorsDataPtr[pos]);
an[1] += median_filter(&an2, sensorsDataPtr[pos+1]);
an[2] += median_filter(&an3, sensorsDataPtr[pos+2]);
}
/* Averaging */
an[0] /= (n/ADC_GRP1_NUM_CHANNELS);
an[1] /= (n/ADC_GRP1_NUM_CHANNELS);
an[2] /= (n/ADC_GRP1_NUM_CHANNELS);
/* Convert to milliVolts */
an[0] *= VBAT_RATIO;
an[1] *= AN_RATIO;
an[2] *= AN_RATIO;
sensors_data.an1 = an[0];
sensors_data.an2 = an[1];
sensors_data.an3 = an[2];
/* Analog/Digital Sensors */
if (settings.sensorsInput == SENSORS_INPUT_DIRECT) {
sensors_data.tps = calculateTpFromMillivolt(settings.tpsMinV, settings.tpsMaxV, sensors_data.an2);
sensors_data.rpm = calculateFreqWithRatio(sensors_data.freq1, settings.rpmMult);
sensors_data.spd = calculateFreqWithRatio(sensors_data.freq2, settings.spdMult);
}
else if (settings.sensorsInput == SENSORS_INPUT_TEST) {
sensors_data.tps = rand16(0, 200);
sensors_data.rpm = rand16(10, 18000);
sensors_data.spd = rand16(5, 10000);
}
/* AFR */
if (settings.afrInput == AFR_INPUT_AN) {
sensors_data.afr = calculateAFRFromMillivolt(settings.AfrMinVal, settings.AfrMaxVal, sensors_data.an3);
}
else if (settings.afrInput == AFR_INPUT_TEST) {
sensors_data.afr = rand16(11000, 16000) / 100;
}
if (dbg_sensors) {
chprintf(DBG_STREAM,"->[SENSORS] TPS mV/PCT: %06u/%04u\r\n", sensors_data.an2, sensors_data.tps);
chprintf(DBG_STREAM,"->[SENSORS] RMP Hz/Mult/Val: %06u/%.4f/%04u\r\n", sensors_data.freq1, settings.rpmMult, sensors_data.rpm);
chprintf(DBG_STREAM,"->[SENSORS] SPD Hz/Mult/Val: %06u/%.4f/%04u\r\n", sensors_data.freq2, settings.spdMult, sensors_data.spd);
}
if (findCell(sensors_data.tps/2, sensors_data.rpm, &row, &col))
{
sensors_data.cell.row = row;
sensors_data.cell.col = col;
if (dbg_sensors) {
chprintf(DBG_STREAM,"->[SENSORS] Row:Value/Col:Value: %02u:%05u/%02u:%05u\r\n", row, tableRows[row], col, tableColumns[col]*100);
}
if ((settings.functions & FUNC_RECORD) && sensors_data.rpm != 0)
{
/* Average */
tableAFR[row][col] = tableAFR[row][col] == 0 ? sensors_data.afr : ((uint16_t)sensors_data.afr+(uint16_t)tableAFR[row][col])/2;
/* Peaks */
tableKnock[row][col] = sensors_data.knock_value > tableKnock[row][col] ? sensors_data.knock_value : tableKnock[row][col];
}
}
}
return;
}
