summaryrefslogtreecommitdiff
path: root/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'drivers')
-rw-r--r--drivers/l3gd20.h28
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);
}
};