Got an iPhone or iPad? We have a brand new Pastebin App for both devices, and it's totally free! Click here to download the new Pastebin App for iOS.
Public Pastes
Guest

Untitled

By: a guest on Feb 2nd, 2010  |  syntax: None  |  size: 9.39 KB  |  hits: 199  |  expires: Never
download  |  raw  |  embed  |  report abuse
Copied
  1. --- sensors.c   2010-02-02 13:21:25.000000000 +0100
  2. +++ sensors_.c  2010-02-02 13:21:06.000000000 +0100
  3.  -137,53 +135,31 @@
  4.                                 LOGD("using %s (name=%s,phys=%s)", devname, name, phys);
  5.                                 break;
  6.                         }
  7. -                       close(fd);
  8. -                       fd = -1;
  9. -               }
  10. -       }
  11. -       closedir(dir);
  12. -       if(fd==-1) {
  13. -               int status;
  14. -               char buf[5] = {0x14,0,0,0,0};
  15. -               int retries=10;
  16. -
  17. -               fd=open("/dev/bma150", O_RDONLY);
  18. -               if (fd < 0) {
  19. -                       LOGD("bma_open: bma150 open FAILED");
  20. -                       return fd;
  21. -               }
  22. -               bma150=1;
  23. -
  24. -               status = ioctl(fd, BMA_IOCTL_INIT, buf);
  25. -               status = ioctl(fd, BMA_IOCTL_READ, buf);
  26. -               if (status < 0) {
  27. -                        LOGE("bma_open: failed to write bwidth range: %d", status);
  28. -                       return status;
  29. -               }
  30. -
  31. -               // 2G = -2G .. 2G
  32. -               // 4G = -4G .. 4G
  33. -               // 8G = -8G .. 8G
  34. -               // BWIDTH: 25-750Hz digital, 1500Hz analog
  35. -               buf[1] = (buf[1] & 0xe7) | (BMA_RANGE_2G << 3);
  36. -               status = ioctl(fd, BMA_IOCTL_WRITE, buf);
  37. -               if (status < 0) {
  38. -                       LOGE("bma_open: failed to write bwidth range: %d", status);
  39. -                       return status;
  40. -               }
  41. +                       if (!strcmp(name, "bma150")) {
  42. +                               bzero(phys, 20);
  43. +                               ioctl(fd, EVIOCGPHYS(sizeof(phys)-1), &phys);
  44.                 axis_order[0]=-1;
  45.                 axis_order[1]=2;
  46.                 axis_order[2]=-3;
  47.  
  48.                 parse_axis_order();
  49. +                               bma150=1;
  50. +      
  51. +                               LOGD("using %s (name=%s,phys=%s)", devname, name, phys);
  52. +                               break;
  53. +                       }
  54. +                       close(fd);
  55. +                       fd = -1;
  56. +               }
  57.         }
  58. +       closedir(dir);
  59.         return fd;
  60.  }
  61.  
  62.  int sFD=-1;
  63.  int evdev_close(struct hw_device_t *dev) {
  64.  #ifdef DEBUG
  65. -       LOGE("called evdev_close");
  66. +       LOGD("called evdev_close");
  67.  #endif
  68.         close(sFD);
  69.         sFD=-1;
  70.  -194,7 +170,7 @@
  71.         native_handle_t *hdl=native_handle_create(1,0);
  72.         hdl->data[0]=sFD;
  73.  #ifdef DEBUG
  74. -       LOGE("called open_data_source");
  75. +       LOGD("called open_data_source");
  76.  #endif
  77.         return hdl;
  78.  }
  79.  -202,7 +178,7 @@
  80.  int activated=0;
  81.  int activate(struct sensors_control_device_t *dev, int handle, int enabled) {
  82.  #ifdef DEBUG
  83. -       LOGE("sensors activate handle=%d, enabled=%d", handle, enabled);
  84. +       LOGD("sensors activate handle=%d, enabled=%d", handle, enabled);
  85.  #endif
  86.         if(enabled)
  87.                 activated|=1<<handle;
  88.  -211,52 +187,31 @@
  89.  
  90.         enabled=!!( ( activated&(1<<SENSOR_TYPE_ACCELEROMETER)) |
  91.                         (activated&(1<<SENSOR_TYPE_ORIENTATION)));
  92. -       if(bma150) {
  93. -               char buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};;
  94. -               int status;
  95. -               int mode;
  96. -               int retries=20;
  97. -               if (enabled)
  98. -                       mode = BMA_MODE_NORMAL;
  99. -               else
  100. -                       mode = BMA_MODE_SLEEP;
  101. -               buf[0] = mode;
  102. -               do {
  103. -                       status = ioctl(sFD, BMA_IOCTL_SET_MODE, buf);
  104. -               } while( status < 0 && retries-- > 0 );
  105. -               if (status < 0) {
  106. -                       LOGE("activate: IGNORING error setting mode after %d retries (fd=%d, mode=%d, result=%d)", 20, sFD, mode, status);
  107. -                       return 0; //XXX: return status;
  108. -               }
  109. -               return 1;
  110. -       }
  111.  
  112.         //TODO Change file path ?
  113.         char *filename;
  114.         asprintf(&filename, "/sys/class/i2c-adapter/i2c-0/%s/enable", phys+4);
  115.         FILE *fd=fopen(filename, "w");
  116.         free(filename);
  117. -       if(!fd)
  118. +       if(!fd) {
  119. +               LOGD("sensors activate couldn't open %s\n", filename);
  120.                 return -1;
  121. +       }
  122.         fprintf(fd, "%d\n", enabled);
  123.         fclose(fd);
  124.  
  125. -       LOGE("sensors activate returing 1");
  126. +       LOGD("sensors activate returing 1");
  127.         return 1;
  128.  }
  129.  
  130.  int set_delay(struct sensors_control_device_t *dev, int32_t ms) {
  131.  #ifdef DEBUG
  132. -       LOGE("sensors set_delay(%d)", ms);
  133. +       LOGD("sensors set_delay(%d)", ms);
  134.  #endif
  135.         if(ms==0) {
  136.                 //Stupid app ? ok ....
  137.                 ms=100;
  138.         }
  139. -       if(bma150) {
  140. -               wait_time=ms;
  141. -               return 1;
  142. -       }
  143.         char *filename;
  144.         asprintf(&filename, "/sys/class/i2c-adapter/i2c-0/%s/rate", phys+4);
  145.         FILE *fd=fopen(filename, "w");
  146.  -270,21 +225,21 @@
  147.  int wake(struct sensors_control_device_t *dev) {
  148.         //Might need a core server
  149.  #ifdef DEBUG
  150. -       LOGE("called wake");
  151. +       LOGD("called wake");
  152.  #endif
  153.         return 0;
  154.  }
  155.  
  156.  int data_open(struct sensors_data_device_t *dev, native_handle_t* nh) {
  157.  #ifdef DEBUG
  158. -       LOGE("Called data_open");
  159. +       LOGD("Called data_open");
  160.  #endif
  161.         return 0;
  162.  }
  163.  
  164.  int data_close(struct sensors_data_device_t *dev, native_handle_t* nh) {
  165.  #ifdef DEBUG
  166. -       LOGE("Called data close");
  167. +       LOGD("Called data close");
  168.  #endif
  169.         return 0;
  170.  }
  171.  -328,7 +283,7 @@
  172.  }
  173.  
  174.  #define MS2 100.0//To be finely calibrated
  175. -double oldx=0,oldy=0,oldz=0,rawx=0,rawy=0,rawz=0;
  176. +double oldx=0,oldy=0,oldz=0,rawx=0,rawy=0,rawz=0,rawtemp=0;
  177.  int state=0;
  178.  int errcnt=0;
  179.  int data_poll(struct sensors_data_device_t *dev, sensors_data_t* data) {
  180.  -336,14 +291,16 @@
  181.         int ret;
  182.         double value;
  183.         double dat[3];
  184. +       static float temp=0;
  185. +       static float gFactor = GRAVITY_EARTH * 2.0f / 512.0f;
  186.  #ifdef DEBUG
  187. -       LOGE("called data poll");
  188. +       LOGD("called data poll");
  189.  #endif
  190.         data->time=time(NULL)*1000*1000*1000;
  191.  
  192.         if(state==1) {
  193.  #ifdef DEBUG
  194. -               LOGE("Got accel data, send orientation");
  195. +               LOGD("Got accel data, send orientation");
  196.  #endif
  197.                 state=2;
  198.                 data->sensor=SENSOR_TYPE_ORIENTATION;
  199.  -359,17 +316,17 @@
  200.                 data->orientation.pitch=-data->orientation.pitch;
  201.                 data->orientation.roll=asin(oldx/value)*180.0/3.14159;
  202.  #ifdef DEBUG
  203. -               LOGE("Returing orientation %f,%f,%f\n", data->acceleration.x, data->acceleration.y, data->acceleration.z);
  204. +               LOGD("Returing orientation %f,%f,%f\n", data->acceleration.x, data->acceleration.y, data->acceleration.z);
  205.  #endif
  206.                 return SENSOR_TYPE_ORIENTATION;
  207.         } else if(state==2) {
  208.  #ifdef DEBUG
  209. -               LOGE("Sent orientation, send light");
  210. +               LOGD("Sent orientation, send light");
  211.  #endif
  212.                 state=3;
  213.                 value=read_light();
  214.                 if(value<0) {
  215. -                       LOGE("Or not...");
  216. +                       LOGD("Or not...");
  217.                         return data_poll(dev, data);
  218.                 }
  219.                 data->sensor=SENSOR_TYPE_LIGHT;
  220.  -377,55 +334,34 @@
  221.                 return SENSOR_TYPE_LIGHT;
  222.         } else if(state==3) {
  223.  #ifdef DEBUG
  224. -               LOGE("Sent light, send proximity");
  225. +               LOGD("Sent light, send proximity");
  226.  #endif
  227. -               state=0;
  228. +               state=4;
  229.                 value=read_prox();
  230.                 if(value<0) {
  231. -                       LOGE("Or not...");
  232. +                       LOGD("Or not...");
  233.                         return data_poll(dev, data);
  234.                 }
  235.                 data->sensor=SENSOR_TYPE_PROXIMITY;
  236.                 data->distance=value;
  237.                 return SENSOR_TYPE_PROXIMITY;
  238. +       } else if(state==4 && bma150) {
  239. +#ifdef DEBUG
  240. +               LOGD("Sent temperature");
  241. +#endif
  242. +               state=0;
  243. +               data->sensor=SENSOR_TYPE_TEMPERATURE;
  244. +               data->temperature=temp;
  245. +#ifdef DEBUG
  246. +               LOGD("Returing temp %f\n", data->temperature);
  247. +#endif
  248. +               return SENSOR_TYPE_TEMPERATURE;
  249.         }
  250. +         else state = 0;
  251. +      
  252.         if(sFD<0)
  253.                 sFD=open_input();
  254.        
  255. -       if(bma150) {
  256. -               short accel[8] = {0, 0, 0, 0, 0, 0, 0, 0};
  257. -               int retries=20;
  258. -               int status;
  259. -               static float gFactor = 9.81 * 2.0f / 512.0f;
  260. -
  261. -               //We should check for Delta t instead of waiting constant time.
  262. -               usleep(wait_time*1000);
  263. -               do {
  264. -                       status = ioctl(sFD, BMA_IOCTL_READ_ACCELERATION, accel);
  265. -               } while( status < 0 && retries-- > 0 );
  266. -
  267. -               if (status < 0) {
  268. -                       LOGE("Error polling for data after %d retries: %d, fd=%d", retries, status, sFD);
  269. -                       if(!errcnt) {
  270. -                               errcnt=1;
  271. -                               close(sFD);
  272. -                               sFD=-1;
  273. -                               return 0;
  274. -                       }
  275. -                       else {
  276. -                               PR_LN();
  277. -                               return -1;
  278. -                       }
  279. -               } else {
  280. -                       errcnt=0;
  281. -                       LOGE("bma_open: BUFFER: %d:%d:%d", accel[0], accel[1], accel[2]);
  282. -               }
  283. -
  284. -               dat[0]=accel[0]*gFactor;
  285. -               dat[1]=accel[1]*gFactor;
  286. -               dat[2]=accel[2]*gFactor;
  287. -
  288. -       } else {
  289.                 int got=0;
  290.                 while(!got) {
  291.                         ret=read(sFD, &event, sizeof(event));
  292.  -437,7 +373,7 @@
  293.                         if(ret<sizeof(event))//Hu ?
  294.                                 continue;
  295.  #ifdef DEBUG
  296. -                       LOGE("Got event type: %d/%d/%d", event.type, event.code, event.value);
  297. +               LOGD("Got event type: %d/%d/%d", event.type, event.code, event.value);
  298.  #endif
  299.                         switch(event.type) {
  300.                                 case EV_SYN:
  301.  -451,12 +387,21 @@
  302.                                                 rawy=event.value;
  303.                                         if(event.code==ABS_Z)
  304.                                                 rawz=event.value;
  305. +                               if(event.code==EVENT_TYPE_TEMPERATURE)
  306. +                                       rawtemp=event.value;
  307.                                         break;
  308.                                 default:
  309. -                                       LOGE("Got unknown event type: %d/%d/%d", event.type, event.code, event.value);
  310. +                               LOGD("Got unknown event type: %d/%d/%d", event.type, event.code, event.value);
  311.                                         break;
  312.                         };
  313.                 }
  314. +       if(bma150) {
  315. +               dat[0]=rawx*gFactor;
  316. +               dat[1]=rawy*gFactor;
  317. +               dat[2]=rawz*gFactor;
  318. +               temp = -30.0 + (float)rawtemp/2;
  319. +       }
  320. +       else {
  321.                 dat[0]=rawx/MS2;
  322.                 dat[1]=rawy/MS2;
  323.                 dat[2]=rawz/MS2;
  324.  -469,18 +414,18 @@
  325.         data->acceleration.y=oldy;
  326.         data->acceleration.z=oldz;
  327.  #ifdef DEBUG
  328. -       LOGE("Returing %f,%f,%f\n", data->acceleration.x, data->acceleration.y, data->acceleration.z);
  329. +       LOGD("Returing %f,%f,%f\n", data->acceleration.x, data->acceleration.y, data->acceleration.z);
  330.  #endif
  331.         return SENSOR_TYPE_ACCELEROMETER;
  332.  }
  333.  
  334.  int evdev_open(const struct hw_module_t* module, const char* id, struct hw_device_t** device) {
  335.  #ifdef DEBUG
  336. -       LOGE("evdev_open");
  337. +       LOGD("evdev_open");
  338.  #endif
  339.         if(strcmp(id, SENSORS_HARDWARE_CONTROL)==0) {
  340.  #ifdef DEBUG
  341. -               LOGE("evdev_open hw ctl");
  342. +               LOGD("evdev_open hw ctl");
  343.  #endif
  344.                 struct sensors_control_device_t *dev;
  345.                 *device=malloc(sizeof(struct sensors_control_device_t));
  346.  -501,12 +446,12 @@
  347.                 if(sFD<0)
  348.                         return -1;
  349.  #ifdef DEBUG
  350. -               LOGE("evdev_open hw ctl returning ok");
  351. +               LOGD("evdev_open hw ctl returning ok");
  352.  #endif
  353.                 return 1;
  354.         } else if(strcmp(id, SENSORS_HARDWARE_DATA)==0) {
  355.  #ifdef DEBUG
  356. -               LOGE("evdev_open hw data");
  357. +               LOGD("evdev_open hw data");
  358.  #endif
  359.                 struct sensors_data_device_t *dev;
  360.                 *device=malloc(sizeof(struct sensors_data_device_t));
  361.  -525,7 +470,7 @@
  362.                 if(sFD<0)
  363.                         return -1;
  364.  #ifdef DEBUG
  365. -               LOGE("evdev_open hw data returning ok");
  366. +               LOGD("evdev_open hw data returning ok");
  367.  #endif
  368.                 return 1;
  369.         } else {
create a new version of this paste RAW Paste Data