Read and send accelerometer.

2 files changed, 44 insertions, 1 deletions

diff --git a/bma150.h b/bma150.h
new file mode 100644
index 0000000..2e043ff
--- /dev/null
+++ b/bma150.h
@@ -0,0 +1,33 @@
+#ifndef BMA150_H
+#define BMA150_H
+
+#include "i2c.h"
+
+class BMA150 {
+	private:
+		I2C& i2c;
+	
+	public:
+		int16_t x, y, z;
+		
+		BMA150(I2C& i2c_bus) : i2c(i2c_bus) {
+			
+		}
+		
+		void init() {
+			uint8_t temp;
+			i2c.read_reg(0x38, 0x14, 1, &temp);
+			i2c.write_reg(0x38, 0x14, (temp & 0xe0) | 0x00 | 0x00); // 2g range, 25 Hz bandwidth
+		}
+		
+		void update() {
+			uint8_t buf[6];
+			i2c.read_reg(0x38, 0x02, 6, buf);
+			
+			x = (buf[1] << 8 | buf[0]) - 0;
+			y = (buf[3] << 8 | buf[2]) - 0;
+			z = (buf[5] << 8 | buf[4]) - 0;
+		}
+};
+
+#endif
diff --git a/main.cpp b/main.cpp
index 5a0cd4d..199ffcd 100644
--- a/main.cpp
+++ b/main.cpp
@@ -7,6 +7,7 @@
 #include "i2c.h"
 
 #include "itg3200.h"
+#include "bma150.h"
 
 #include "usart.h"
 #include "xbee.h"
@@ -59,6 +60,7 @@ class PID {
 
 I2C i2c;
 ITG3200 gyro(i2c);
+BMA150 accel(i2c);
 
 void threadmain() {
 	while(1) {
@@ -71,9 +73,15 @@ void threadmain() {
 			gyro.y >> 8,
 			gyro.z & 0xff,
 			gyro.z >> 8,
+			accel.x & 0xff,
+			accel.x >> 8,
+			accel.y & 0xff,
+			accel.y >> 8,
+			accel.z & 0xff,
+			accel.z >> 8,
 		};
 		
-		xbee_send(6, buf);
+		xbee_send(sizeof(buf), buf);
 		Time::sleep(100);
 	}
 }
@@ -104,6 +112,7 @@ int main() {
 	
 	//ITG3200 gyro(i2c);
 	gyro.init();
+	accel.init();
 	
 	PPMSum ppmsum;
 	ppmsum.enable();
@@ -134,6 +143,7 @@ int main() {
 		
 		// Read sensors.
 		gyro.update();
+		accel.update();
 		
 		// Update filter.