Facebook
From Tiny Motmot, 1 Year ago, written in Plain Text.
Embed
Download Paste or View Raw
Hits: 60
  1. #include "mma8451_pi.h"
  2. #include <linux/i2c-dev.h>
  3. #include <sys/ioctl.h>
  4. #include <sys/types.h>
  5. #include <sys/stat.h>
  6. #include <stdio.h>
  7. #include <stdlib.h>
  8. #include <unistd.h>
  9. #include <fcntl.h>
  10. #include <stdint.h>
  11.  
  12. void mma8451_write_byte(mma8451* handle, int reg, char data)
  13. {
  14.     unsigned char outbuf[2];
  15.     struct i2c_rdwr_ioctl_data packets;
  16.     struct i2c_msg messages[1]; //We are just writing one byte to the device
  17.  
  18.     messages[0].addr  = handle->address;
  19.     messages[0].flags = 0;
  20.     messages[0].len   = sizeof(outbuf);
  21.     messages[0].buf   = outbuf;
  22.  
  23.     //Register address
  24.     outbuf[0] = reg;
  25.  
  26.     //Data
  27.     outbuf[1] = data;
  28.  
  29.     //Configure packet list
  30.     packets.msgs  = messages;
  31.     packets.nmsgs = 1;
  32.  
  33.     //Send to the bus
  34.     if(ioctl(handle->file, I2C_RDWR, &packets) < 0)
  35.     {
  36.         perror("Unable to send data");
  37.     }
  38.  
  39. }
  40.  
  41. char mma8451_read_byte(mma8451* handle, int reg)
  42. {
  43.     unsigned char inbuf, outbuf;
  44.     struct i2c_rdwr_ioctl_data packets;
  45.     struct i2c_msg messages[2];
  46.  
  47.     //Send only the address in a write
  48.     outbuf = reg;
  49.     messages[0].addr  = handle->address;
  50.     messages[0].flags = 0;
  51.     messages[0].len   = sizeof(outbuf);
  52.     messages[0].buf   = &outbuf;
  53.  
  54.     //Structure where the data will be written
  55.     messages[1].addr  = handle->address;
  56.     messages[1].flags = I2C_M_RD;
  57.     messages[1].len   = sizeof(inbuf);
  58.     messages[1].buf   = &inbuf;
  59.  
  60.     //Build packet list
  61.     packets.msgs      = messages;
  62.     packets.nmsgs     = 2;
  63.  
  64.     //Send to the bus
  65.     if(ioctl(handle->file, I2C_RDWR, &packets) < 0)
  66.     {
  67.         perror("Unable to send data");
  68.     }
  69.  
  70.     //return the value
  71.     return inbuf;
  72. }
  73.  
  74. void mma8451_read_multibyte(mma8451* handle, int reg, char* output, size_t len)
  75. {
  76.     unsigned char outbuf;
  77.     struct i2c_rdwr_ioctl_data packets;
  78.     struct i2c_msg messages[2];
  79.  
  80.     //Send only the address in a write
  81.     outbuf = reg;
  82.     messages[0].addr  = handle->address;
  83.     messages[0].flags = 0;
  84.     messages[0].len   = sizeof(outbuf);
  85.     messages[0].buf   = &outbuf;
  86.  
  87.     //Structure where the data will be written
  88.     //
  89.     //the mma8451 auto-increment address on successive reads. Reading 3 bytes from 0x01 will read 0x01, 0x02 and 0x03
  90.     messages[1].addr  = handle->address;
  91.     messages[1].flags = I2C_M_RD;
  92.     messages[1].len   = len;    //trusting the user that the lengs is less or equals the size of whatever's output is pointing to
  93.     messages[1].buf   = output; //pointer given by the user
  94.  
  95.     //Build packet list
  96.     packets.msgs      = messages;
  97.     packets.nmsgs     = 2;
  98.  
  99.     //Send to the bus
  100.     if(ioctl(handle->file, I2C_RDWR, &packets) < 0)
  101.     {
  102.         perror("Unable to send data");
  103.     }
  104. }
  105.  
  106. mma8451 mma8451_initialise(int device, int addr)
  107. {
  108.     mma8451 handle;
  109.     handle.file = -1;
  110.     handle.address = addr;
  111.     char buf[15];
  112.    
  113.     printf("Initalising MMA8451 sensor at address %#02x\n", addr);
  114.  
  115.     //Open /dev/i2c-x file without a buffer
  116.     sprintf(buf, "/dev/i2c-%d", device);
  117.     if ((handle.file = open(buf, O_RDWR)) < 0)
  118.     {
  119.         handle.file = -2;
  120.         return handle;
  121.     }
  122.  
  123.     //Configure slave i2c address via ioctl
  124.     if(ioctl(handle.file, I2C_SLAVE, addr) < 0)
  125.     {
  126.         handle.file = -3;
  127.         return handle;
  128.     }
  129.  
  130.     //Check if we read correctly from the sensor
  131.     char whoami = mma8451_read_byte(&handle, 0x0D);
  132.     printf("whoami read %#02x\n", whoami);
  133.  
  134.     //Undefined behavior for the rest of device operation if the device is not returning hex 1A
  135.     if(whoami != 0x1A) perror("mma451_pi warning: Device correctly intialized but not returning 0x1A at WHO_AM_I request.\n"
  136.             "Are you sure you are using a MMA8451 accelerometer on this address?");
  137.  
  138.     //Send reset request
  139.     mma8451_write_byte(&handle, 0x2B, 0x40);
  140.     printf("Waiting for accelerometer to be reset\n");
  141.     while(mma8451_read_byte(&handle, 0x2B) & 0x40); //reset done
  142.     printf("Done\n");
  143.  
  144.     mma8451_set_range(&handle, 2);
  145.     mma8451_write_byte(&handle, 0x2B, 0x02); //high resolution mode
  146.     mma8451_write_byte(&handle, 0x2A, 0x01 | 0x04); //high rate low noise
  147.  
  148.     //Deactivate fifo
  149.     mma8451_write_byte(&handle, 0x09, 0);
  150.     //turn on orientation configuration
  151.     mma8451_write_byte(&handle, 0x11, 0x40);
  152.  
  153.     printf("MMA8451 at address %#02x configured for real time sampling, in high rate, low noise mode, at high resolution, on a 2G max range\n", addr);
  154.  
  155.     return handle;
  156. }
  157.  
  158. void mma8451_get_raw_sample(mma8451* handle, char* output)
  159. {
  160.     mma8451_read_multibyte(handle, 0x01, output, 6);
  161. }
  162.  
  163. uint16_t get_divider(const unsigned char range)
  164. {
  165.     switch (range)
  166.     {
  167.         default:
  168.             perror("unknown range. use 2, 4 or 8 only!");
  169.         case 8:
  170.             return 1024;
  171.         case 4:
  172.             return 2048;
  173.         case 2:
  174.             return 4096;
  175.  
  176.     }
  177. }
  178.  
  179. void mma8451_get_acceleration(mma8451* handle, mma8451_vector3* vect)
  180. {
  181.     int16_t x, y, z;
  182.  
  183.     mma8451_get_raw_sample(handle, handle->raw_data);
  184.    
  185.     //Convert the 14bit two's complement numbers into 16bit integers that makes some sense
  186.     x = handle->raw_data[0];
  187.     x <<= 8;
  188.     x |= handle->raw_data[1];
  189.     x >>= 2;
  190.  
  191.     y = handle->raw_data[2];
  192.     y <<= 8;
  193.     y |= handle->raw_data[3];
  194.     y >>= 2;
  195.  
  196.     z = handle->raw_data[4];
  197.     z <<= 8;
  198.     z |= handle->raw_data[5];
  199.     z >>= 2;
  200.  
  201.     //Result is in "count" over a number that depend on the range.
  202.     //Computer a float that represent the number of G experienced
  203.     //on each axies and store them in the vector
  204.     vect->x = (float) x / get_divider(handle->range);
  205.     vect->y = (float) y / get_divider(handle->range);
  206.     vect->z = (float) z / get_divider(handle->range);
  207. }
  208.  
  209. mma8451_vector3 mma8451_get_acceleration_vector(mma8451* handle)
  210. {
  211.     mma8451_vector3 vector;
  212.     mma8451_get_acceleration(handle, &vector);
  213.     return vector;
  214. }
  215.  
  216. void mma8451_set_range(mma8451* handle, unsigned char range)
  217. {
  218.     handle->range = range;
  219.     unsigned char XYZ_DATA_CFG = 0, REG1 = 0;
  220.     switch(range)
  221.     {
  222.         default:
  223.             perror("unknown range. use 2, 4 or 8 only!");
  224.         case 2:
  225.             XYZ_DATA_CFG = 0b00;
  226.             break;
  227.         case 4:
  228.             XYZ_DATA_CFG = 0b01;
  229.             break;
  230.         case 8:
  231.             XYZ_DATA_CFG = 0b10;
  232.             break;
  233.     }
  234.  
  235.     REG1 = mma8451_read_byte(handle, 0x2A) | 0x01;
  236.     mma8451_write_byte(handle, 0x2A, 0x00);
  237.     mma8451_write_byte(handle, 0x0E, XYZ_DATA_CFG);
  238.     mma8451_write_byte(handle, 0x2A, REG1);
  239. }