hmc5883: added support for temperature compensation

added "hmc5883 tempon" and "hmc5883 tempoff" to enable/disable

1 files changed, 105 insertions, 4 deletions

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index fd4d4cff5..fa501844f 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -94,6 +94,10 @@
 #define ADDR_DATA_OUT_Y_LSB		0x08
 #define ADDR_STATUS			0x09
 
+/* temperature on hmc5983 only */
+#define ADDR_TEMP_OUT_MSB		0x31
+#define ADDR_TEMP_OUT_LSB		0x32
+
 /* modes not changeable outside of driver */
 #define HMC5883L_MODE_NORMAL		(0 << 0)  /* default */
 #define HMC5883L_MODE_POSITIVE_BIAS	(1 << 0)  /* positive bias */
@@ -110,6 +114,8 @@
 #define STATUS_REG_DATA_OUT_LOCK	(1 << 1) /* page 16: set if data is only partially read, read device to reset */
 #define STATUS_REG_DATA_READY		(1 << 0) /* page 16: set if all axes have valid measurements */
 
+#define HMC5983_TEMP_SENSOR_ENABLE	(1 << 7)
+
 enum HMC5883_BUS {
 	HMC5883_BUS_ALL = 0,
 	HMC5883_BUS_I2C_INTERNAL,
@@ -219,6 +225,11 @@ private:
 	int			set_excitement(unsigned enable);
 
 	/**
+	 * enable hmc5983 temperature compensation
+	 */
+	int			set_temperature_compensation(unsigned enable);
+
+	/**
 	 * Set the sensor range.
 	 *
 	 * Sets the internal range to handle at least the argument in Gauss.
@@ -722,6 +733,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		debug("MAGIOCGEXTERNAL in main driver");
 		return _interface->ioctl(cmd, dummy);
 
+	case MAGIOCSTEMPCOMP:
+		return set_temperature_compensation(arg);
+
 	case DEVIOCGDEVICEID:
 		return _interface->ioctl(cmd, dummy);
 
@@ -847,6 +861,7 @@ HMC5883::collect()
 	perf_begin(_sample_perf);
 	struct mag_report new_report;
 	bool sensor_is_onboard = false;
+	uint8_t raw_temperature[2];
 
 	float xraw_f;
 	float yraw_f;
@@ -888,6 +903,20 @@ HMC5883::collect()
 		goto out;
 	}
 
+	/* get measurements from the device */
+	new_report.temperature = 0;
+	if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) {
+		/* if temperature compensation is enabled read the
+		 * temperature too */
+		ret = _interface->read(ADDR_TEMP_OUT_MSB, 
+				       raw_temperature, sizeof(raw_temperature));
+		if (ret == OK) {
+			int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + 
+				raw_temperature[1];
+			new_report.temperature = 25 + (temp16 / (16*8.0f));
+		}
+	}
+
 	/*
 	 * RAW outputs
 	 *
@@ -927,9 +956,6 @@ HMC5883::collect()
 	/* z remains z */
 	new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;
 
-	/* this sensor does not measure temperature, output a constant value */
-	new_report.temperature = 0.0f;
-
 	if (!(_pub_blocked)) {
 
 		if (_mag_topic != -1) {
@@ -1234,6 +1260,50 @@ int HMC5883::set_excitement(unsigned enable)
 	return !(_conf_reg == conf_reg_ret);
 }
 
+
+/*
+  enable/disable temperature compensation on the HMC5983
+
+  Unfortunately we don't yet know of a way to auto-detect the
+  difference between the HMC5883 and HMC5983. Both of them do
+  temperature sensing, but only the 5983 does temperature
+  compensation. We have noy yet found a behaviour that can be reliably
+  distinguished by reading registers to know which type a particular
+  sensor is
+ */
+int HMC5883::set_temperature_compensation(unsigned enable)
+{
+	int ret;
+	/* get current config */
+	ret = read_reg(ADDR_CONF_A, _conf_reg);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+                return -EIO;
+        }
+
+	if (enable != 0) {
+		_conf_reg |= HMC5983_TEMP_SENSOR_ENABLE;
+	} else {
+		_conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE;
+	}
+
+	ret = write_reg(ADDR_CONF_A, _conf_reg);
+
+	if (OK != ret) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	uint8_t conf_reg_ret = 0;
+	if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) {
+		perf_count(_comms_errors);
+		return -EIO;
+	}
+
+	return conf_reg_ret == _conf_reg;
+}
+
 int
 HMC5883::write_reg(uint8_t reg, uint8_t val)
 {
@@ -1276,6 +1346,7 @@ HMC5883::print_info()
 	printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", 
 	       (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
 	       (double)(1.0f/_range_scale), (double)_range_ga);
+	printf("temperature %.2f\n", (double)_last_report.temperature);
 	_reports->print_info("report queue");
 }
 
@@ -1318,6 +1389,7 @@ void	test(enum HMC5883_BUS busid);
 void	reset(enum HMC5883_BUS busid);
 int	info(enum HMC5883_BUS busid);
 int	calibrate(enum HMC5883_BUS busid);
+void	temp_enable(HMC5883_BUS busid, bool enable);
 void	usage();
 
 /**
@@ -1559,6 +1631,27 @@ reset(enum HMC5883_BUS busid)
 	exit(0);
 }
 
+
+/**
+ * enable/disable temperature compensation
+ */
+void
+temp_enable(enum HMC5883_BUS busid, bool enable)
+{
+	struct hmc5883_bus_option &bus = find_bus(busid);
+	const char *path = bus.devpath;
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0)
+		err(1, "set temperature compensation failed");
+
+	exit(0);
+}
+
 /**
  * Print a little info about the driver.
  */
@@ -1652,6 +1745,14 @@ hmc5883_main(int argc, char *argv[])
 		hmc5883::reset(busid);
 
 	/*
+	 * enable/disable temperature compensation
+	 */
+	if (!strcmp(verb, "tempoff"))
+		hmc5883::temp_enable(busid, false);
+	if (!strcmp(verb, "tempon"))
+		hmc5883::temp_enable(busid, true);
+
+	/*
 	 * Print driver information.
 	 */
 	if (!strcmp(verb, "info") || !strcmp(verb, "status"))
@@ -1669,5 +1770,5 @@ hmc5883_main(int argc, char *argv[])
 		}
 	}
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'");
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate', 'tempoff', 'tempon' or 'info'");
 }