diff options
Diffstat (limited to 'drivers')
| -rw-r--r-- | drivers/l3gd20.h | 28 |
1 files changed, 18 insertions, 10 deletions
diff --git a/drivers/l3gd20.h b/drivers/l3gd20.h index 56dff08..0113792 100644 --- a/drivers/l3gd20.h +++ b/drivers/l3gd20.h @@ -1,6 +1,8 @@ #ifndef L3GD20_H #define L3GD20_H +#include <math.h> + #include <gpio/pin.h> #include <spi/spi.h> @@ -10,29 +12,35 @@ class L3GD20 { SPI_t& spi; public: - int16_t x, y, z; + float x, y, z; L3GD20(Pin& cs_pin, SPI_t& spi_bus) : cs(cs_pin), spi(spi_bus) {} void init() { spi.reg.CR1 = 0x37f; cs.off(); - spi.reg.DR = 0x40 | 0x20; while(!(spi.reg.SR & 0x01)); (void)spi.reg.DR; - spi.reg.DR = 0x0f; while(!(spi.reg.SR & 0x01)); (void)spi.reg.DR; + spi.transfer_byte(0x40 | 0x20); + spi.transfer_byte(0x0f); cs.on(); } void update() { + int16_t raw_x, raw_y, raw_z; + spi.reg.CR1 = 0x37f; cs.off(); - spi.reg.DR = 0xc0 | 0x28; while(!(spi.reg.SR & 0x01)); (void)spi.reg.DR; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); x = spi.reg.DR; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); x |= spi.reg.DR << 8; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); y = spi.reg.DR; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); y |= spi.reg.DR << 8; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); z = spi.reg.DR; - spi.reg.DR = 0; while(!(spi.reg.SR & 0x01)); z |= spi.reg.DR << 8; + spi.transfer_byte(0xc0 | 0x28); + raw_x = spi.transfer_byte(); + raw_x |= spi.transfer_byte() << 8; + raw_y = spi.transfer_byte(); + raw_y |= spi.transfer_byte() << 8; + raw_z = spi.transfer_byte(); + raw_z |= spi.transfer_byte() << 8; cs.on(); + + x = float(raw_x) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0); + y = float(raw_y) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0); + z = float(raw_z) * (2.0 * M_PI * 250.0 / 360.0 / 32768.0); } }; |