1 files changed, 77 insertions, 0 deletions
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);
+}