Diff: hepta_sat/HeptaAccel.cpp

Revision:

0:d721efd58e4e

Child:

1:63c3921c608c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hepta_sat/HeptaAccel.cpp	Fri Dec 09 03:40:15 2016 +0000
@@ -0,0 +1,317 @@
+#include "HeptaAccel.h"
+#include "mbed.h"
+
+HeptaAccel::HeptaAccel(PinName sda, PinName scl, int aaddr ) : accel(sda,scl),addr(aaddr)
+{
+    _cmd[0] = 0x2A;//CTL_REG
+    _cmd[1] = 0x01;//ACTIVE
+    accel.write(addr, _cmd, 2);
+    accel.start();
+    accel.write(addr);
+    accel.stop(); 
+}
+
+void HeptaAccel::setup()
+{
+    _cmd[0] = 0x2A;
+    _cmd[1] = 0x01;
+    accel.frequency(100000);
+    accel.write(addr, _cmd, 2);
+    accel.start();
+    accel.write(addr);
+    accel.stop();
+}
+
+void HeptaAccel::sensing(float *ax,float *ay,float *az)
+{
+    int16_t acc;
+    accel.start();//start
+    accel.write(addr);//write address
+    accel.write(0x01);//X-axis_MSB_REG
+    accel.start();//restart, switch from write to read
+    accel.write(addr|0x01);//read address
+    _xmsb = accel.read(0);//read from selected register with NACK
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x02);//X-axis_LSB_REG
+    accel.start();
+    accel.write(addr|0x01);
+    _xlsb = accel.read(0);
+    accel.stop();
+    acc = (_xmsb << 6) | (_xlsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }
+    *ax = acc/4096.0*9.81;
+    
+    accel.start();
+    accel.write(addr);
+    accel.write(0x03);
+    accel.start();
+    accel.write(addr|0x01);
+    _ymsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x04);
+    accel.start();
+    accel.write(addr|0x01);
+    _ylsb = accel.read(0);
+    accel.stop();
+    acc = (_ymsb << 6) | (_ylsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }
+    *ay = acc/4096.0*9.81;   
+        
+    accel.start();
+    accel.write(addr);
+    accel.write(0x05);
+    accel.start();
+    accel.write(addr|0x01);
+    _zmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x06);
+    accel.start();
+    accel.write(addr|0x01);
+    _zlsb = accel.read(0);
+    accel.stop();
+    acc = (_zmsb << 6) | (_zlsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }   
+    *az = acc/4096.0*9.81;  
+}
+    
+float HeptaAccel::x()
+{  
+    int16_t acc;
+    float ax;
+    accel.start();
+    accel.write(addr);
+    accel.write(0x01);
+    accel.start();
+    accel.write(addr|0x01);
+    _xmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x02);
+    accel.start();
+    accel.write(addr|0x01);
+    _xlsb = accel.read(0);
+    accel.stop();  
+    acc = (_xmsb << 6) | (_xlsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }
+    ax = acc/4096.0*9.81;
+    return(ax);
+}
+    
+float HeptaAccel::y()
+{
+    int16_t acc;
+    float ay;
+    accel.start();
+    accel.write(addr);
+    accel.write(0x03);
+    accel.start();
+    accel.write(addr|0x01);
+    _ymsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x04);
+    accel.start();
+    accel.write(addr|0x01);
+    _ylsb = accel.read(0);
+    accel.stop();
+    acc = (_ymsb << 6) | (_ylsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }   
+    ay = acc/4096.0*9.81;
+    return(ay);
+}
+    
+float HeptaAccel::z()
+{
+    int16_t acc;
+    float az;
+    accel.start();
+    accel.write(addr);
+    accel.write(0x2C);
+    accel.start();
+    accel.write(addr|0x01);
+    _zmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x2D);
+    accel.start();
+    accel.write(addr|0x01);
+    _zlsb = accel.read(0);
+    accel.stop();
+    acc = (_zmsb << 6) | (_zlsb >> 2);
+    if (acc > UINT14_MAX/2)
+    {
+        acc -= UINT14_MAX;
+    }   
+    az = acc/4096.0*9.81;
+    return(az);
+}
+    
+void HeptaAccel::sensing_u16(char* ax_u16,char* ay_u16,char* az_u16, int *dsize)
+{
+    char a1[8]={0x00},a2[8]={0x00};
+    accel.start();
+    accel.write(addr);
+    accel.write(0x01);
+    accel.start();
+    accel.write(addr|0x01);
+    _xmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x02);
+    accel.start();
+    accel.write(addr|0x01);
+    _xlsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_xmsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_xlsb)) & 0xFF);
+    ax_u16[0]=a1[0];
+    ax_u16[1]=a1[1];
+    ax_u16[2]=a2[0];
+    ax_u16[3]=a2[1];
+        
+    accel.start();
+    accel.write(addr);
+    accel.write(0x03);
+    accel.start();
+    accel.write(addr|0x01);
+    _ymsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x04);
+    accel.start();
+    accel.write(addr|0x01);
+    _ylsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_ymsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_ylsb)) & 0xFF);
+    ay_u16[0]=a1[0];
+    ay_u16[1]=a1[1];
+    ay_u16[2]=a2[0];
+    ay_u16[3]=a2[1];
+        
+    accel.start();
+    accel.write(addr);
+    accel.write(0x05);
+    accel.start();
+    accel.write(addr|0x01);
+    _zmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x06);
+    accel.start();
+    accel.write(addr|0x01);
+    _zlsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_zmsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_zlsb)) & 0xFF);
+    az_u16[0]=a1[0];
+    az_u16[1]=a1[1];
+    az_u16[2]=a2[0];
+    az_u16[3]=a2[1];
+    *dsize = 4;
+}
+        
+void HeptaAccel::x_u16(char* a_u16, int *dsize)
+{
+    char a1[8]={0x00},a2[8]={0x00};
+    accel.start();
+    accel.write(addr);
+    accel.write(0x01);
+    accel.start();
+    accel.write(addr|0x01);
+    _xmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x02);
+    accel.start();
+    accel.write(addr|0x01);
+    _xlsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_xmsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_xlsb)) & 0xFF);
+    a_u16[0]=a1[0];
+    a_u16[1]=a1[1];
+    a_u16[2]=a2[0];
+    a_u16[3]=a2[1];
+    *dsize = 4;
+}
+    
+void HeptaAccel::y_u16(char* a_u16, int *dsize)
+{
+    char a1[8]={0x00},a2[8]={0x00};
+    accel.start();
+    accel.write(addr);
+    accel.write(0x03);
+    accel.start();
+    accel.write(addr|0x01);
+    _ymsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x04);
+    accel.start();
+    accel.write(addr|0x01);
+    _ylsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_ymsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_ylsb)) & 0xFF);
+    a_u16[0]=a1[0];
+    a_u16[1]=a1[1];
+    a_u16[2]=a2[0];
+    a_u16[3]=a2[1];
+    *dsize = 4;
+}
+     
+void HeptaAccel::z_u16(char* a_u16, int *dsize)
+{
+    char a1[8]={0x00},a2[8]={0x00};
+    accel.start();
+    accel.write(addr);
+    accel.write(0x05);
+    accel.start();
+    accel.write(addr|0x01);
+    _zmsb = accel.read(0);
+    accel.stop();
+    accel.start();
+    accel.write(addr);
+    accel.write(0x06);
+    accel.start();
+    accel.write(addr|0x01);
+    _zlsb = accel.read(0);
+    accel.stop();  
+    sprintf( a1, "%02X", ((_zmsb)) & 0xFF);
+    sprintf( a2, "%02X", ((_zlsb)) & 0xFF);
+    a_u16[0]=a1[0];
+    a_u16[1]=a1[1];
+    a_u16[2]=a2[0];
+    a_u16[3]=a2[1];
+    *dsize = 4;
+}
\ No newline at end of file