Create I2CSensor class and subclasses for each sensor.

6 files changed, 192 insertions, 79 deletions

diff --git a/ak8975.h b/ak8975.h
new file mode 100644
index 0000000..beb9940
--- /dev/null
+++ b/ak8975.h
@@ -0,0 +1,24 @@
+#ifndef AK8975_H
+#define AK8975_H
+
+#include "i2csensor.h"
+
+class AK8975 : public I2CSensor {
+	public:
+		int16_t x, y, z;
+		
+		void init() {
+			i2c_address = 0x0c;
+			write(0x0a, 0x01); // Start first measurement.
+		}
+		
+		void update() {
+			read(0x03, 6);
+			x = (rxdata[0] | rxdata[1] << 8);
+			y = (rxdata[2] | rxdata[3] << 8);
+			z = (rxdata[4] | rxdata[5] << 8);
+			write(0x0a, 0x01); // Start a new measurement.
+		}
+};
+
+#endif
diff --git a/bma150.h b/bma150.h
new file mode 100644
index 0000000..0cd6b27
--- /dev/null
+++ b/bma150.h
@@ -0,0 +1,24 @@
+#ifndef BMA150_H
+#define BMA150_H
+
+#include "i2csensor.h"
+
+class BMA150 : public I2CSensor {
+	public:
+		int16_t x, y, z;
+		
+		void init() {
+			i2c_address = 0x38;
+			read(0x14, 1);
+			write(0x14, (rxdata[0] & 0xe0) | 0x00 | 0x00); // 2g range, 25 Hz bandwidth
+		}
+		
+		void update() {
+			read(0x02, 6);
+			x = ((rxdata[0] & 0xc0) | rxdata[1] << 8);
+			y = ((rxdata[2] & 0xc0) | rxdata[3] << 8);
+			z = ((rxdata[4] & 0xc0) | rxdata[5] << 8);
+		}
+};
+
+#endif
diff --git a/i2csensor.cpp b/i2csensor.cpp
new file mode 100644
index 0000000..f9d8d11
--- /dev/null
+++ b/i2csensor.cpp
@@ -0,0 +1,77 @@
+#include "i2csensor.h"
+
+namespace {
+	void callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
+		if(i2cp->id_slave_config->restart) {
+			i2cp->id_slave_config->restart = FALSE;
+			i2cMasterReceive(i2cp, i2cscfg);
+		} else {
+			i2cMasterStop(i2cp);
+		}
+	}
+	
+	void err_callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {
+		
+	}
+	
+	I2CConfig i2c_conf = {
+		opmodeI2C,
+		100000,
+		stdDutyCycle,
+		0,
+		0
+	};
+};
+
+uint8_t I2CSensor::rxdata[];
+uint8_t I2CSensor::txdata[];
+
+I2CSlaveConfig I2CSensor::i2c_slave_conf = {
+	callback, err_callback,
+	
+	rxdata, sizeof(rxdata), 0, 0,
+	txdata, sizeof(txdata), 0, 0,
+	
+	0,
+	FALSE,
+};
+
+void I2CSensor::enable_bus() {
+	palSetPadMode(GPIOB, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
+	palSetPadMode(GPIOB, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
+	
+	i2cStart(&I2CD2, &i2c_conf);
+	
+	while(I2CD2.id_state != I2C_READY) {
+		chThdSleepMilliseconds(1);
+	}
+}
+
+#include "usbserial.h"
+extern USBSerial usbs;
+int usbprintf(USBSerial& usbs, const char* format, ...);
+
+void I2CSensor::write(uint8_t reg, uint8_t value) {
+	I2CD2.id_state = I2C_READY;
+	i2c_slave_conf.address = i2c_address;
+	i2c_slave_conf.txbytes = 2;
+	i2c_slave_conf.txbuf[0] = reg;
+	i2c_slave_conf.txbuf[1] = value;
+	
+	i2cMasterTransmit(&I2CD2, &i2c_slave_conf);
+	chThdSleepMilliseconds(2);
+}
+
+void I2CSensor::read(uint8_t reg, size_t len) {
+	I2CD2.id_state = I2C_READY;
+	i2c_slave_conf.address = i2c_address;
+	i2c_slave_conf.txbufhead = 0;
+	i2c_slave_conf.txbuf[0] = reg;
+	i2c_slave_conf.txbytes = 1;
+	i2c_slave_conf.rxbufhead = 0;
+	i2c_slave_conf.rxbytes = len;
+	i2c_slave_conf.restart = TRUE;
+	
+	i2cMasterTransmit(&I2CD2, &i2c_slave_conf);
+	chThdSleepMilliseconds(2);
+}
diff --git a/i2csensor.h b/i2csensor.h
new file mode 100644
index 0000000..1878e16
--- /dev/null
+++ b/i2csensor.h
@@ -0,0 +1,26 @@
+#ifndef I2CSENSOR_H
+#define I2CSENSOR_H
+
+#include <stdint.h>
+
+#include <ch.h>
+#include <hal.h>
+
+class I2CSensor {
+	private:
+		static I2CSlaveConfig i2c_slave_conf;
+	
+	public:
+		static void enable_bus();
+	
+	protected:
+		uint8_t i2c_address;
+		
+		void write(uint8_t reg, uint8_t value);
+		void read(uint8_t reg, size_t len);
+		
+		static uint8_t rxdata[8];
+		static uint8_t txdata[2];
+};
+
+#endif
diff --git a/itg3200.h b/itg3200.h
new file mode 100644
index 0000000..f03b6f3
--- /dev/null
+++ b/itg3200.h
@@ -0,0 +1,24 @@
+#ifndef ITG3200_H
+#define ITG3200_H
+
+#include "i2csensor.h"
+
+class ITG3200 : public I2CSensor {
+	public:
+		int16_t x, y, z;
+		
+		void init() {
+			i2c_address = 0x68;
+			write(0x3e, 0x03); // Select clock reference.
+			write(0x16, 0x18 | 0x05); // 2000 deg/sec range, 10 Hz bandwidth.
+		}
+		
+		void update() {
+			read(0x1d, 6);
+			x = (rxdata[0] << 8 | rxdata[1]) - -50;
+			y = (rxdata[2] << 8 | rxdata[3]) - 36;
+			z = (rxdata[4] << 8 | rxdata[5]) - 2;
+		}
+};
+
+#endif
diff --git a/main.cpp b/main.cpp
index 62f7494..911e760 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1,10 +1,11 @@
 #include "thread.h"

 #include "usbserial.h"

+#include "itg3200.h"

+#include "bma150.h"

+#include "ak8975.h"

 

 #include <ch.h>

 #include <hal.h>

-#include <pwm.h>

-#include <i2c.h>

 

 class LEDThread : public BaseThread<LEDThread, 128> {

 	public:

@@ -59,95 +60,32 @@ class USBThread : public BaseThread<USBThread, 256> {
 USBThread usb_thread;

 USBSerial usbs;

 

-I2CConfig i2cconfig = {

-	opmodeI2C,

-	100000,       /*!< Specifies the clock frequency. Must be set to a value lower than 400kHz */

-	stdDutyCycle, /*!< Specifies the I2C fast mode duty cycle */

-	0,      /*!< Specifies the first device 7-bit own address. */

-	0     /*!< Specifies the second part of device own address in 10-bit mode. Set to NULL if not used. */

-};

-

-void cba(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {

-	if(i2cp->id_slave_config->restart) {

-		i2cp->id_slave_config->restart = FALSE;

-		i2cMasterReceive(i2cp, i2cscfg);

-		//palSetPad(GPIOA, 5);

-	} else {

-		i2cMasterStop(i2cp);

-	}

-}

-

-void cbe(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) {

-	//palClearPad(GPIOA, 5);

-	//while(1);

-}

-

-i2cblock_t rxdata[6];

-i2cblock_t txdata[2] = {0x1d};

-

-I2CSlaveConfig i2c_gyro = {

-	cba,

-	

-	cbe,

-	

-	rxdata,

-	6,

-	6,

-	0,

-	

-	txdata,

-	2,

-	1,

-	0,

-	

-	0x68,

-	FALSE

-};

-

-uint8_t strbuf[64];

-

 #include "foo.h"

 

 class I2CThread : public BaseThread<I2CThread, 256> {

 	public:

+		ITG3200 gyro;

+		BMA150 acc;

+		AK8975 magn;

+		

 		int16_t x, y, z;

 		

 		noreturn_t thread_main() {

-			i2cStart(&I2CD2, &i2cconfig);

-			while(I2CD2.id_state != I2C_READY) {

-				chThdSleepMilliseconds(1);

-			}

-			

-			palSetPadMode(GPIOB, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);

-			palSetPadMode(GPIOB, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);

-			

-			i2c_gyro.txbytes = 2;

-			i2c_gyro.txbuf[0] = 0x3e;

-			i2c_gyro.txbuf[1] = 0x03;

-			i2cMasterTransmit(&I2CD2, &i2c_gyro);

-			chThdSleepMilliseconds(2);

+			I2CSensor::enable_bus();

 			

-			i2c_gyro.txbytes = 2;

-			i2c_gyro.txbuf[0] = 0x16;

-			i2c_gyro.txbuf[1] = 0x18;

-			i2cMasterTransmit(&I2CD2, &i2c_gyro);

-			chThdSleepMilliseconds(2);

+			gyro.init();

+			acc.init();

+			magn.init();

 			

 			while (1) {

-				I2CD2.id_state = I2C_READY;

-				i2c_gyro.txbufhead = 0;

-				i2c_gyro.txbuf[0] = 0x1d;

-				i2c_gyro.txbytes = 1;

-				i2c_gyro.rxbufhead = 0;

-				i2c_gyro.rxbytes = 6;

-				i2c_gyro.restart = TRUE;

+				gyro.update();

+				acc.update();

+				magn.update();

+				x = gyro.x;

+				y = gyro.y;

+				z = gyro.z;

 				

-				i2cMasterTransmit(&I2CD2, &i2c_gyro);

-				chThdSleepMilliseconds(2);

 				

-				x = rxdata[0] << 8 | rxdata[1];

-				y = rxdata[2] << 8 | rxdata[3];

-				z = rxdata[4] << 8 | rxdata[5];

 				

 				usbprintf(usbs, "%6d, %6d, %6d\r\n", x, y, z);