File content as of revision 1:166ddf929155:

#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;
}