1 files changed, 64 insertions, 0 deletions
diff --git a/source/Core/Drivers/BMA223.cpp b/source/Core/Drivers/BMA223.cpp
new file mode 100644
index 00000000..4da4ae76
--- /dev/null
+++ b/source/Core/Drivers/BMA223.cpp
@@ -0,0 +1,64 @@
+/*
+ * BMA223.cpp
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#include <BMA223.hpp>
+#include <array>
+
+bool BMA223::detect() {
+	if (FRToSI2C::probe(BMA223_ADDRESS)) {
+		//Read chip id to ensure its not an address collision
+		uint8_t id = 0;
+		if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_BGW_CHIPID, &id, 1)) {
+			return id == 0b11111000;
+		}
+	}
+
+	return false;
+}
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { //
+		//
+				{ BMA223_PMU_RANGE, 0b00000011, 0 },    //2G range
+				{ BMA223_PMU_BW, 0b00001101, 0 },    //250Hz filter
+				{ BMA223_PMU_LPW, 0b00000000, 0 },    //Full power
+				{ BMA223_ACCD_HBW, 0b00000000, 0 },    //filtered data out
+				{ BMA223_INT_OUT_CTRL, 0b00001010, 0 },    //interrupt active low and OD to get it hi-z
+				{ BMA223_INT_RST_LATCH, 0b10000000, 0 },    //interrupt active low and OD to get it hi-z
+				{ BMA223_INT_EN_0, 0b01000000, 0 },    //Enable orientation
+				{ BMA223_INT_A, 0b00100111, 0 },    //Setup orientation detection
+
+		//
+		};
+bool BMA223::initalize() {
+	//Setup acceleration readings
+	//2G range
+	//bandwidth = 250Hz
+	//High pass filter on (Slow compensation)
+	//Turn off IRQ output pins
+	//Orientation recognition in symmetrical mode
+	// Hysteresis is set to ~ 16 counts
+	//Theta blocking is set to 0b10
+
+	return FRToSI2C::writeRegistersBulk(BMA223_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+
+}
+
+void BMA223::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	//The BMA is odd in that its output data width is only 8 bits
+	//And yet there are MSB and LSB registers _sigh_.
+	uint8_t sensorData[6] = { 0, 0, 0, 0, 0, 0 };
+
+	if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_ACCD_X_LSB, sensorData, 6) == false) {
+		x = y = z = 0;
+		return;
+	}
+	//Shift 6 to make its range ~= the other accelerometers
+	x = sensorData[1] << 6;
+	y = sensorData[3] << 6;
+	z = sensorData[5] << 6;
+
+}