int main( int argc, char ** argv )
{
ros::init( argc, argv, "sonar_driver" );
if (argc < 3) {
ROS_ERROR("sonar_driver usage: sudo rosrun sonar_driver sonar_driver PORT BITRATE\n");
//port 0, bitrate 40000kHz
return 1;
}
// Initialize Cheetah parameters
port = atoi(argv[1]);
bitrate = atoi(argv[2]); //kHz
// Open the device
handle = ch_open(port);
if (handle <= 0) {
ROS_ERROR("Unable to open Cheetah device on port %d (Error code = %d: %s)", port, handle, ch_status_string(handle));
exit(1);
}
ROS_INFO("Opened Cheetah device on port %d", port);
ROS_INFO("Host interface is %s", (ch_host_ifce_speed(handle)) ? "high speed" : "full speed");
// Configure SPI subsystem
ch_spi_configure(handle, CH_SPI_POL_RISING_FALLING, CH_SPI_PHASE_SETUP_SAMPLE, CH_SPI_BITORDER_MSB, 0x0);
ROS_INFO("SPI configuration set to mode %d, %s shift, SS[2:0] active low", mode, "MSB");
// Power the target using the Cheetah's 5V power supply
ch_target_power(handle, CH_TARGET_POWER_ON);
ch_sleep_ms(100);
// Set the bitrate
bitrate = ch_spi_bitrate(handle, bitrate);
ROS_INFO("Bitrate set to %d kHz", bitrate);
// Make a simple queue to assert OE
ch_spi_queue_clear(handle);
ch_spi_queue_oe(handle, 1);
ch_spi_batch_shift(handle, 0, 0);
// Queue the batch
ROS_INFO("Beginning to queue SPI packets...");
ch_spi_queue_clear(handle);
for (int i = 0; i < DATA_BLOCK_SIZE; ++i) {
// Convert Slave 1
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xE);
// Convert Slave 2
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xD);
// Convert Slave 3
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xB);
}
ROS_INFO("Finished queueing packets\n");
// Open output filestreams
std::ofstream file1 ("1_adc_samples.txt");
std::ofstream file2 ("2_adc_samples.txt");
std::ofstream file3 ("3_adc_samples.txt");
int batch_cnt = 1; //count number of sample batches
double elapsed;
s64 start;
while( ros::ok() ) {
// Submit the batch and collect data
ROS_INFO("Collecting data from batch %d...", batch_cnt);
start = _timeMillis();
ch_spi_async_submit(handle);
ret = ch_spi_async_collect(handle, TX_LENGTH, data_in);
elapsed = ((double)(_timeMillis() - start)) / 1000;
ROS_INFO("collected %d bytes from batch #%04d in %.4lf seconds\n", ret, batch_cnt, elapsed);
if (ret < 0) ROS_INFO("status error: %s\n", ch_status_string(ret));
batch_cnt++;
// Process raw data for the 12-bit ADC's, and write data to text files
int data_idx = 0;
if ( file1.is_open() && file2.is_open() && file3.is_open() ) {
for (int j = 0; j < TX_LENGTH; j += 6) {
// SS3 Data
input = (data_in[j] << 8) + data_in[j+1];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000; //convert 2's comp to signed
data3[data_idx] = valid_data_point;
file3 << data3[data_idx] << ",";
// SS1 Data
input = (data_in[j+2] << 8) + data_in[j+3];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data1[data_idx] = valid_data_point;
file1 << data1[data_idx] << ",";
// SS2 Data
input = (data_in[j+4] << 8) + data_in[j+5];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data2[data_idx] = valid_data_point;
file2 << data2[data_idx] << ",";
++data_idx;
}
}
else std::cout << "Error opening output filestream!" << std::endl;
}
//=========================================================================
// FUNCTIONS
//=========================================================================
static void _blast (Cheetah handle, u32 length) {
double elapsed;
int delay = 0;
u32 i;
int batch;
int count;
u08 *data_in;
s64 start = _timeMillis();
// Queue the read sequence
ch_spi_queue_clear(handle);
ch_spi_queue_oe(handle, 1);
ch_spi_queue_ss(handle, 0);
ch_spi_queue_ss(handle, 0x1);
for (i = 0; i < length; ++i) {
ch_spi_queue_byte(handle, 1, (u08)(i & 0xff));
delay = ch_spi_queue_delay_ns(handle, BYTE_DELAY);
}
printf("Queued delay of %d ns between bytes.\n", delay);
fflush(stdout);
ch_spi_queue_ss(handle, 0);
ch_spi_queue_oe(handle, 0);
elapsed = ((double)(_timeMillis() - start)) / 1000;
printf("Took %.2lf seconds to queue the batch.\n", elapsed);
fflush(stdout);
// Perform the shift
batch = ch_spi_batch_length(handle);
data_in = (u08 *)malloc(batch);
start = _timeMillis();
count = ch_spi_batch_shift(handle, batch, data_in);
elapsed = ((double)(_timeMillis() - start)) / 1000;
printf("Took %.2lf seconds to shift the batch.\n", elapsed);
fflush(stdout);
if (count != batch) {
printf("Expected %d bytes but only received %d bytes\n",
batch, count);
goto cleanup;
}
#ifdef SHOW_DATA
// Output the data to the screen
printf("\nData:");
for (i = 0; i < length; ++i) {
if ((i&0x07) == 0) printf("\n%04x: ", i);
printf("%02x/%02x ", (i & 0xff), data_in[i]);
}
printf("\n");
fflush(stdout);
#endif
cleanup:
// Cleanup and exit
free(data_in);
}
//=========================================================================
// FUNCTIONS
//=========================================================================
static void _blast_async (Cheetah handle, u32 txnlen, u32 iter) {
double elapsed;
u32 i;
int count = 0;
u08 data_out[4];
s64 start;
int ret;
// Make a simple queue to just assert OE.
ch_spi_queue_clear(handle);
ch_spi_queue_oe(handle, 1);
ch_spi_batch_shift(handle, 0, 0);
// Queue the batch which is a sequence of SPI packets
// (back-to-back) each of length 4.
ch_spi_queue_clear(handle);
for (i = 0; i < txnlen; ++i) {
// Convert Slave 1
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xE);
// Convert Slave 2
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xD);
// Convert Slave 3
ch_spi_queue_ss(handle, 0xF);
ch_spi_queue_array(handle, 2, data_out);
ch_spi_queue_ss(handle, 0xB);
}
start = _timeMillis();
// First, submit first batch
ch_spi_async_submit(handle);
for (i = 0; i < iter-1; ++i) {
// Submit another batch, while the previous one is in
// progress. The application may even clear the current
// batch queue and queue a different set of SPI
// transactions before submitting this batch
// asynchronously.
ch_spi_async_submit(handle);
// The application can now perform some other functions
// while the Cheetah is both finishing the previous batch
// and shifting the current batch as well. In order to
// keep the Cheetah's pipe full, this entire loop must
// complete AND another batch must be submitted
// before the current batch completes.
ch_sleep_ms(25);
// Collect the previous batch
ret = ch_spi_async_collect(handle, 0, 0);
elapsed = ((double)(_timeMillis() - start)) / 1000;
printf("collected batch #%03d in %.2lf seconds\n", i+1, elapsed);
if (ret < 0) printf("status error: %s\n", ch_status_string(ret));
fflush(stdout);
start = _timeMillis();
// The current batch is now shifting out on the SPI
// interface. The application can again do some more tasks
// here but this entire loop must finish so that a new
// batch is armed before the current batch completes.
ch_sleep_ms(25);
}
// Collect batch the last batch
ret = ch_spi_async_collect(handle, 0, 0);
elapsed = ((double)(_timeMillis() - start)) / 1000;
printf("collected batch #%03d in %.2lf seconds\n", i+1, elapsed);
if (ret < 0) printf("status error: %s\n", ch_status_string(ret));
fflush(stdout);
// Process raw data for the 12-bit ADC's, and write data to text files
int data_idx = 0;
if ( file1.is_open() && file2.is_open() && file3.is_open() ) {
for (int j = 0; j < TX_LENGTH; j += 6) {
// SS3 Data
input = (data_in[j] << 8) + data_in[j+1];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000; //convert 2's comp to signed
data3[data_idx] = valid_data_point;
file3 << data3[data_idx] << ",";
// SS1 Data
input = (data_in[j+2] << 8) + data_in[j+3];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data1[data_idx] = valid_data_point;
file1 << data1[data_idx] << ",";
// SS2 Data
input = (data_in[j+4] << 8) + data_in[j+5];
valid_data_point = (input & 0x3ffc) >> 2;
if(valid_data_point >= 0x0800) valid_data_point = valid_data_point - 0x1000;
data2[data_idx] = valid_data_point;
file2 << data2[data_idx] << ",";
++data_idx;
}
}
else std::cout << "Error opening output filestream!" << std::endl;
