Added L3GD20 driver.

1 files changed, 40 insertions, 0 deletions

diff --git a/drivers/l3gd20.h b/drivers/l3gd20.h
new file mode 100644
index 0000000..c7e8935
--- /dev/null
+++ b/drivers/l3gd20.h
@@ -0,0 +1,40 @@
+#ifndef L3GD20_H
+#define L3GD20_H
+
+#include "pin.h"
+#include "spi.h"
+
+class L3GD20 {
+	private:
+		Pin& cs;
+		SPI_t& spi;
+	
+	public:
+		int16_t 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;
+			cs.on();
+		}
+		
+		void update() {
+			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;
+			cs.on();
+		}
+};
+
+
+#endif