![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
3Axis gyro sensor sample. target sensor: L3GD20(ST)
L3GD20.cpp@1:191e963655c2, 2018-12-18 (annotated)
- Committer:
- satoshi1204
- Date:
- Tue Dec 18 09:08:07 2018 +0000
- Revision:
- 1:191e963655c2
- Parent:
- 0:a1d1374eea97
3Axis gyro sensor sample
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
satoshi1204 | 0:a1d1374eea97 | 1 | #include "L3GD20.h" |
satoshi1204 | 0:a1d1374eea97 | 2 | |
satoshi1204 | 0:a1d1374eea97 | 3 | L3GD20::L3GD20(PinName sda, PinName scl) : i2c(sda, scl), address(0xD6) |
satoshi1204 | 0:a1d1374eea97 | 4 | { |
satoshi1204 | 1:191e963655c2 | 5 | turnOn(); |
satoshi1204 | 0:a1d1374eea97 | 6 | } |
satoshi1204 | 0:a1d1374eea97 | 7 | |
satoshi1204 | 0:a1d1374eea97 | 8 | L3GD20::~L3GD20() |
satoshi1204 | 0:a1d1374eea97 | 9 | { |
satoshi1204 | 0:a1d1374eea97 | 10 | } |
satoshi1204 | 0:a1d1374eea97 | 11 | |
satoshi1204 | 1:191e963655c2 | 12 | void L3GD20::turnOn(void) |
satoshi1204 | 1:191e963655c2 | 13 | { |
satoshi1204 | 1:191e963655c2 | 14 | writeByte(0x20, 0x0F); // turn on a sensor |
satoshi1204 | 1:191e963655c2 | 15 | } |
satoshi1204 | 1:191e963655c2 | 16 | |
satoshi1204 | 0:a1d1374eea97 | 17 | void L3GD20::readData(double &x, double &y, double &z) |
satoshi1204 | 0:a1d1374eea97 | 18 | { |
satoshi1204 | 0:a1d1374eea97 | 19 | static const double rate = 0.00875; |
satoshi1204 | 0:a1d1374eea97 | 20 | char h, l; |
satoshi1204 | 0:a1d1374eea97 | 21 | l = readByte(0x28); |
satoshi1204 | 0:a1d1374eea97 | 22 | h = readByte(0x29); |
satoshi1204 | 0:a1d1374eea97 | 23 | x = (short)(h << 8 | l) * rate; |
satoshi1204 | 0:a1d1374eea97 | 24 | l = readByte(0x2A); |
satoshi1204 | 0:a1d1374eea97 | 25 | h = readByte(0x2B); |
satoshi1204 | 0:a1d1374eea97 | 26 | y = (short)(h << 8 | l) * rate; |
satoshi1204 | 0:a1d1374eea97 | 27 | l = readByte(0x2C); |
satoshi1204 | 0:a1d1374eea97 | 28 | h = readByte(0x2D); |
satoshi1204 | 0:a1d1374eea97 | 29 | z = (short)(h << 8 | l) * rate; |
satoshi1204 | 0:a1d1374eea97 | 30 | } |
satoshi1204 | 0:a1d1374eea97 | 31 | |
satoshi1204 | 0:a1d1374eea97 | 32 | char L3GD20::readByte(char reg) |
satoshi1204 | 0:a1d1374eea97 | 33 | { |
satoshi1204 | 1:191e963655c2 | 34 | static const int data_len = 1; |
satoshi1204 | 1:191e963655c2 | 35 | i2c.write(address, ®, data_len); |
satoshi1204 | 0:a1d1374eea97 | 36 | char data; |
satoshi1204 | 1:191e963655c2 | 37 | i2c.read(address, &data, data_len); |
satoshi1204 | 0:a1d1374eea97 | 38 | return data; |
satoshi1204 | 0:a1d1374eea97 | 39 | } |
satoshi1204 | 0:a1d1374eea97 | 40 | |
satoshi1204 | 0:a1d1374eea97 | 41 | void L3GD20::writeByte(char reg, char data) |
satoshi1204 | 0:a1d1374eea97 | 42 | { |
satoshi1204 | 1:191e963655c2 | 43 | static const int data_len = 2; |
satoshi1204 | 1:191e963655c2 | 44 | char buf[data_len]; |
satoshi1204 | 0:a1d1374eea97 | 45 | buf[0] = reg; |
satoshi1204 | 0:a1d1374eea97 | 46 | buf[1] = data; |
satoshi1204 | 1:191e963655c2 | 47 | i2c.write(address, buf, data_len); |
satoshi1204 | 0:a1d1374eea97 | 48 | } |