00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
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
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;
00050
00051
00052
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
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
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
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
00095 int16_t s16int;
00096
00097 if ( (chMSB & 0x80) == 0 ) {
00098
00099 s16int = (int16_t) ((((uint16_t)chMSB)&0xFF)<<6) + ((((uint16_t)chLSB)&0xFC)>>2);
00100 }
00101 else {
00102
00103
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
00117
00118 if (usecs >= 10000) {
00119 usleep(usecs);
00120 return 1;
00121 }
00122
00123 do {
00124 gettimeofday(&CurrentTime, NULL);
00125 } while (timercmp(&CurrentTime, &EndTime, <));
00126 return 1;
00127 }
00128
00129
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
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
00145 int ret = 0, fd0;
00146
00147
00148 fd0 = open(device0, O_RDWR);
00149 if (fd0 < 0)
00150 pabort("can't open device");
00151 conv_device(ret, fd0);
00152
00153
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
00162
00163
00164 uint8_t tx_meas[] = {0x82, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80};
00165
00166
00167
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
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
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
00212
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
00221
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
00230 for (i = 0; i < NUM_SENSORS; i++) {
00231 if(s>=(shm + xSHMSZ)){
00232 s = shm;
00233 s++;
00234 }
00235
00236 ret = ioctl(fd0, SPI_IOC_MESSAGE(1), &tr_acc);
00237
00238
00239 valAvail[i][0] = rx_meas[1] & 0x1;
00240 valAvail[i][1] = rx_meas[3] & 0x1;
00241 valAvail[i][2] = rx_meas[5] & 0x1;
00242
00243
00244 valAccel[i][0] = reint_as_2c(rx_meas[2], rx_meas[1]);
00245 valAccel[i][1] = reint_as_2c(rx_meas[4], rx_meas[3]);
00246 valAccel[i][2] = reint_as_2c(rx_meas[6], rx_meas[5]);
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
00279 if(rem_time>0) {
00280 udelay(rem_time);
00281 } else {
00282
00283 msgs_undersampled++;
00284 }
00285 gettimeofday(&curr_t, NULL);
00286
00287
00288 } while(timercmp(&curr_t, &endLoop, <));
00289
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
00296
00297
00298 cout << "End memory <writer> : " << s << endl;
00299
00300
00301 exit(0);
00302 }