Hepta_template_program
Dependencies: mbed PowerControl SDFileSystem
Fork of Hepta_UplinkData by
Diff: hepta_sat/HeptaGyro.cpp
- Revision:
- 0:b96079b7d167
diff -r 000000000000 -r b96079b7d167 hepta_sat/HeptaGyro.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hepta_sat/HeptaGyro.cpp Tue Dec 13 08:03:46 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