From 72d033de0fb016128144cd889e4849e784daa83b Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Wed, 20 Feb 2013 21:10:57 +0100 Subject: Let L3GD20 driver output values in float, scaled to rad/s. --- drivers/l3gd20.h | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) (limited to 'drivers') 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 + #include #include @@ -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); } }; -- cgit v1.2.3