bma180-writer.cpp
Go to the documentation of this file.
00001 /* -------------------------------------------------------------------------- */
00002 // BMA180 Spi Communication Node (Mux+Counter)
00003 //
00004 //    - Reads from 8 BMA 180's with specified kHz Sampling frequency
00005 //    - Writes message to shared memory with accel data
00006 //
00007 //  created by: Sebastian Haug 2010 | Robert Bosch RTC
00008 //  edited by : Nikhil Deshpande 2011 | Robert Bosch RTC
00009 /* -------------------------------------------------------------------------- */
00010 
00011 // Includes
00012 /* -------------------------------------------------------------------------- */
00013 #include <sstream>
00014 #include <iostream>
00015 #include <stdint.h>
00016 #include <stdio.h>
00017 #include <stdlib.h>
00018 #include <fcntl.h>
00019 #include <ctime>
00020 #include <unistd.h>
00021 #include <sys/ioctl.h>
00022 #include <sys/time.h>
00023 #include <sys/types.h>
00024 #include <sys/ipc.h>
00025 #include <sys/shm.h>
00026 #include <linux/spi/spidev.h>
00027 #include <sched.h>
00028 #include <ros/ros.h>
00029 #include <std_msgs/String.h>
00030 #include <gumstix_memwrite_bma180/writermsg.h>
00031 #include <errno.h>
00032 
00033 using namespace std;
00034 
00035 
00036 // Macros, Defines
00037 /* -------------------------------------------------------------------------- */
00038 #define ARRAY_SIZE(a) (sizeof(a) / sizeof((a)[0]))
00039 #define NUM_SENSORS 8 
00040 #define SHMSZ   30720000        
00041 #define xSHMSZ  0x1D4C000  // hex converted
00042 
00043 // Configuration
00044 /* -------------------------------------------------------------------------- */
00045 static const char *device0      = "/dev/spidev1.1";
00046 static uint8_t mode             = SPI_MODE_3;
00047 static uint8_t bits             = 8;
00048 static uint32_t speed           = 6000000;
00049 static uint16_t delay           = 0; //in us
00050 
00051 
00052 // Static helper functions
00053 /* -------------------------------------------------------------------------- */
00054 static void pabort(const char *s) {
00055         perror(s);
00056         abort();
00057 }
00058 
00059 static void conv_device(int ret, int fd) {
00060         // Spi mode
00061         ret = ioctl(fd, SPI_IOC_WR_MODE, &mode);
00062         if (ret == -1)
00063                 pabort("can't set spi mode");
00064 
00065         ret = ioctl(fd, SPI_IOC_RD_MODE, &mode);
00066         if (ret == -1)
00067                 pabort("can't get spi mode");
00068 
00069         // Bits per word
00070         ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
00071         if (ret == -1)
00072                 pabort("can't set bits per word");
00073 
00074         ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);
00075         if (ret == -1)
00076                 pabort("can't get bits per word");
00077 
00078         // Max speed hz
00079         ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
00080         if (ret == -1)
00081                 pabort("can't set max speed hz");
00082 
00083         ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);
00084         if (ret == -1)
00085                 pabort("can't get max speed hz");
00086 
00087         printf("spi mode: %d\n", mode);
00088         printf("bits per word: %d\n", bits);
00089         printf("max speed: %d Hz (%d MHz)\n", speed, speed / 1000000);
00090 }
00091 
00092 int16_t reint_as_2c(char chMSB, char chLSB)
00093 {
00094         // Reinterprets unsigned integer as 7 bit two's complement number and returns signed integer
00095         int16_t         s16int;                 //2 byte int to build message
00096         //verify if pos or neg
00097         if ( (chMSB & 0x80) == 0 ) {
00098                 //positive number
00099                 s16int = (int16_t) ((((uint16_t)chMSB)&0xFF)<<6) + ((((uint16_t)chLSB)&0xFC)>>2);
00100         }
00101         else {
00102                 //negative number
00103                 //set MSB (sign) to zero and build 2 complement unsigned; offset 8192 for 2^13
00104                 s16int = (int16_t) (((((uint16_t)chMSB)&0x7F)<<6) + ((((uint16_t)chLSB)&0xFC)>>2) - 8192);
00105         }
00106 
00107         return s16int;
00108 }
00109 
00110 int udelay (unsigned int usecs) {
00111         struct timeval StartTime, EndTime, CurrentTime, delay_r;
00112         delay_r.tv_sec = 0;
00113         delay_r.tv_usec = usecs;
00114         gettimeofday(&StartTime, NULL);
00115         timeradd(&StartTime, &delay_r, &EndTime);
00116         //cout << "usecs: " << usecs << endl;
00117         //if usecs > 10000, then usleep works well using fewer resources
00118         if (usecs >= 10000) {
00119                 usleep(usecs);
00120                 return 1;
00121         }
00122         // if usecs < 10000, use tighter loop...
00123         do {
00124                 gettimeofday(&CurrentTime, NULL);
00125         } while (timercmp(&CurrentTime, &EndTime, <));
00126         return 1;
00127 }
00128 
00129 // Main
00130 /* -------------------------------------------------------------------------- */
00131 int main(int argc, char** argv) {
00132         struct sched_param prmtr;
00133         prmtr.sched_priority = 99;
00134         sched_setscheduler(0,SCHED_FIFO,&prmtr);
00135 
00136         // Ros
00137         ros::init(argc, argv, "gumstix_memwrite_bma180");
00138         ros::NodeHandle n;
00139         ros::Publisher pub = n.advertise<gumstix_memwrite_bma180::writermsg> ("writermsg", 10); 
00140         gumstix_memwrite_bma180::writermsg msg;
00141 
00142         double rate_Hz;
00143         int duration;
00144         // Initialize members
00145         int ret = 0, fd0;
00146 
00147         // Open SPI device 0
00148         fd0 = open(device0, O_RDWR);
00149         if (fd0 < 0)
00150                 pabort("can't open device");
00151         conv_device(ret, fd0);
00152 
00153         // get Parameters
00154         if (n.getParam("/gumstix_memwrite_bma180/rate_Hz", rate_Hz)==false) {
00155                 rate_Hz = 1200;
00156         }
00157         if (n.getParam("/gumstix_memwrite_bma180/duration", duration)==false) {
00158                 duration = 2;
00159         }
00160 
00161         // Initialize measurement buffer
00162         /* -------------------------------------------------------------------------- */
00163         // Init tx buffer
00164         uint8_t tx_meas[] = {0x82, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80};
00165         //                   addr, xlsb, xmsb, ylsb, ymsb, zlsb, zmsb
00166 
00167         // Init rx buffer
00168         char rx_meas[ARRAY_SIZE(tx_meas)] = {0};
00169         int16_t valAvail[NUM_SENSORS][3];
00170         int16_t valAccel[NUM_SENSORS][3];
00171         long i=0, k=0, msgs_not_logged=0, msgs_undersampled=0;
00172         uint64_t j=0;
00173         long int num_msgs_sampled=1;
00174 
00175         // Prepare spi message for acceleration measurement
00176         struct spi_ioc_transfer tr_acc;
00177         tr_acc.tx_buf                   = (unsigned long) tx_meas;
00178         tr_acc.rx_buf                   = (unsigned long) rx_meas;
00179         tr_acc.len                      = ARRAY_SIZE(tx_meas);
00180         tr_acc.delay_usecs              = delay;
00181         tr_acc.speed_hz                 = speed;
00182         tr_acc.bits_per_word            = bits;
00183         tr_acc.cs_change                = true;
00184         /* -------------------------------------------------------------------------- */
00185 
00186         // Initialize Shared Memory
00187         int16_t *shm, *s, shmid;
00188         key_t key = 42;
00189         if ((shmid = shmget(key, SHMSZ, IPC_CREAT | 0666)) < 0) {
00190             if(errno == EINVAL)
00191                 printf("Invalid segment size specified\n");
00192             else if(errno == EEXIST)
00193                 printf("Segment exists, cannot create it\n");
00194             else if(errno == EIDRM)
00195                 printf("Segment is marked for deletion or was removed\n");
00196             else if(errno == ENOENT)
00197                 printf("Segment does not exist\n");
00198             else if(errno == EACCES)
00199                 printf("Permission denied\n");
00200             else if(errno == ENOMEM)
00201                 printf("Not enough memory to create segment\n");
00202 
00203         }
00204         if ((shm = (int16_t *) shmat(shmid, NULL, 0)) == (int16_t *) -1) {
00205                 cerr << "shmat did not work! " << shm << endl;
00206         }
00207         s = shm;
00208         s++;
00209         *shm = 10001;
00210         cout << "Begin memory <writer> : " << s << endl;
00211         // Rate object
00212         //ros::Rate loop_rate(rate_Hz);
00213         cout << "Sampling rate: " << rate_Hz << " Hz" << endl;
00214         cout << "Sampling duration: " << duration << " seconds" << endl;
00215 
00216         struct timeval start_t, curr_t, endLoop, loop_t, beforeProc, afterProc, delay_t;
00217         unsigned int delVal = ((unsigned int)(1000000/rate_Hz)); 
00218         long int rem_time;
00219 
00220         // Main loop
00221         // specify data logging duration in seconds...
00222         bool doOnce=true;
00223         int pubThresh=100*duration;
00224         loop_t.tv_sec = duration;       loop_t.tv_usec = 0;
00225         gettimeofday(&start_t, NULL);
00226         timeradd(&start_t, &loop_t, &endLoop);
00227         do {
00228                 gettimeofday(&beforeProc, NULL);
00229                 // Transfer/receive messages
00230                 for (i = 0; i < NUM_SENSORS; i++) {
00231                         if(s>=(shm + xSHMSZ)){
00232                                 s = shm;
00233                                 s++;
00234                         }
00235                         // Do Transfer
00236                         ret = ioctl(fd0, SPI_IOC_MESSAGE(1), &tr_acc);          // get accel
00237 
00238                         // Extract new data bit
00239                         valAvail[i][0] = rx_meas[1] & 0x1;      // new x
00240                         valAvail[i][1] = rx_meas[3] & 0x1;      // new y
00241                         valAvail[i][2] = rx_meas[5] & 0x1;      // new z
00242 
00243                         // Extract and convert acceleration values
00244                         valAccel[i][0] = reint_as_2c(rx_meas[2], rx_meas[1]);   // acc x
00245                         valAccel[i][1] = reint_as_2c(rx_meas[4], rx_meas[3]);   // acc y
00246                         valAccel[i][2] = reint_as_2c(rx_meas[6], rx_meas[5]);   // acc z
00247                         for (k=0; k<3 ;k++) {
00248                                 *(s++) = valAvail[i][k];
00249                                 if(s>=(shm + xSHMSZ)){
00250                                         s = shm;
00251                                         s++;
00252                                 }
00253                                 j++;
00254                         }
00255                         for (k=0; k<3; k++) {
00256                                 *(s++) = valAccel[i][k]; 
00257                                 if(s>=(shm + xSHMSZ)){
00258                                         s = shm;
00259                                         s++;
00260                                 }
00261                                 j++;
00262                         }
00263                 }
00264                 if(num_msgs_sampled>=pubThresh){
00265                         msg.written = true;
00266                         if(doOnce){
00267                                 doOnce = false;
00268                                 pub.publish(msg);
00269                                 ros::spinOnce();
00270                         }
00271                 } else {
00272                         msg.written = false;
00273                 }
00274                 num_msgs_sampled++;
00275                 gettimeofday(&afterProc, NULL);
00276                 timersub(&afterProc, &beforeProc, &delay_t);
00277                 rem_time = delVal - ((delay_t.tv_sec*1000000) + delay_t.tv_usec);
00278                 //cout << "Process time: " << delay_t.tv_usec << " Delay: " << rem_time << endl;
00279                 if(rem_time>0) {
00280                         udelay(rem_time);
00281                 } else {
00282                         //cout << " ** Process delay longer than desired Loop rate: " << rem_time << endl;
00283                         msgs_undersampled++;// = num_msgs_sampled;
00284                 }
00285                 gettimeofday(&curr_t, NULL);
00286                 //cout << "Loop Time: " << curr_t.tv_usec << endl;
00287                 //cout << curr_t.tv_sec << " <compare> " << endLoop.tv_sec << endl;
00288         } while(timercmp(&curr_t, &endLoop, <));
00289         // Close spi devices
00290         close(fd0);
00291         *shm = -112;
00292         cout << "Done writing..." << endl;
00293         cout << "Values stored: " << j << endl;
00294         cout << "Msgs sampled: " << num_msgs_sampled << endl;
00295 //      while(*shm != (j+1)){
00296 //              udelay(6500);
00297 //      }
00298         cout << "End memory <writer> : " << s << endl; 
00299 //      msg.written = false;
00300 //      pub.publish(msg);
00301         exit(0);
00302 }


gumstix_memwrite_bma180
Author(s): Nikhil Deshpande (Maintained by Philip Roan)
autogenerated on Sat Dec 28 2013 16:48:45