Create I2CSensor class and subclasses for each sensor.

1 files changed, 17 insertions, 79 deletions

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