Reading ITG-3200 to USB.

7 files changed, 212 insertions, 5 deletions

diff --git a/board.h b/board.h
index 06f9c24..c0b01d0 100644
--- a/board.h
+++ b/board.h
@@ -48,7 +48,7 @@
  * PB15 - Alternate output  (MMC SPI2 MOSI).

  */

 #define VAL_GPIOBCRL            0x88888888      /*  PB7...PB0 */

-#define VAL_GPIOBCRH            0xB4B88888      /* PB15...PB8 */

+#define VAL_GPIOBCRH            0xB4B84488      /* PB15...PB8 */

 #define VAL_GPIOBODR            0xFFFFFFFF

 

 /*

diff --git a/chibios b/chibios
-Subproject 2aa3090be4b5ff0d74a76c2795ce3af152859ef
+Subproject 0537de03cb144584727f982eb210262b0ddd4d6
diff --git a/foo.h b/foo.h
new file mode 100644
index 0000000..d6bb028
--- /dev/null
+++ b/foo.h
@@ -0,0 +1,84 @@
+int usbprintf(USBSerial& usbs, const char* format, ...) {
+	int32_t* argp = (int32_t*)&format;
+	int num;
+	
+	while(*format) {
+		if(*format != '%') {
+			usbs.putc(*format++);
+			num++;
+			continue;
+		}
+		format++;
+		
+		bool is_signed = false;
+		bool zero_pad = false;
+		int radix = 16;
+		int min_len = 0;
+		
+		if(*format == '0') {
+			zero_pad = true;
+		}
+		
+		while(*format >= '0' && *format <= '9') {
+			min_len = min_len * 10 + *format++ - '0';
+		}
+		
+		switch(*format++) {
+			case '%':
+				usbs.putc('%');
+				num++;
+				break;
+			
+			case 'c':
+				usbs.putc((char)*++argp);
+				num++;
+				break;
+			
+			case 's': {
+				char* str = (char*)*++argp;
+				while(*str) {
+					usbs.putc(*str++);
+					num++;
+				}
+			} break;
+			
+			case 'd':
+				is_signed = true;
+			case 'u':
+				radix = 10;
+			case 'x':
+				;
+				uint32_t x = (uint32_t)*(++argp);
+				if(is_signed && (int32_t)x < 0) {
+					x = -x;
+					usbs.putc('-');
+					min_len--;
+				}
+				
+				char buf[11];
+				char* bufp = &buf[sizeof(buf)];
+				*--bufp = 0;
+				
+				do {
+					int d = x % radix;
+					*--bufp = d < 10 ? '0' + d : 'a' - 10 + d;
+					x /= radix;
+					min_len--;
+				} while(x);
+				
+				while(min_len > 0) {
+					usbs.putc(zero_pad ? '0' : ' ');
+					num++;
+					min_len--;
+				}
+				
+				while(*bufp) {
+					usbs.putc(*bufp++);
+					num++;
+				}
+				break;
+		}
+	}
+	
+	return num;
+}
diff --git a/halconf.h b/halconf.h
index 3860b73..d10e1fe 100644
--- a/halconf.h
+++ b/halconf.h
@@ -66,7 +66,7 @@
  * @brief   Enables the I2C subsystem.

  */

 #if !defined(HAL_USE_I2C) || defined(__DOXYGEN__)

-#define HAL_USE_I2C                 FALSE

+#define HAL_USE_I2C                 TRUE

 #endif

 

 /**

diff --git a/main.cpp b/main.cpp
index 18c2ab6..88d8693 100644
--- a/main.cpp
+++ b/main.cpp
@@ -4,6 +4,7 @@
 #include <ch.h>

 #include <hal.h>

 #include <pwm.h>

+#include <i2c.h>

 

 class LEDThread : public BaseThread<LEDThread, 128> {

 	public:

@@ -90,10 +91,116 @@ static PWMConfig pwmcfg = {
 

 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:

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

+			

+			i2c_gyro.txbytes = 2;

+			i2c_gyro.txbuf[0] = 0x16;

+			i2c_gyro.txbuf[1] = 0x18;

+			i2cMasterTransmit(&I2CD2, &i2c_gyro);

+			chThdSleepMilliseconds(2);

+			

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

+				

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

+				

+				chThdSleepMilliseconds(100);

+			}

+		}

+};

+

+I2CThread i2c_thread;

+

 int main(void) {

 	halInit();

 	chSysInit();

 	

+	led_thread.start();

+	

+	usbs.init();

+	

+	i2c_thread.start();

+	

+	/*

 	pwmStart(&PWMD2, &pwmcfg);

 	palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL);

 	palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);

@@ -105,11 +212,9 @@ int main(void) {
 	pwmEnableChannel(&PWMD2, 2, 1000);

 	pwmEnableChannel(&PWMD2, 3, 1000);

 	

-	led_thread.start();

-	

-	usbs.init();

 	usb_thread.usbs = &usbs;

 	usb_thread.start();

+	*/

 	

 	while (1) {

 		chThdSleepMilliseconds(1000);

diff --git a/mcuconf.h b/mcuconf.h
index 960c09a..b25409a 100644
--- a/mcuconf.h
+++ b/mcuconf.h
@@ -141,3 +141,15 @@
 #define STM32_USB_LOW_POWER_ON_SUSPEND      FALSE

 #define STM32_USB_USB1_HP_IRQ_PRIORITY      6

 #define STM32_USB_USB1_LP_IRQ_PRIORITY      14

+

+/*

+ * I2C driver system settings.

+ */

+#define STM32_I2C_USE_I2C1                  TRUE

+#define STM32_I2C_USE_I2C2                  TRUE

+#define STM32_I2C_I2C1_IRQ_PRIORITY         11

+#define STM32_I2C_I2C2_IRQ_PRIORITY         11

+#define STM32_I2C_I2C1_DMA_PRIORITY         4

+#define STM32_I2C_I2C2_DMA_PRIORITY         4

+#define STM32_I2C_I2C1_DMA_ERROR_HOOK()     chSysHalt()

+#define STM32_I2C_I2C2_DMA_ERROR_HOOK()     chSysHalt()

diff --git a/usbserial.h b/usbserial.h
index 4eaed81..c312821 100644
--- a/usbserial.h
+++ b/usbserial.h
@@ -21,10 +21,16 @@ class USBSerial {
 		}
 		
 		inline msg_t putc(uint8_t data, systime_t timeout = TIME_INFINITE) {
+			if(USBD1.state != USB_ACTIVE) {
+				return 0;
+			}
 			return chOQPutTimeout(&susbdriver.oqueue, data, timeout);
 		}
 		
 		inline size_t write(uint8_t* buf, size_t len, systime_t timeout = TIME_INFINITE) {
+			if(USBD1.state != USB_ACTIVE) {
+				return 0;
+			}
 			return chOQWriteTimeout(&susbdriver.oqueue, buf, len, timeout);
 		}
 };