biotac.c
Go to the documentation of this file.
00001 /*=========================================================================
00002 | (c) 2011-2012  SynTouch LLC
00003 |--------------------------------------------------------------------------
00004 | Project : BioTac C Library for Cheetah
00005 | File    : biotac.c
00006 | Authors : Gary Lin (gary.lin@syntouchllc.com)
00007 |                       Tomonori Yamamoto (tomonori.yamamoto@syntouchllc.com)
00008 |                       Jeremy Fishel (jeremy.fishel@syntouchllc.com)
00009 | Modified: Ian McMahon (University of Pennsylvania)
00010 |--------------------------------------------------------------------------
00011 | Function: BioTac-Cheetah communication functions
00012 |--------------------------------------------------------------------------
00013 | Redistribution and use of this file in source and binary forms, with
00014 | or without modification, are permitted.
00015 |
00016 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00017 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00018 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00019 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
00020 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00022 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00024 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00025 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00026 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 | POSSIBILITY OF SUCH DAMAGE.
00028  ========================================================================*/
00029 
00030 #define DEFAULT_TIMER
00031 //#define MACHINE_TIMER
00032 
00033 //=========================================================================
00034 // INCLUDES
00035 //=========================================================================
00036 #ifdef _WIN32
00037 #define _CRT_SECURE_NO_DEPRECATE
00038 #endif
00039 #include <stdio.h>
00040 #include <stdlib.h>
00041 #include <string.h>
00042 
00043 #if defined (__linux__) || defined (__APPLE__)
00044 #include <sys/time.h>
00045 #include <unistd.h>
00046 #elif defined (_WIN32)
00047 #include <Windows.h>
00048 #endif
00049 
00050 #include "biotac_sensors/biotac.h"
00051 
00052 //=========================================================================
00053 // CONSTANTS
00054 //=========================================================================
00055 static const unsigned char parity_values[] = \
00056                 { 0x01, 0x02, 0x04, 0x07, 0x08, 0x0B, 0x0D, 0x0E, \
00057                   0x10, 0x13, 0x15, 0x16, 0x19, 0x1A, 0x1C, 0x1F, \
00058                   0x20, 0x23, 0x25, 0x26, 0x29, 0x2A, 0x2C, 0x2F, \
00059                   0x31, 0x32, 0x34, 0x37, 0x38, 0x3B, 0x3D, 0x3E, \
00060                   0x40, 0x43, 0x45, 0x46, 0x49, 0x4A, 0x4C, 0x4F, \
00061                   0x51, 0x52, 0x54, 0x57, 0x58, 0x5B, 0x5D, 0x5E, \
00062                   0x61, 0x62, 0x64, 0x67, 0x68, 0x6B, 0x6D, 0x6E, \
00063                   0x70, 0x73, 0x75, 0x76, 0x79, 0x7A, 0x7C, 0x7F, \
00064                   0x80, 0x83, 0x85, 0x86, 0x89, 0x8A, 0x8C, 0x8F, \
00065                   0x91, 0x92, 0x94, 0x97, 0x98, 0x9B, 0x9D, 0x9E, \
00066                   0xA1, 0xA2, 0xA4, 0xA7, 0xA8, 0xAB, 0xAD, 0xAE, \
00067                   0xB0, 0xB3, 0xB5, 0xB6, 0xB9, 0xBA, 0xBC, 0xBF, \
00068                   0xC1, 0xC2, 0xC4, 0xC7, 0xC8, 0xCB, 0xCD, 0xCE, \
00069                   0xD0, 0xD3, 0xD5, 0xD6, 0xD9, 0xDA, 0xDC, 0xDF, \
00070                   0xE0, 0xE3, 0xE5, 0xE6, 0xE9, 0xEA, 0xEC, 0xEF, \
00071                   0xF1, 0xF2, 0xF4, 0xF7, 0xF8, 0xFB, 0xFD, 0xFE};
00072 
00073 static const char *command_name[] = \
00074                 { "PAC", "PDC", "TAC", "TDC", "   ", "   ", "   ", "   ", "   ", "   ", \
00075                   "   ", "   ", "   ", "   ", "   ", "HAL", "REV", "E01", "E02", "E03", \
00076                   "E04", "E05", "E06", "E07", "E08", "E09", "E10", "E11", "E12", "E13", \
00077                   "E14", "E15", "E16", "E17", "E18", "E19", "   ", "   ", "   ", "   ", \
00078                   "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", \
00079                   "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", "   ", \
00080                   "   ", "   ", "   ", "   "};
00081 
00082 static const char read_command_in_array[] = BT_READ_PROPERTY_COMMAND_ARRAY;
00083 #define NUMBER_OF_READ_COMMAND_IN_ARRAY (int)(sizeof(read_command_in_array)/(sizeof(char)))
00084 
00085 #if defined (MACHINE_TIMER)
00086 #if defined (__linux__) || defined (__APPLE__)
00087 static double time_of_frame_start, time_of_frame_end;
00088 static struct timeval tv;
00089 time_t curtime_sec, curtime_usec;
00090 double time_step;
00091 #elif defined (_WIN32)
00092 LARGE_INTEGER frequency;
00093 LARGE_INTEGER time_of_frame_start, time_of_frame_end;
00094 double time_step;
00095 #endif
00096 #endif /* MACHINE_TIMER */
00097 
00098 static int count = 0;
00099 
00100 
00101 //=========================================================================
00102 // INITIAILIZE CHEETAH FOR BIOTAC COMMUNICATION
00103 //=========================================================================
00104 BioTac bt_cheetah_initialize(const bt_info *biotac, Cheetah *ch_handle)
00105 {
00106           int mode = 0;
00107     u16 ports[16];
00108     u32 unique_ids[16];
00109     int nelem = 1;
00110 
00111     int i;
00112     int count;
00113 
00114     // Find all the attached Cheetah devices
00115   if(PRINT_ON) printf("Searching for Cheetah adapters...\n");
00116         count = ch_find_devices_ext(nelem, ports, nelem, unique_ids);
00117   if(PRINT_ON) printf("%d device(s) found:\n", count);
00118     // Check the number of Cheetah devices found
00119     if (count == 0)
00120     {
00121         if(PRINT_ON) printf("Error: No Cheetah devices found!\n");
00122       return BT_NO_CHEETAH_DETECTED;
00123     }
00124     else if (count > nelem)
00125     {
00126         // the current version supports only one Cheetah device
00127         if(PRINT_ON) printf("WARNING: The current version of software supports one Cheetah device\n");
00128         count = nelem;
00129     }
00130 
00131     for (i = 0; i < count; ++i)
00132     {
00133         // Determine if the device is in-use
00134         const char *status = "(avail) ";
00135         if (ports[i] & CH_PORT_NOT_FREE)
00136         {
00137                 ports[i] &= ~CH_PORT_NOT_FREE;
00138             status = "(in-use)";
00139         }
00140 
00141         // Display device port number, in-use status, and serial number
00142         if(PRINT_ON) printf("    port=%-3d %s (%04d-%06d)\n",
00143                ports[i], status,
00144                unique_ids[i]/1000000,
00145                unique_ids[i]%1000000);
00146 
00147         // Open the device
00148         *ch_handle = ch_open(ports[i]);
00149         if ((*ch_handle) <= 0)
00150         {
00151                 if(PRINT_ON) printf("Unable to open Cheetah device on port %d\n", ports[i]);
00152                 if(PRINT_ON) printf("Error code = %d (%s)\n", (*ch_handle), ch_status_string((*ch_handle)));
00153                 return BT_UNABLE_TO_OPEN_CHEETAH;
00154         }
00155 
00156         if(PRINT_ON) printf("Opened Cheetah device on port %d\n", ports[i]);
00157 
00158         if(PRINT_ON) printf("Host interface is %s\n", (ch_host_ifce_speed((*ch_handle))) ? "high speed" : "full speed");
00159 
00160         // Ensure that the SPI subsystem is configured
00161         ch_spi_configure((*ch_handle), (mode >> 1), mode & 1, CH_SPI_BITORDER_MSB, 0x0);
00162         if(PRINT_ON) printf("SPI configuration set to mode %d, MSB shift, SS[2:0] active low\n", mode);
00163         if(PRINT_ON) fflush(stdout);
00164 
00165                 ch_target_power((*ch_handle), CH_TARGET_POWER_ON);
00166                 ch_sleep_ms(100);
00167 
00168                 // Set the spi_clock_speed
00169                 ch_spi_bitrate((*ch_handle), biotac->spi_clock_speed);
00170                 if(PRINT_ON) printf("Bitrate set to %d kHz\n", biotac->spi_clock_speed);
00171                 if(PRINT_ON) fflush(stdout);
00172         
00173                 /* PLACEHOLDER FOR FUTURE BIOTAC PROPERTY MEASUREMENTS */
00174 
00175                 ch_spi_queue_clear((*ch_handle));
00176                 ch_spi_queue_oe((*ch_handle), 1);
00177     }
00178 
00179         return BT_OK;
00180 }
00181 
00182 
00183 //=========================================================================
00184 // GET BIOTAC PROPERTIES
00185 //=========================================================================
00186 BioTac bt_cheetah_get_properties(Cheetah ch_handle, int bt_select, bt_property *property)
00187 {
00188         int i, len;
00189         BioTac bt_err_code = BT_OK;
00190         u08 tmp[100];
00191         u08 read_command_in_array[] = BT_READ_PROPERTY_COMMAND_ARRAY;
00192         u08 bt_select_cmd = CS_NONE_BT;
00193 
00194         switch(bt_select)
00195         {
00196         case 1:
00197                 bt_select_cmd = CS_BT1;
00198                 break;
00199         case 2:
00200                 bt_select_cmd = CS_BT2;
00201                 break;
00202         case 3:
00203                 bt_select_cmd = CS_BT3;
00204                 break;
00205 #if MAX_BIOTACS_PER_CHEETAH == 5
00206         case 4:
00207                 bt_select_cmd = CS_BT4;
00208                 break;
00209         case 5:
00210                 bt_select_cmd = CS_BT5;
00211                 break;
00212 #endif
00213         default:
00214                 bt_err_code = BT_WRONG_NUMBER_ASSIGNED;
00215                 return bt_err_code;
00216         }
00217 
00218         for(i = 0; i < NUMBER_OF_READ_COMMAND_IN_ARRAY; i++)
00219         {
00220                 ch_spi_queue_clear(ch_handle);                                          // clean previous commands in Cheetah buffer
00221                 ch_spi_queue_ss(ch_handle, bt_select_cmd);                              // select a BioTac
00222                 ch_spi_queue_byte(ch_handle, 1, BT_READ_COMMAND);                       // queue the read command (1 byte)
00223                 ch_spi_queue_byte(ch_handle, 1, read_command_in_array[i]);              // queue the property command (1 byte)
00224                 ch_spi_queue_ss(ch_handle, CS_NONE_BT);                                 // deselect all the Biotacs
00225                 ch_spi_queue_delay_ns(ch_handle, BT_AFTERSAMPLE_DELAY_DEFAULT);         // delay for BioTac processing the command
00226         ch_spi_batch_shift(ch_handle, 2, tmp);                                                                  // send out a 2-byte command
00227 
00228         len = 0;
00229 
00230                 switch(read_command_in_array[i])
00231                 {
00232                 case BT_FLEX_VERSION_READ_COMMAND:
00233                         do {
00234                                         ch_spi_queue_clear(ch_handle);
00235                                         ch_spi_queue_ss(ch_handle, bt_select_cmd);
00236                                         ch_spi_queue_byte(ch_handle, 1, 0);
00237                                         ch_spi_batch_shift(ch_handle, 1, &(property->flex_version[len]));
00238                                         len += 1;
00239                                         if(len > 100)
00240                                         {
00241                                                 break;
00242                                         }
00243                         } while (property->flex_version[len-1] != '\0' && property->flex_version[len-1] != 0xFF);
00244                         ch_spi_queue_clear(ch_handle);
00245                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00246                         ch_spi_batch_shift(ch_handle, 0, tmp);
00247                         break;
00248                 case BT_FIRMWARE_VERSION_READ_COMMAND:
00249                         do {
00250                                         ch_spi_queue_clear(ch_handle);
00251                                         ch_spi_queue_ss(ch_handle, bt_select_cmd);
00252                                         ch_spi_queue_byte(ch_handle, 1, 0);
00253                                         ch_spi_batch_shift(ch_handle, 1, &(property->firmware_version[len]));
00254                                         len += 1;
00255                                         if(len > 100)
00256                                         {
00257                                                 break;
00258                                         }
00259                         } while (property->firmware_version[len-1] != '\0' && property->firmware_version[len-1] != 0xFF);
00260                         ch_spi_queue_clear(ch_handle);
00261                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00262                         ch_spi_batch_shift(ch_handle, 0, tmp);
00263                         break;
00264                 case BT_SERIAL_NUMBER_READ_COMMAND:
00265                         do {
00266                                         ch_spi_queue_clear(ch_handle);
00267                                         ch_spi_queue_ss(ch_handle, bt_select_cmd);
00268                                         ch_spi_queue_byte(ch_handle, 1, 0);
00269                                         ch_spi_batch_shift(ch_handle, 1, &(property->serial_number[len]));
00270                                         len += 1;
00271                                         if(len > 100)
00272                                         {
00273                                                 break;
00274                                         }
00275                         } while (property->serial_number[len-1] != '\0' && property->serial_number[len-1] != 0xFF);
00276                         ch_spi_queue_clear(ch_handle);
00277                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00278                         ch_spi_batch_shift(ch_handle, 0, tmp);
00279                         break;
00280                 case BT_CPU_SPEED_READ_COMMAND:
00281                         ch_spi_queue_clear(ch_handle);
00282                         ch_spi_queue_ss(ch_handle, bt_select_cmd);
00283                         ch_spi_queue_byte(ch_handle, 2, 0);
00284                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00285                         ch_spi_batch_shift(ch_handle, 2, tmp);
00286                         property->cpu_speed.value = tmp[0] * 256 + tmp[1];
00287                         break;
00288                 default:
00289                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00290                         bt_err_code = BT_ERROR_UNKNOWN_COMMAND;
00291                         return bt_err_code;
00292                 }
00293         }
00294 
00295         // Print out properties of the BioTac(s)
00296         if(PRINT_ON) printf("\n------- BioTac %d -------\n", bt_select);
00297 
00298         if (property->serial_number[0] == 'B' && property->serial_number[1] == 'T')
00299         {
00300                 property->bt_connected = YES;
00301                 if(property->firmware_version[0] == '0' && property->firmware_version[1] == '2')
00302                 {
00303                         if(PRINT_ON) printf("Flex Version:\t\t %c.%c\n", property->flex_version[0],  property->flex_version[1]);
00304             if(PRINT_ON) printf("Firmware Version:\t %c.%c%c\n", \
00305                         property->firmware_version[1], \
00306                                         property->firmware_version[2], property->firmware_version[3]);
00307                         if(PRINT_ON) printf("Serial Number:\t\t %c%c-%c%c-%c%c.%c.%c-%c-%c%c-%c-%c%c%c%c\n", \
00308                                         property->serial_number[0], property->serial_number[1], \
00309                     property->serial_number[2], property->serial_number[3], \
00310                     property->serial_number[4], property->serial_number[5], \
00311                     property->serial_number[6], property->serial_number[7], \
00312                     property->serial_number[8], property->serial_number[9], \
00313                     property->serial_number[10], property->serial_number[11], \
00314                     property->serial_number[12], property->serial_number[13], \
00315                     property->serial_number[14], property->serial_number[15]);
00316                         if(PRINT_ON) printf("CPU Speed:\t\t %.1f MIPS\n", (double)(property->cpu_speed.value/(double)1000));
00317                 }
00318                 else
00319                 {
00320                         if(PRINT_ON) printf("Flex Version:\t\t %s\n", property->flex_version);
00321             if(PRINT_ON) printf("Firmware Version:\t %s\n", property->firmware_version);
00322             if(PRINT_ON) printf("Serial Number:\t\t %s\n", property->serial_number);
00323             if(PRINT_ON) printf("CPU Speed:\t\t %.1f MIPS\n", (double)(property->cpu_speed.value/(double)1000));
00324                 }
00325         }
00326         else
00327         {
00328                 property->bt_connected = NO;
00329 
00330                 if(PRINT_ON) printf("Flex Version:\t\t %s\n", "N/A");
00331                 if(PRINT_ON) printf("Software Version:\t %s\n", "N/A");
00332                 if(PRINT_ON) printf("Serial Number:\t\t %s\n", "N/A");
00333                 if(PRINT_ON) printf("CPU Speed:\t\t %s\n", "N/A");
00334         }
00335 
00336         return bt_err_code;
00337 }
00338 
00339 //=========================================================================
00340 // CONFIGURATION OF BATCH
00341 //=========================================================================
00342 BioTac bt_cheetah_configure_batch(Cheetah ch_handle, bt_info *biotac, int num_samples)
00343 {
00344         BioTac bt_err_code = BT_OK;
00345         int i, j, k;
00346         int bt_select_cmd;
00347         int words_per_sample = MAX_BIOTACS_PER_CHEETAH + 1;
00348         int cs_per_sample = MAX_BIOTACS_PER_CHEETAH + 3;
00349         double additional_delay = \
00350                         (1/(double)biotac->sample_rate_Hz)*1000000000 \
00351                         - ((words_per_sample*16 + cs_per_sample*8) / (double)biotac->spi_clock_speed)*1000000 \
00352                         - BT_AFTERSAMPLE_DELAY_DEFAULT - BT_INTERWORD_DELAY_DEFAULT - 3000;     // in nano second
00353         const char frame_structure_tmp[] = BT_FRAME_STRUCTURE_DEFAULT;
00354 
00355         // Set frame structure and frame count (a future version may support change of frame structures while running the program) 
00356         strcpy(biotac->frame.frame_structure, frame_structure_tmp);
00357         biotac->frame.frame_size = (int)(sizeof(frame_structure_tmp)/(sizeof(char)) - 1); // -1: subtract the null character
00358         biotac->batch.batch_frame_count = (int)(biotac->batch.batch_ms / \
00359                 (double)((1/(double)biotac->sample_rate_Hz) * (biotac->frame.frame_size) * 1000));
00360 
00361         // Check if num_samples is long enough
00362         if(num_samples < ((biotac->frame.frame_size) * (biotac->batch.batch_frame_count)))
00363         {
00364                 bt_err_code = BT_DATA_SIZE_TOO_SMALL;
00365                 return bt_err_code;
00366         }
00367 
00368         // Configure BioTac sampling
00369         ch_spi_queue_clear(ch_handle);
00370 
00371         for (i = 0; i < biotac->batch.batch_frame_count; i++)
00372         {
00373                 for (j = 0; j < biotac->frame.frame_size; j++)
00374                 {
00375                         ch_spi_queue_ss(ch_handle, CS_ALL_BT);
00376                         ch_spi_queue_byte(ch_handle, 1, biotac->frame.frame_structure[j]);
00377                         ch_spi_queue_byte(ch_handle, 1, 0x00);
00378                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00379                         ch_spi_queue_delay_ns(ch_handle, BT_AFTERSAMPLE_DELAY_DEFAULT);
00380                         for (k = 1; k <= MAX_BIOTACS_PER_CHEETAH; k++)
00381                         {
00382                                 switch(k)
00383                                 {
00384                                 case 1:
00385                                         bt_select_cmd = CS_BT1;
00386                                         break;
00387                                 case 2:
00388                                         bt_select_cmd = CS_BT2;
00389                                         break;
00390                                 case 3:
00391                                         bt_select_cmd = CS_BT3;
00392                                         break;
00393 #if MAX_BIOTACS_PER_CHEETAH == 5
00394                                 case 4:
00395                                         bt_select_cmd = CS_BT4;
00396                                         break;
00397                                 case 5:
00398                                         bt_select_cmd = CS_BT5;
00399                                         break;
00400 #endif
00401                                 }
00402                                 ch_spi_queue_ss(ch_handle, (u08)bt_select_cmd);
00403                                 ch_spi_queue_byte(ch_handle, 2, 0x00);
00404                         }
00405                         ch_spi_queue_ss(ch_handle, CS_NONE_BT);
00406                         ch_spi_queue_delay_ns(ch_handle, BT_INTERWORD_DELAY_DEFAULT);
00407                         ch_spi_queue_delay_ns(ch_handle, (int)additional_delay);
00408                 }
00409         }
00410 
00411         /**** Time stamps from a computer (by default, it's disabled) ****/
00412 #ifdef MACHINE_TIMER
00413 #if defined (__linux__) || defined (__APPLE__)
00414         // Get a start time stamp
00415         gettimeofday(&tv, NULL);
00416         curtime_sec = tv.tv_sec;
00417         curtime_usec = tv.tv_usec;
00418 
00419         time_of_frame_start = curtime_sec + (double)curtime_usec/1000000;
00420 #elif defined (_WIN32)
00421         QueryPerformanceFrequency(&frequency);
00422         QueryPerformanceCounter(&time_of_frame_start);
00423 #endif
00424 #endif /* MACHINE_TIMER */
00425 
00426         // Send initial batches
00427         for (i = 0; i < 16; i++)
00428         {
00429                 ch_spi_async_submit(ch_handle);
00430         }
00431 
00432         return bt_err_code;
00433 }
00434 
00435 
00436 //=========================================================================
00437 // CONFIGURATION OF SAVE BUFFER
00438 //=========================================================================
00439 bt_data* bt_configure_save_buffer(int number_of_samples)
00440 {
00441         bt_data *data;
00442         count = 0;
00443         data = malloc(number_of_samples * sizeof *data);
00444         return data;
00445 }
00446 
00447 
00448 //=========================================================================
00449 // COLLECT BATCH
00450 //=========================================================================
00451 void bt_cheetah_collect_batch(Cheetah ch_handle, const bt_info *biotac, bt_data *data, BOOL print_flag)
00452 {
00453         int i, j;
00454         int byte_shift = 2 + (MAX_BIOTACS_PER_CHEETAH * 2);                     // 2 bytes of command + 2 bytes per BioTac data
00455         int spi_data_len;
00456         int number_of_samples_in_batch;
00457         u08 *bt_raw_data;
00458 
00459         spi_data_len = ch_spi_batch_length(ch_handle);
00460         bt_raw_data = malloc(spi_data_len * sizeof *bt_raw_data);
00461         ch_spi_async_collect(ch_handle, spi_data_len, bt_raw_data);
00462         ch_spi_async_submit(ch_handle);
00463 
00464         number_of_samples_in_batch = spi_data_len/byte_shift;
00465 
00466         /**** Time stamps from a computer (by default, it's disabled) ****/
00467 #ifdef MACHINE_TIMER
00468 #if defined (__linux__) || defined (__APPLE__)
00469         gettimeofday(&tv, NULL);
00470         curtime_sec = tv.tv_sec;
00471         curtime_usec = tv.tv_usec;
00472 
00473         time_of_frame_end = curtime_sec + (double)curtime_usec/1000000;
00474         time_step = (time_of_frame_end - time_of_frame_start) / number_of_samples_in_batch;
00475         time_of_frame_start = time_of_frame_end;
00476 #elif defined (_WIN32)
00477         QueryPerformanceCounter(&time_of_frame_end);
00478         time_step = ((time_of_frame_end.QuadPart - time_of_frame_start.QuadPart) / (double)frequency.QuadPart) / number_of_samples_in_batch;
00479         time_of_frame_start = time_of_frame_end;
00480 #endif
00481 #endif /* MACHINE_TIMER */
00482 
00483         for(i = 0; i < number_of_samples_in_batch; i++)
00484         {
00485                 if(count != 0)
00486                 {
00487                         if(i==0)
00488                         {
00489                                 data[count].batch_index = data[count-1].batch_index + 1;
00490                         }
00491                         else
00492                         {
00493                                 data[count].batch_index = data[count-1].batch_index;
00494                         }
00495                         if((i%(biotac->frame.frame_size)) == 0)
00496                         {
00497                                 data[count].frame_index = data[count-1].frame_index + 1;
00498                         }
00499                         else
00500                         {
00501                                 data[count].frame_index = data[count-1].frame_index;
00502                         }
00503                         /**** Time stamps from a computer (by default, it's disabled) ****/
00504 #ifdef DEFAULT_TIMER
00505                         data[count].time = 0.0;
00506 #elif defined MACHINE_TIMER
00507                         data[count].time = data[count-1].time + time_step;
00508 #endif
00509                 }
00510                 else
00511                 {
00512                         data[count].batch_index = 1;
00513                         data[count].frame_index = 1;
00514                         data[count].time = 0;
00515                 }
00516 
00517                 data[count].channel_id = (biotac->frame.frame_structure[i%(biotac->frame.frame_size)] & 0x7E) >> 1;
00518 
00519                 for(j = 0; j < MAX_BIOTACS_PER_CHEETAH; j++)
00520                 {
00521                         data[count].d[j].word = (bt_raw_data[i*byte_shift + j*2 + 2] >> 1) * 32 + (bt_raw_data[i*byte_shift + j*2 + 3] >> 3);
00522 
00523                         if((parity_values[bt_raw_data[i*byte_shift + j*2 + 2] >> 1] == bt_raw_data[i*byte_shift + j*2 + 2]) && \
00524                                         (parity_values[bt_raw_data[i*byte_shift + j*2 + 3] >> 1] == bt_raw_data[i*byte_shift + j*2 + 3]))
00525                         {
00526                                 data[count].bt_parity[j] = PARITY_GOOD;
00527                         }
00528                         else
00529                         {
00530                                 data[count].bt_parity[j] = PARITY_BAD;
00531                         }
00532                 }
00533 
00534                 // Print data on Terminal
00535                 if(print_flag)
00536                 {
00537                         if(PRINT_ON) printf("%8d,  ", count);
00538                         /**** Time stamps from a computer (by default, it only displays NULL) ****/
00539 #ifdef DEFAULT_TIMER
00540                         if(PRINT_ON) printf("%s, ", "NULL");
00541 #elif defined MACHINE_TIMER
00542                         if(PRINT_ON) printf("%3.6f, ", data[count].time);
00543 #endif
00544                         if(PRINT_ON) printf("%6.0f, %6.0f,  %s ", data[count].batch_index, data[count].frame_index, command_name[data[count].channel_id]);
00545                         for(j = 0; j < MAX_BIOTACS_PER_CHEETAH; j++)
00546                         {
00547                                 if(PRINT_ON) printf("%6d, %d; ", data[count].d[j].word, data[count].bt_parity[j]);
00548                         }
00549                         if(PRINT_ON) printf("\n");
00550                 }
00551                 count++;
00552         }
00553         free(bt_raw_data);
00554 }
00555 
00556 
00557 //=========================================================================
00558 // SAVE DATA IN A FILE
00559 //=========================================================================
00560 BioTac bt_save_buffer_data(const char *file_name, const bt_data *data, int num_samples)
00561 {
00562         int i, j;
00563         FILE *fp;
00564 
00565         fp = fopen(file_name, "w");
00566         if (!fp)
00567         {
00568                 if(PRINT_ON) printf("Error: Cannot open output file.\n");
00569                 return BT_UNABLE_TO_OPEN_FILE;
00570         }
00571 
00572         // Write data in the file. By default, a saved data format is as follows:
00573         // (time, batch_index, frame_index, channel_id, value[0], bt_parity[0],  value[1], bt_parity[1],  value[2], bt_parity[2], ...)
00574         for (i = 0; i < num_samples; i++)
00575         {
00576                 fprintf(fp, "%.6f %.0f %.0f %u ", data[i].time, data[i].batch_index, data[i].frame_index, data[i].channel_id);
00577 //              fprintf(fp, "%.6f %.0f %.0f %s ", data[i].time, data[i].batch_index, data[i].frame_index, command_name[data[i].channel_id]);
00578                 for (j = 0; j < MAX_BIOTACS_PER_CHEETAH; j++)
00579                 {
00580                         fprintf(fp, "%d %d ", data[i].d[j].word, data[i].bt_parity[j]);
00581                 }
00582                 fprintf(fp, "\n");
00583         }
00584 
00585         fclose(fp);
00586 
00587         if(PRINT_ON) printf("Saved data in %s\n", file_name);
00588   return BT_OK;
00589 }
00590 
00591 
00592 //=========================================================================
00593 // PRINT ERROR CODES
00594 //=========================================================================
00595 void bt_display_errors(BioTac bt_err_code)
00596 {
00597         char *error_str = malloc(100 * sizeof(*error_str));
00598 
00599         switch(bt_err_code)
00600         {
00601         case BT_WRONG_NUMBER_ASSIGNED:
00602                 strcpy(error_str, "Wrong BioTac number assigned!");
00603                 break;
00604         case BT_NO_BIOTAC_DETECTED:
00605                 strcpy(error_str, "No BioTac detected!");
00606                 break;
00607         case BT_WRONG_MAX_BIOTAC_NUMBER:
00608                 strcpy(error_str, "Wrong maximum number of BioTacs assigned (should be 3 or 5)!");
00609                 break;
00610         case BT_DATA_SIZE_TOO_SMALL:
00611                 strcpy(error_str, "The number of samples is too small!");
00612                 break;
00613   case BT_NO_CHEETAH_DETECTED:
00614     strcpy(error_str, "No Cheetah device detected!");
00615     break;
00616   case BT_UNABLE_TO_OPEN_CHEETAH:
00617     strcpy(error_str, "Unable to open Cheetah device on current port.");
00618     break;
00619   case BT_UNABLE_TO_OPEN_FILE:
00620     strcpy(error_str, "Cannot open output file.");
00621     break;
00622   default:
00623     strcpy(error_str, "Unrecognized Biotac error encountered.");
00624     break;
00625         }
00626 
00627         if(PRINT_ON) printf("\nError: %s\n\n", error_str);
00628 
00629         free(error_str);
00630 }
00631 
00632 
00633 //=========================================================================
00634 // CLOSE CHEETAH CONFIGURATION
00635 //=========================================================================
00636 void bt_cheetah_close(Cheetah ch_handle)
00637 {
00638         ch_spi_queue_oe(ch_handle, 0);
00639         ch_close(ch_handle);
00640 }


biotac_sensors
Author(s): Ian McMahon
autogenerated on Thu Aug 27 2015 12:35:30