Akito
/
L3GD20_test
gyro sensor L3GD20 test
Revision 1:635b9b4215b1, committed 2017-09-14
- Comitter:
- Akito914
- Date:
- Thu Sep 14 11:48:38 2017 +0000
- Parent:
- 0:24c1e45f1a27
- Commit message:
- ???????
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Sep 14 09:02:51 2017 +0000 +++ b/main.cpp Thu Sep 14 11:48:38 2017 +0000 @@ -9,6 +9,7 @@ #define ADDR_CTRL_REG3 0x22 #define ADDR_CTRL_REG4 0x23 #define ADDR_CTRL_REG5 0x24 +#define ADDR_OUT_TEMP 0x26 #define ADDR_STATUS_REG 0x27 #define ADDR_OUT_X_L 0x28 #define ADDR_OUT_X_H 0x29 @@ -23,6 +24,8 @@ Serial pc(USBTX, USBRX); +double omega_offset[3] = {0, 0, 0}; + char getData(char addr){ char readData; i2c.write(I2C_ADDRESS, &addr, 1, true); @@ -57,8 +60,8 @@ default : return 0.0; } omega = omega_h << 8 | omega_l; - //return omega * 0.00875; // +- 250 dps mode - return omega * 0.01750; // +- 500 dps mode + return omega * 0.00875; // +- 250 dps mode + //return omega * 0.01750; // +- 500 dps mode //return omega * 0.070; // +- 2000 dps mode } @@ -75,9 +78,9 @@ int read_period_us; double omega[3] = {0, 0, 0}; - omega[0] = get_angular_velocity('X'); - omega[1] = get_angular_velocity('Y'); - omega[2] = get_angular_velocity('Z'); + omega[0] = get_angular_velocity('X') - omega_offset[0]; + omega[1] = get_angular_velocity('Y') - omega_offset[1]; + omega[2] = get_angular_velocity('Z') - omega_offset[2]; read_period_us = gyro_timer.read_us(); gyro_timer.reset(); @@ -91,8 +94,14 @@ return; } +int getTemp(){ + return (int)getData(ADDR_OUT_TEMP) * -1 + 40; // 40 : Set manually +} + + int main(){ double angle[3] = {0, 0, 0}; + double accum[3] = {0, 0, 0}; i2c.frequency(400000); @@ -104,12 +113,26 @@ setData(ADDR_CTRL_REG1, 0x0f); - //setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode - setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode + setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode + //setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode gyro_timer.start(); + for(int count = 0; count < 1000; count++){ + accum[0] += get_angular_velocity('X'); + accum[1] += get_angular_velocity('Y'); + accum[2] += get_angular_velocity('Z'); + wait_ms(2); + } + + for(int axis = 0; axis < 3; axis++){ + omega_offset[axis] = accum[axis] / 1000.0; + } + + pc.printf("accum(%f, %f, %f)\r\n", accum[0], accum[1], accum[2]); + pc.printf("offset(%f, %f, %f)\r\n", omega_offset[0], omega_offset[1], omega_offset[2]); + while(1){ wait_ms(1); @@ -121,6 +144,8 @@ getAngle(angle, false); pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]); + pc.printf("temp = %d ℃\r\n", getTemp()); + //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z'));