![](/media/cache/profiles/1129cb8ba4665189d5a9bd861a2e28b9.jpg.50x50_q85.jpg)
gyro sensor L3GD20 test
main.cpp@0:24c1e45f1a27, 2017-09-14 (annotated)
- Committer:
- Akito914
- Date:
- Thu Sep 14 09:02:51 2017 +0000
- Revision:
- 0:24c1e45f1a27
- Child:
- 1:635b9b4215b1
??????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Akito914 | 0:24c1e45f1a27 | 1 | #include "mbed.h" |
Akito914 | 0:24c1e45f1a27 | 2 | |
Akito914 | 0:24c1e45f1a27 | 3 | #define I2C_ADDRESS 0xD4 |
Akito914 | 0:24c1e45f1a27 | 4 | |
Akito914 | 0:24c1e45f1a27 | 5 | |
Akito914 | 0:24c1e45f1a27 | 6 | #define ADDR_WHO_AM_I 0x0f |
Akito914 | 0:24c1e45f1a27 | 7 | #define ADDR_CTRL_REG1 0x20 |
Akito914 | 0:24c1e45f1a27 | 8 | #define ADDR_CTRL_REG2 0x21 |
Akito914 | 0:24c1e45f1a27 | 9 | #define ADDR_CTRL_REG3 0x22 |
Akito914 | 0:24c1e45f1a27 | 10 | #define ADDR_CTRL_REG4 0x23 |
Akito914 | 0:24c1e45f1a27 | 11 | #define ADDR_CTRL_REG5 0x24 |
Akito914 | 0:24c1e45f1a27 | 12 | #define ADDR_STATUS_REG 0x27 |
Akito914 | 0:24c1e45f1a27 | 13 | #define ADDR_OUT_X_L 0x28 |
Akito914 | 0:24c1e45f1a27 | 14 | #define ADDR_OUT_X_H 0x29 |
Akito914 | 0:24c1e45f1a27 | 15 | #define ADDR_OUT_Y_L 0x2A |
Akito914 | 0:24c1e45f1a27 | 16 | #define ADDR_OUT_Y_H 0x2B |
Akito914 | 0:24c1e45f1a27 | 17 | #define ADDR_OUT_Z_L 0x2C |
Akito914 | 0:24c1e45f1a27 | 18 | #define ADDR_OUT_Z_H 0x2D |
Akito914 | 0:24c1e45f1a27 | 19 | |
Akito914 | 0:24c1e45f1a27 | 20 | I2C i2c(PB_9, PB_8); |
Akito914 | 0:24c1e45f1a27 | 21 | |
Akito914 | 0:24c1e45f1a27 | 22 | DigitalOut myled(LED1); |
Akito914 | 0:24c1e45f1a27 | 23 | |
Akito914 | 0:24c1e45f1a27 | 24 | Serial pc(USBTX, USBRX); |
Akito914 | 0:24c1e45f1a27 | 25 | |
Akito914 | 0:24c1e45f1a27 | 26 | char getData(char addr){ |
Akito914 | 0:24c1e45f1a27 | 27 | char readData; |
Akito914 | 0:24c1e45f1a27 | 28 | i2c.write(I2C_ADDRESS, &addr, 1, true); |
Akito914 | 0:24c1e45f1a27 | 29 | i2c.read(I2C_ADDRESS | 0x01, &readData, 1); |
Akito914 | 0:24c1e45f1a27 | 30 | return readData; |
Akito914 | 0:24c1e45f1a27 | 31 | } |
Akito914 | 0:24c1e45f1a27 | 32 | |
Akito914 | 0:24c1e45f1a27 | 33 | void setData(char addr, char data){ |
Akito914 | 0:24c1e45f1a27 | 34 | char sendData[] = {addr, data}; |
Akito914 | 0:24c1e45f1a27 | 35 | // i2c.write(I2C_ADDRESS, &addr, 1, true); |
Akito914 | 0:24c1e45f1a27 | 36 | // i2c.write(I2C_ADDRESS, &data, 1); |
Akito914 | 0:24c1e45f1a27 | 37 | i2c.write(I2C_ADDRESS, sendData, 2); |
Akito914 | 0:24c1e45f1a27 | 38 | return; |
Akito914 | 0:24c1e45f1a27 | 39 | } |
Akito914 | 0:24c1e45f1a27 | 40 | |
Akito914 | 0:24c1e45f1a27 | 41 | double get_angular_velocity(char axis){ |
Akito914 | 0:24c1e45f1a27 | 42 | int omega_l, omega_h; |
Akito914 | 0:24c1e45f1a27 | 43 | int16_t omega; |
Akito914 | 0:24c1e45f1a27 | 44 | switch(axis){ |
Akito914 | 0:24c1e45f1a27 | 45 | case 'X': case 'x': |
Akito914 | 0:24c1e45f1a27 | 46 | omega_l = getData(ADDR_OUT_X_L); |
Akito914 | 0:24c1e45f1a27 | 47 | omega_h = getData(ADDR_OUT_X_H); |
Akito914 | 0:24c1e45f1a27 | 48 | break; |
Akito914 | 0:24c1e45f1a27 | 49 | case 'Y': case 'y': |
Akito914 | 0:24c1e45f1a27 | 50 | omega_l = getData(ADDR_OUT_Y_L); |
Akito914 | 0:24c1e45f1a27 | 51 | omega_h = getData(ADDR_OUT_Y_H); |
Akito914 | 0:24c1e45f1a27 | 52 | break; |
Akito914 | 0:24c1e45f1a27 | 53 | case 'Z': case 'z': |
Akito914 | 0:24c1e45f1a27 | 54 | omega_l = getData(ADDR_OUT_Z_L); |
Akito914 | 0:24c1e45f1a27 | 55 | omega_h = getData(ADDR_OUT_Z_H); |
Akito914 | 0:24c1e45f1a27 | 56 | break; |
Akito914 | 0:24c1e45f1a27 | 57 | default : return 0.0; |
Akito914 | 0:24c1e45f1a27 | 58 | } |
Akito914 | 0:24c1e45f1a27 | 59 | omega = omega_h << 8 | omega_l; |
Akito914 | 0:24c1e45f1a27 | 60 | //return omega * 0.00875; // +- 250 dps mode |
Akito914 | 0:24c1e45f1a27 | 61 | return omega * 0.01750; // +- 500 dps mode |
Akito914 | 0:24c1e45f1a27 | 62 | //return omega * 0.070; // +- 2000 dps mode |
Akito914 | 0:24c1e45f1a27 | 63 | |
Akito914 | 0:24c1e45f1a27 | 64 | } |
Akito914 | 0:24c1e45f1a27 | 65 | |
Akito914 | 0:24c1e45f1a27 | 66 | double trapezoid_integr(double data, double ex_data, double period){ |
Akito914 | 0:24c1e45f1a27 | 67 | return (data + ex_data) * period / 2.0; |
Akito914 | 0:24c1e45f1a27 | 68 | } |
Akito914 | 0:24c1e45f1a27 | 69 | |
Akito914 | 0:24c1e45f1a27 | 70 | Timer gyro_timer; |
Akito914 | 0:24c1e45f1a27 | 71 | |
Akito914 | 0:24c1e45f1a27 | 72 | void getAngle(double *result, bool reset){ |
Akito914 | 0:24c1e45f1a27 | 73 | static double angle_list[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 74 | static double ex_omega[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 75 | int read_period_us; |
Akito914 | 0:24c1e45f1a27 | 76 | double omega[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 77 | |
Akito914 | 0:24c1e45f1a27 | 78 | omega[0] = get_angular_velocity('X'); |
Akito914 | 0:24c1e45f1a27 | 79 | omega[1] = get_angular_velocity('Y'); |
Akito914 | 0:24c1e45f1a27 | 80 | omega[2] = get_angular_velocity('Z'); |
Akito914 | 0:24c1e45f1a27 | 81 | |
Akito914 | 0:24c1e45f1a27 | 82 | read_period_us = gyro_timer.read_us(); |
Akito914 | 0:24c1e45f1a27 | 83 | gyro_timer.reset(); |
Akito914 | 0:24c1e45f1a27 | 84 | |
Akito914 | 0:24c1e45f1a27 | 85 | for(int axis = 0; axis < 3; axis++){ |
Akito914 | 0:24c1e45f1a27 | 86 | angle_list[axis] += trapezoid_integr(omega[axis], ex_omega[axis], read_period_us / 1000000.0); |
Akito914 | 0:24c1e45f1a27 | 87 | ex_omega[axis] = omega[axis]; |
Akito914 | 0:24c1e45f1a27 | 88 | if(reset)angle_list[axis] = 0; |
Akito914 | 0:24c1e45f1a27 | 89 | result[axis] = angle_list[axis]; |
Akito914 | 0:24c1e45f1a27 | 90 | } |
Akito914 | 0:24c1e45f1a27 | 91 | return; |
Akito914 | 0:24c1e45f1a27 | 92 | } |
Akito914 | 0:24c1e45f1a27 | 93 | |
Akito914 | 0:24c1e45f1a27 | 94 | int main(){ |
Akito914 | 0:24c1e45f1a27 | 95 | double angle[3] = {0, 0, 0}; |
Akito914 | 0:24c1e45f1a27 | 96 | |
Akito914 | 0:24c1e45f1a27 | 97 | i2c.frequency(400000); |
Akito914 | 0:24c1e45f1a27 | 98 | |
Akito914 | 0:24c1e45f1a27 | 99 | pc.baud(115200); |
Akito914 | 0:24c1e45f1a27 | 100 | |
Akito914 | 0:24c1e45f1a27 | 101 | pc.printf("Hello\r\n"); |
Akito914 | 0:24c1e45f1a27 | 102 | |
Akito914 | 0:24c1e45f1a27 | 103 | pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I)); |
Akito914 | 0:24c1e45f1a27 | 104 | |
Akito914 | 0:24c1e45f1a27 | 105 | setData(ADDR_CTRL_REG1, 0x0f); |
Akito914 | 0:24c1e45f1a27 | 106 | |
Akito914 | 0:24c1e45f1a27 | 107 | //setData(ADDR_CTRL_REG4, 0b00000000); // +- 250 dps mode |
Akito914 | 0:24c1e45f1a27 | 108 | setData(ADDR_CTRL_REG4, 0b00010000); // +- 500 dps mode |
Akito914 | 0:24c1e45f1a27 | 109 | //setData(ADDR_CTRL_REG4, 0b00100000); // +- 2000 dps mode |
Akito914 | 0:24c1e45f1a27 | 110 | |
Akito914 | 0:24c1e45f1a27 | 111 | gyro_timer.start(); |
Akito914 | 0:24c1e45f1a27 | 112 | |
Akito914 | 0:24c1e45f1a27 | 113 | while(1){ |
Akito914 | 0:24c1e45f1a27 | 114 | |
Akito914 | 0:24c1e45f1a27 | 115 | wait_ms(1); |
Akito914 | 0:24c1e45f1a27 | 116 | |
Akito914 | 0:24c1e45f1a27 | 117 | //pc.printf("whoAmI = 0x%.2x\r\n", getData(ADDR_WHO_AM_I)); |
Akito914 | 0:24c1e45f1a27 | 118 | |
Akito914 | 0:24c1e45f1a27 | 119 | //pc.printf("status = 0x%.2x\r\n", getData(ADDR_STATUS_REG)); |
Akito914 | 0:24c1e45f1a27 | 120 | |
Akito914 | 0:24c1e45f1a27 | 121 | getAngle(angle, false); |
Akito914 | 0:24c1e45f1a27 | 122 | pc.printf("(%f, %f, %f)\r\n", angle[0], angle[1], angle[2]); |
Akito914 | 0:24c1e45f1a27 | 123 | |
Akito914 | 0:24c1e45f1a27 | 124 | //pc.printf("(%f, %f, %f)\r\n", get_angular_velocity('X'), get_angular_velocity('Y'), get_angular_velocity('Z')); |
Akito914 | 0:24c1e45f1a27 | 125 | |
Akito914 | 0:24c1e45f1a27 | 126 | |
Akito914 | 0:24c1e45f1a27 | 127 | //get_angular_velocity_Z(); |
Akito914 | 0:24c1e45f1a27 | 128 | |
Akito914 | 0:24c1e45f1a27 | 129 | |
Akito914 | 0:24c1e45f1a27 | 130 | //pc.printf("CTRL_REG1 = 0x%.2x\r\n", getData(ADDR_CTRL_REG1)); |
Akito914 | 0:24c1e45f1a27 | 131 | //pc.printf("CTRL_REG2 = 0x%.2x\r\n", getData(ADDR_CTRL_REG2)); |
Akito914 | 0:24c1e45f1a27 | 132 | //pc.printf("CTRL_REG3 = 0x%.2x\r\n", getData(ADDR_CTRL_REG3)); |
Akito914 | 0:24c1e45f1a27 | 133 | //pc.printf("CTRL_REG4 = 0x%.2x\r\n", getData(ADDR_CTRL_REG4)); |
Akito914 | 0:24c1e45f1a27 | 134 | //pc.printf("CTRL_REG5 = 0x%.2x\r\n", getData(ADDR_CTRL_REG5)); |
Akito914 | 0:24c1e45f1a27 | 135 | |
Akito914 | 0:24c1e45f1a27 | 136 | } |
Akito914 | 0:24c1e45f1a27 | 137 | } |
Akito914 | 0:24c1e45f1a27 | 138 | |
Akito914 | 0:24c1e45f1a27 | 139 | |
Akito914 | 0:24c1e45f1a27 | 140 |