Let L3GD20 driver output values in float, scaled to rad/s.

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);
 		}
 };