From 490eeeb9d984445188f7541c10fa2b3328d2220a Mon Sep 17 00:00:00 2001
From: Kevin Lopez Alvarez <kevin.lopez.alvarez@drotek.com>
Date: Wed, 9 Jan 2019 14:57:19 +0100
Subject: [PATCH] RM3100: enable I2C (#11038)

---
 src/drivers/magnetometer/rm3100/rm3100.h      |  3 ++-
 .../magnetometer/rm3100/rm3100_i2c.cpp        | 21 +++++++++++++------
 .../magnetometer/rm3100/rm3100_main.cpp       |  2 +-
 3 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h
index d31239c217..23aa3d5d51 100644
--- a/src/drivers/magnetometer/rm3100/rm3100.h
+++ b/src/drivers/magnetometer/rm3100/rm3100.h
@@ -59,7 +59,8 @@
  * RM3100 internal constants and data structures.
  */
 
-#define RM3100_CONVERSION_INTERVAL	6850	// Microseconds, corresponds to 146 Hz (cycle count 200 on 3 axis)
+/* At 146 Hz we encounter errors, 100 Hz is safer */
+#define RM3100_CONVERSION_INTERVAL	10000	// Microseconds, corresponds to 100 Hz (cycle count 200 on 3 axis)
 #define UTESLA_TO_GAUSS			100.0f
 #define RM3100_SENSITIVITY		75.0f
 
diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
index 900010db2a..98469bc5c4 100644
--- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
+++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp
@@ -61,9 +61,6 @@
 
 #define RM3100_ADDRESS		0x20
 
-#define DIR_READ		(1<<7)
-#define DIR_WRITE		(0<<7)
-
 class RM3100_I2C : public device::I2C
 {
 public:
@@ -143,8 +140,20 @@ RM3100_I2C::probe()
 int
 RM3100_I2C::read(unsigned address, void *data, unsigned count)
 {
-	uint8_t cmd = address | DIR_READ;
-	return transfer(&cmd, 1, (uint8_t *)data, count);
+	uint8_t cmd = address;
+	int ret;
+
+	/* We need a first transfer where we write the register to read */
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (ret != OK) {
+		return ret;
+	}
+
+	/* Now we read the previously selected register */
+	ret = transfer(nullptr, 0, (uint8_t *)data, count);
+
+	return ret;
 }
 
 int
@@ -156,7 +165,7 @@ RM3100_I2C::write(unsigned address, void *data, unsigned count)
 		return -EIO;
 	}
 
-	buf[0] = address | DIR_WRITE;
+	buf[0] = address;
 	memcpy(&buf[1], data, count);
 
 	return transfer(&buf[0], count + 1, nullptr, 0);
diff --git a/src/drivers/magnetometer/rm3100/rm3100_main.cpp b/src/drivers/magnetometer/rm3100/rm3100_main.cpp
index ddeabbadaf..bdfbe63b3b 100644
--- a/src/drivers/magnetometer/rm3100/rm3100_main.cpp
+++ b/src/drivers/magnetometer/rm3100/rm3100_main.cpp
@@ -73,7 +73,7 @@ rm3100::init(RM3100_BUS bus_id)
 		return false;
 
 	} else {
-		PX4_INFO("Poll rate set to max (146 Hz)");
+		PX4_INFO("Poll rate set to 100 Hz");
 	}
 
 	close(fd);