00001 #include <stdio.h> 00002 #include <stdlib.h> 00003 #include <math.h> 00004 #include <vector> 00005 #include <fftw3.h> 00006 #include <cstring> 00007 00008 #include "sonar_utils.h" 00009 #include "sonar_fft.h" 00010 #include "cheetah.h" 00011 00012 // Globals 00013 const double SPEED_SOUND_WATER = 1482; // [m/s] 00014 const double SENSOR_SPACING_2_1 = 0.024; // [m] 00015 const double SENSOR_SPACING_3_2 = 0.024; // [m] 00016 const double SENSOR_SPACING_1_3 = 0.024; // [m] 00017 const int DATA_BLOCK_SIZE = 512; 00018 const int DATA_RETENTION_LENGTH = 3 * DATA_BLOCK_SIZE; 00019 00020 // Declare Cheetah Variables 00021 Cheetah handle; 00022 int port, bitrate; 00023 uint8_t mode = 0; 00024 int ret, input, ready_bit, valid_data_point; 00025 00026 // FFT & Bin Prediction 00027 const int FP_FFT_LENGTH = 32; // First-pass FFT length 00028 double BIN_PREDICTION_M = 10.59; 00029 double BIN_PREDICTION_B = -1193.82; 00030 00031 int target_frequency1, target_frequency2; 00032 double target_wavelength1, target_wavelength2; 00033 double angle_estimate1, angle_estimate2; 00034 double bin_current_mag_sq_1, bin_current_mag_sq_2, bin_current_mag_sq_3; 00035 double adc1_bin_mean, adc2_bin_mean, adc3_bin_mean; 00036 std::vector<bool> FP_bin_indicator; 00037 00038 // Raw Data 00039 const int TX_LENGTH = DATA_BLOCK_SIZE * 3 * 2; 00040 uint8_t data_in[TX_LENGTH]; 00041 uint8_t data_out[2]; 00042 00043 // Hysterisis Vars 00044 const int FP_BIN_HISTORY_LENGTH = 10; 00045 const double MEAN_SCALE_FACTOR = 10.0; 00046 const int FP_MIN_SAMPLE_LENGTH = 128; 00047 int FP_bin_mean_idx = 1; 00048 double FP_adc1_bin_history[FP_BIN_HISTORY_LENGTH]; 00049 double FP_adc2_bin_history[FP_BIN_HISTORY_LENGTH]; 00050 double FP_adc3_bin_history[FP_BIN_HISTORY_LENGTH]; 00051 int FP_bin_history_fill = 0; 00052 std::vector<double> *adc1_data_history, *adc2_data_history, *adc3_data_history, *tmp_data_buffer; 00053 00054 00055 int main (int argc, char const *argv[]) { 00056 //fprintf(stderr, "Starting up...\n"); 00057 port = 0; 00058 bitrate = 10000; 00059 00060 double sampling_frequency = SeaBee3_Sonar::getSamplingFrequency(bitrate, BIN_PREDICTION_M, BIN_PREDICTION_B); 00061 00062 // Assign Target Frequencies 00063 target_frequency1 = 25000; //atoi(argv[2]); 00064 target_frequency2 = 23000; //atoi(argv[3]); 00065 target_wavelength1 = (double)SPEED_SOUND_WATER / (double)target_frequency1; 00066 target_wavelength2 = (double)SPEED_SOUND_WATER / (double)target_frequency2; 00067 00068 // Declare Data Vectors 00069 double FP_adc1_samples[FP_FFT_LENGTH], FP_adc2_samples[FP_FFT_LENGTH], FP_adc3_samples[FP_FFT_LENGTH]; 00070 00071 adc1_data_history = new std::vector<double>; 00072 adc2_data_history = new std::vector<double>; 00073 adc3_data_history = new std::vector<double>; 00074 00075 // Define and allocate the Data Vectors 00076 double *data1, *data2, *data3; 00077 data1 = (double *) fftw_malloc(sizeof(double) * DATA_BLOCK_SIZE); 00078 data2 = (double *) fftw_malloc(sizeof(double) * DATA_BLOCK_SIZE); 00079 data3 = (double *) fftw_malloc(sizeof(double) * DATA_BLOCK_SIZE); 00080 memset(data1, 0, sizeof(double) * DATA_BLOCK_SIZE); 00081 memset(data2, 0, sizeof(double) * DATA_BLOCK_SIZE); 00082 memset(data3, 0, sizeof(double) * DATA_BLOCK_SIZE); 00083 00084 // Open the device 00085 handle = ch_open(port); 00086 00087 if (handle <= 0) { 00088 fprintf(stderr, "Unable to open Cheetah device on port %d\n", port); 00089 fprintf(stderr, "Error code = %d (%s)\n", handle, ch_status_string(handle)); 00090 exit(1); 00091 } 00092 fprintf(stderr, "Opened Cheetah device on port %d\n", port); 00093 00094 fprintf(stderr, "Host interface is %s\n", 00095 (ch_host_ifce_speed(handle)) ? "high speed" : "full speed"); 00096 00097 // Ensure that the SPI subsystem is configured. 00098 ch_spi_configure(handle, CheetahSpiPolarity(mode >> 1), CheetahSpiPhase(mode & 1), CH_SPI_BITORDER_MSB, 0x0); 00099 fprintf(stderr, "SPI configuration set to mode %d, %s shift, SS[2:0] active low\n", mode, "MSB"); 00100 00101 // Power the target using the Cheetah adapter's power supply. 00102 ch_target_power(handle, CH_TARGET_POWER_ON); 00103 ch_sleep_ms(100); 00104 00105 // Set the bitrate. 00106 bitrate = ch_spi_bitrate(handle, bitrate); 00107 fprintf(stderr, "Bitrate set to %d kHz\n", bitrate); 00108 00109 // Make a simple queue to just assert OE. 00110 ch_spi_queue_clear(handle); 00111 ch_spi_queue_oe(handle, 1); 00112 ch_spi_batch_shift(handle, 0, 0); 00113 00114 // Queue the batch, which is a sequence of SPI packets (back-to-back) each of length 2. 00115 fprintf(stderr, "Beginning to queue SPI packets..."); 00116 data_out[0] = 0xff; 00117 data_out[1] = 0xff; 00118 ch_spi_queue_clear(handle); 00119 00120 for (int i = 0; i < DATA_BLOCK_SIZE; ++i) { 00121 // Convert Slave 1 00122 ch_spi_queue_ss(handle, 0xF); 00123 ch_spi_queue_array(handle, 2, data_out); 00124 ch_spi_queue_ss(handle, 0xE); 00125 00126 // Convert Slave 2 00127 ch_spi_queue_ss(handle, 0xF); 00128 ch_spi_queue_array(handle, 2, data_out); 00129 ch_spi_queue_ss(handle, 0xD); 00130 00131 // Convert Slave 3 00132 ch_spi_queue_ss(handle, 0xF); 00133 ch_spi_queue_array(handle, 2, data_out); 00134 ch_spi_queue_ss(handle, 0xB); 00135 } 00136 fprintf(stderr, " Done\n"); 00137 00138 // Submit the first batch 00139 ch_spi_async_submit(handle); 00140 00141 int z = 0; 00142 while (true) { 00143 //fprintf(stderr, "Starting Loop!\n"); 00144 // Submit another batch, while the previous one is in 00145 // progress. The application may even clear the current 00146 // batch queue and queue a different set of SPI 00147 // transactions before submitting this batch 00148 // asynchronously. 00149 ch_spi_async_submit(handle); 00150 00151 // The application can now perform some other functions 00152 // while the Cheetah is both finishing the previous batch 00153 // and shifting the current batch as well. In order to 00154 // keep the Cheetah's pipe full, this entire loop must 00155 // complete AND another batch must be submitted 00156 // before the current batch completes. 00157 //ch_sleep_ms(1); 00158 00159 // Collect the previous batch 00160 // The length of the batch, FP_FFT_LENGTH * 6, come from the fact that 3 ADCs 00161 // are batched and the return data requires 2 bytes. (2 * 3 = 6) 00162 ret = ch_spi_async_collect(handle, TX_LENGTH, data_in); 00163 00164 int data_idx = 0; 00165 for (int j = 0; j < TX_LENGTH; j += 6) { 00166 // SS3 Data 00167 input = (data_in[j] << 8) + data_in[j+1]; 00168 ready_bit = input & 0x4000; 00169 valid_data_point = (input & 0x3ffc) >> 2; 00170 data3[data_idx] = valid_data_point; 00171 00172 // SS1 Data 00173 input = (data_in[j+2] << 8) + data_in[j+3]; 00174 ready_bit = input & 0x4000; 00175 valid_data_point = (input & 0x3ffc) >> 2; 00176 data1[data_idx] = valid_data_point; 00177 00178 // SS2 Data 00179 input = (data_in[j+4] << 8) + data_in[j+5]; 00180 ready_bit = input & 0x4000; 00181 valid_data_point = (input & 0x3ffc) >> 2; 00182 data2[data_idx] = valid_data_point; 00183 ++data_idx; 00184 } 00185 00186 // Append current data to history vectors 00187 adc1_data_history->insert(adc1_data_history->end(), data1, data1 + DATA_BLOCK_SIZE); 00188 adc2_data_history->insert(adc2_data_history->end(), data2, data2 + DATA_BLOCK_SIZE); 00189 adc3_data_history->insert(adc3_data_history->end(), data3, data3 + DATA_BLOCK_SIZE); 00190 00191 00192 //printf("Vector Length: %d\n", adc1_data_history->size()); 00193 for (int vector_idx = 0; vector_idx <= adc1_data_history->size() - FP_FFT_LENGTH; vector_idx += FP_FFT_LENGTH) { 00194 //fprintf(stderr, "Copying vector memory\n"); 00195 std::copy(adc1_data_history->begin() + vector_idx, adc1_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc1_samples); 00196 std::copy(adc2_data_history->begin() + vector_idx, adc2_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc2_samples); 00197 std::copy(adc3_data_history->begin() + vector_idx, adc3_data_history->begin() + vector_idx + FP_FFT_LENGTH, FP_adc3_samples); 00198 //fprintf(stderr, "Done copying vector memory\n"); 00199 00200 /* for (int i = 0; i < FP_FFT_LENGTH; ++i) 00201 fprintf(stderr, "%5.0f", FP_adc1_samples[i]); 00202 fprintf(stderr, "\n");*/ 00203 00204 //fprintf(stderr, "Prepping DataToFFT\n"); 00205 SeaBee3_Sonar::DataToFFT fp_fft_f1 = SeaBee3_Sonar::DataToFFT(FP_adc1_samples, FP_adc2_samples, FP_adc3_samples, 00206 FP_FFT_LENGTH, sampling_frequency, target_frequency1); 00207 SeaBee3_Sonar::DataToFFT fp_fft_f2 = SeaBee3_Sonar::DataToFFT(FP_adc1_samples, FP_adc2_samples, FP_adc3_samples, 00208 FP_FFT_LENGTH, sampling_frequency, target_frequency2); 00209 //fprintf(stderr, "Done prepping DataToFFT\n"); 00210 00211 bin_current_mag_sq_1 = fp_fft_f1.getMagnitude(1); 00212 bin_current_mag_sq_2 = fp_fft_f1.getMagnitude(2); 00213 bin_current_mag_sq_3 = fp_fft_f1.getMagnitude(3); 00214 00215 // Index Check 00216 if (FP_bin_mean_idx >= FP_BIN_HISTORY_LENGTH) 00217 FP_bin_mean_idx = 0; 00218 00219 // Initilize Mean Array 00220 if (FP_bin_history_fill < FP_BIN_HISTORY_LENGTH) { 00221 FP_adc1_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_1; 00222 FP_adc2_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_2; 00223 FP_adc3_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_3; 00224 ++FP_bin_mean_idx; 00225 ++FP_bin_history_fill; 00226 continue; 00227 } 00228 00229 adc1_bin_mean = 0; 00230 adc2_bin_mean = 0; 00231 adc3_bin_mean = 0; 00232 00233 for (int i = 0; i < FP_BIN_HISTORY_LENGTH; ++i) { 00234 adc1_bin_mean += FP_adc1_bin_history[i]; 00235 adc2_bin_mean += FP_adc2_bin_history[i]; 00236 adc3_bin_mean += FP_adc3_bin_history[i]; 00237 } 00238 adc1_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; 00239 adc2_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; 00240 adc3_bin_mean /= (double)FP_BIN_HISTORY_LENGTH; 00241 00242 if (bin_current_mag_sq_1 > MEAN_SCALE_FACTOR * adc1_bin_mean && 00243 bin_current_mag_sq_2 > MEAN_SCALE_FACTOR * adc2_bin_mean && 00244 bin_current_mag_sq_3 > MEAN_SCALE_FACTOR * adc3_bin_mean) { 00245 FP_bin_indicator.push_back(true); 00246 printf("1 "); 00247 printf("%15.0f\n", bin_current_mag_sq_3); 00248 } else { 00249 FP_bin_indicator.push_back(false); 00250 //printf("0 "); 00251 //printf("%15.0f\n", bin_current_mag_sq_3); 00252 FP_adc1_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_1; 00253 FP_adc2_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_2; 00254 FP_adc3_bin_history[FP_bin_mean_idx] = bin_current_mag_sq_3; 00255 //printf("%10.0f, %10.0f, %10.0f\n", adc1_bin_mean, adc2_bin_mean, adc3_bin_mean); 00256 ++FP_bin_mean_idx; 00257 } 00258 00259 if (FP_bin_indicator[0] == 0 && FP_bin_indicator[1] == 1) { 00260 printf("Just had a pulse!\n"); 00261 00262 int SP_bin_count = 0; 00263 while (FP_bin_indicator[SP_bin_count + 1] != 0) 00264 ++SP_bin_count; 00265 printf("Signal Bin Count of %d\n", SP_bin_count); 00266 00267 if (SP_bin_count * FP_FFT_LENGTH >= FP_MIN_SAMPLE_LENGTH) { 00268 int SP_fft_length = SP_bin_count * FP_FFT_LENGTH; 00269 00270 // Copy the sample data into new double array 00271 double *SP_adc1_samples = new double[SP_fft_length]; 00272 double *SP_adc2_samples = new double[SP_fft_length]; 00273 double *SP_adc3_samples = new double[SP_fft_length]; 00274 00275 std::copy(adc1_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc1_data_history->begin() + vector_idx, SP_adc1_samples); 00276 std::copy(adc2_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc2_data_history->begin() + vector_idx, SP_adc2_samples); 00277 std::copy(adc3_data_history->begin() + vector_idx - (SP_bin_count) * FP_FFT_LENGTH, adc3_data_history->begin() + vector_idx, SP_adc3_samples); 00278 00279 // Delete all data up to and including the detected ping 00280 adc1_data_history->erase(adc1_data_history->begin(), adc1_data_history->begin() + vector_idx); 00281 adc2_data_history->erase(adc2_data_history->begin(), adc2_data_history->begin() + vector_idx); 00282 adc3_data_history->erase(adc3_data_history->begin(), adc3_data_history->begin() + vector_idx); 00283 /* // DEBUG 00284 for (int blah = 0; blah < FP_FFT_LENGTH; ++blah) 00285 printf("%5.0f", FP_adc1_samples[blah]); 00286 printf("\n\n\n\n");*/ 00287 00288 SeaBee3_Sonar::DataToFFT sp_fft_f1 = SeaBee3_Sonar::DataToFFT(FP_adc1_samples, FP_adc2_samples, FP_adc3_samples, 00289 FP_FFT_LENGTH, sampling_frequency, target_frequency1); 00290 SeaBee3_Sonar::DataToFFT sp_fft_f2 = SeaBee3_Sonar::DataToFFT(FP_adc1_samples, FP_adc2_samples, FP_adc3_samples, 00291 FP_FFT_LENGTH, sampling_frequency, target_frequency2); 00292 00293 fprintf(stderr, "SP Target Bin: %d\n", sp_fft_f1.getTargetBin()); 00294 00295 double phase1 = sp_fft_f1.getPhase(1); 00296 double phase2 = sp_fft_f1.getPhase(2); 00297 double phase3 = sp_fft_f1.getPhase(3); 00298 00299 // Calculate usable phase differences 00300 double delta1 = phase2 - phase1; 00301 double delta2 = phase3 - phase2; 00302 double delta3 = phase1 - phase3; 00303 00304 fprintf(stderr, "Phase: %5.5f %5.5f %5.5f\n", phase1, phase2, phase3); 00305 fprintf(stderr, "Deltas: %5.5f %5.5f %5.5f\n", delta1, delta2, delta3); 00306 00307 // Determine minimum phase difference for pair selection 00308 int min_index = 3; 00309 if (fabs(delta2) < fabs(delta1) && fabs(delta2) < fabs(delta3)) 00310 min_index = 2; 00311 if (fabs(delta1) < fabs(delta2) && fabs(delta1) < fabs(delta3)) 00312 min_index = 1; 00313 00314 double delta_tmp; 00315 switch (min_index) { 00316 case 1: 00317 delta_tmp = delta1; 00318 if (delta3 > delta2) 00319 delta_tmp = -1.0 * delta1 + SeaBee3_Sonar::sign(delta_tmp) * 2.0 * M_PI; 00320 angle_estimate1 = delta_tmp * (double)(((double)target_wavelength1 / 2.0) / SENSOR_SPACING_2_1) * 180.0 / M_PI / 2.0; 00321 break; 00322 case 2: 00323 delta_tmp = delta2; 00324 if (delta1 > delta3) 00325 delta_tmp = -1.0 * delta2 + 2.0 * M_PI; 00326 angle_estimate1 = (delta_tmp - 4.0 / 3.0 * M_PI) * ((target_wavelength1 / 2.0) / SENSOR_SPACING_3_2) * 180.0 / M_PI / 2.0; 00327 break; 00328 case 3: 00329 delta_tmp = delta3; 00330 if (delta2 > delta1) 00331 delta_tmp = -1.0 * delta3 - 2.0 * M_PI; 00332 angle_estimate1 = (delta_tmp + 4.0 / 3.0 * M_PI ) * (((double)target_wavelength1 / 2.0) / SENSOR_SPACING_1_3) * 180.0 / M_PI / 2.0; 00333 break; 00334 default: 00335 fprintf(stderr, "Sonar: Invalid min_index for phase difference."); 00336 } 00337 fprintf(stderr, "Min Index: %d\n", min_index); 00338 fprintf(stderr, "Angle Estimate (1): %3.2f\n", angle_estimate1); 00339 00340 /* // DEBUG 00341 for (int blah = 0; blah < SP_bin_count * FP_FFT_LENGTH; ++blah) 00342 printf("%5.0f", SP_adc1_samples[blah]); 00343 printf("\n");*/ 00344 00345 } 00346 } 00347 00348 // Check data history vector length constraints 00349 if (adc1_data_history->size() > DATA_RETENTION_LENGTH) { 00350 //printf("Data history is too long, reducing vector sizes\n"); 00351 tmp_data_buffer = new std::vector<double> (adc1_data_history->begin() + adc1_data_history->size() - DATA_RETENTION_LENGTH, adc1_data_history->end()); 00352 delete adc1_data_history; 00353 adc1_data_history = tmp_data_buffer; 00354 tmp_data_buffer = NULL; 00355 } 00356 if (adc2_data_history->size() > DATA_RETENTION_LENGTH) { 00357 tmp_data_buffer = new std::vector<double> (adc2_data_history->begin() + adc2_data_history->size() - DATA_RETENTION_LENGTH, adc2_data_history->end()); 00358 delete adc2_data_history; 00359 adc2_data_history = tmp_data_buffer; 00360 tmp_data_buffer = NULL; 00361 } 00362 if (adc3_data_history->size() > DATA_RETENTION_LENGTH) { 00363 tmp_data_buffer = new std::vector<double> (adc3_data_history->begin() + adc3_data_history->size() - DATA_RETENTION_LENGTH, adc3_data_history->end()); 00364 delete adc3_data_history; 00365 adc3_data_history = tmp_data_buffer; 00366 tmp_data_buffer = NULL; 00367 } 00368 00369 if (FP_bin_indicator.size() > FP_BIN_HISTORY_LENGTH) 00370 FP_bin_indicator.erase(FP_bin_indicator.begin()); 00371 } 00372 } 00373 00374 return 0; 00375 } 00376