Diff: hepta_sat/HeptaGyro.cpp

Revision:

0:d53e9c6fc771

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/hepta_sat/HeptaGyro.cpp	Fri Dec 09 04:16:34 2016 +0000
@@ -0,0 +1,279 @@
+#include "HeptaGyro.h"
+#include "mbed.h"
+
+HeptaGyro::HeptaGyro(PinName sda, PinName scl, int aaddr ) : gyro(sda,scl),addr(aaddr)
+{
+    _cmd[0] = 0x20;
+    _cmd[1] = 0x0F;
+    gyro.frequency(100000);
+    gyro.write(addr, _cmd, 2);
+    gyro.start();
+    gyro.write(addr);
+    gyro.stop(); 
+}
+
+void HeptaGyro::setup()
+{
+    _cmd[0] = 0x20;
+    _cmd[1] = 0x0F;
+    gyro.frequency(100000);
+    gyro.write(addr, _cmd, 2);
+    gyro.start();
+    gyro.write(addr);
+    gyro.stop();
+}
+    
+void HeptaGyro::sensing(float *gx,float *gy,float *gz)
+{
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x28);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x29);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xh = gyro.read(0);
+    gyro.stop(); 
+    *gx = (short((_xh<<8)|_xl)*0.00875);
+        
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2A);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2B);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yh = gyro.read(0);
+    gyro.stop();
+    *gy = (short((_yh<<8)|_yl)*0.00875);
+        
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2C);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2D);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zh = gyro.read(0);
+    gyro.stop();
+    *gz = (short((_zh<<8)|_zl)*0.00875);  
+}
+    
+float HeptaGyro::x()
+{
+       
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x28);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x29);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xh = gyro.read(0);
+    gyro.stop();  
+    return(short((_xh<<8)|_xl)*0.00875);
+}
+    
+float HeptaGyro::y()
+{
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2A);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2B);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yh = gyro.read(0);
+    gyro.stop();
+    return(short((_yh<<8)|_yl)*0.00875);
+}
+    
+float HeptaGyro::z()
+{
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2C);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2D);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zh = gyro.read(0);
+    gyro.stop();
+    return(short((_zh<<8)|_zl)*0.00875);
+}
+    
+void HeptaGyro::sensing_u16(char* gx_u16,char* gy_u16,char* gz_u16, int *dsize)
+{
+    char g1[8]={0x00},g2[8]={0x00};
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x28);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x29);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_xh)) & 0xFF);
+    sprintf( g2, "%02X", ((_xl)) & 0xFF);
+    gx_u16[0]=g1[0];
+    gx_u16[1]=g1[1];
+    gx_u16[2]=g2[0];
+    gx_u16[3]=g2[1];
+        
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2A);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2B);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_yh)) & 0xFF);
+    sprintf( g2, "%02X", ((_yl)) & 0xFF);
+    gy_u16[0]=g1[0];
+    gy_u16[1]=g1[1];
+    gy_u16[2]=g2[0];
+    gy_u16[3]=g2[1];
+        
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2C);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2D);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_zh)) & 0xFF);
+    sprintf( g2, "%02X", ((_zl)) & 0xFF);
+    gz_u16[0]=g1[0];
+    gz_u16[1]=g1[1];
+    gz_u16[2]=g2[0];
+    gz_u16[3]=g2[1];
+    *dsize = 4;
+}
+        
+void HeptaGyro::x_u16(char* g_u16, int *dsize)
+{
+    char g1[8]={0x00},g2[8]={0x00};
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x28);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x29);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _xh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_xh)) & 0xFF);
+    sprintf( g2, "%02X", ((_xl)) & 0xFF);
+    g_u16[0]=g1[0];
+    g_u16[1]=g1[1];
+    g_u16[2]=g2[0];
+    g_u16[3]=g2[1];
+    *dsize = 4;
+}
+    
+void HeptaGyro::y_u16(char* g_u16, int *dsize)
+{
+    char g1[8]={0x00},g2[8]={0x00};
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2A);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2B);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _yh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_yh)) & 0xFF);
+    sprintf( g2, "%02X", ((_yl)) & 0xFF);
+    g_u16[0]=g1[0];
+    g_u16[1]=g1[1];
+    g_u16[2]=g2[0];
+    g_u16[3]=g2[1];
+    *dsize = 4;
+}
+     
+void HeptaGyro::z_u16(char* g_u16, int *dsize)
+{
+    char g1[8]={0x00},g2[8]={0x00};
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2C);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zl = gyro.read(0);
+    gyro.stop();
+    gyro.start();
+    gyro.write(addr);
+    gyro.write(0x2D);
+    gyro.start();
+    gyro.write(addr|0x01);
+    _zh = gyro.read(0);
+    gyro.stop();  
+    sprintf( g1, "%02X", ((_zh)) & 0xFF);
+    sprintf( g2, "%02X", ((_zl)) & 0xFF);
+    g_u16[0]=g1[0];
+    g_u16[1]=g1[1];
+    g_u16[2]=g2[0];
+    g_u16[3]=g2[1];
+    *dsize = 4;
+}
\ No newline at end of file