Diff: HMC6343.cpp
- Revision:
- 1:bdf678f27614
- Parent:
- 0:5b09476278da
- Child:
- 2:2645c4d75671
--- a/HMC6343.cpp Fri Apr 29 22:10:27 2011 +0000
+++ b/HMC6343.cpp Sun May 01 13:51:14 2011 +0000
@@ -1,211 +1,245 @@
-/**
- * @author Aaron Berk
- * @author Serge Sozonoff
- * Based on the work of Aaron Berk for the HMC6343
- *
- * @section LICENSE
- *
- * Copyright (c) 2010 ARM Limited
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- *
- * @section DESCRIPTION
- *
- * Honeywell HMC6343 digital compass.
- *
- * Datasheet:
- *
- * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6343.pdf
- */
-
-/**
- * Includes
- */
-#include "HMC6343.h"
-
-template<class TYPE> inline TYPE BIT(const TYPE & x) {
- return TYPE(1) << x;
-}
-
-template<class TYPE> inline bool IsBitSet(const TYPE & x, const TYPE & y) {
- return 0 != (x & y);
-}
-
-HMC6343::HMC6343(PinName sda, PinName scl) {
-
- i2c_ = new I2C(sda, scl);
- //100KHz, as specified by the datasheet.
- i2c_->frequency(100000);
-
- operationMode_ = getOpMode();
-
-}
-
-double HMC6343::sampleHeading(void) {
-
- char tx[1];
- char rx[6];
-
- tx[0] = HMC6343_GET_HEADING_DATA;
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
- wait_ms(1);
-
- i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 6);
-
- return ((double)((((int)rx[0] << 8) | (int)rx[1])) / 10);
-}
-
-
-void HMC6343::setReset(void) {
- char tx[1];
- tx[0] = HMC6343_RESET;
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
- wait_ms(7);
-}
-
-void HMC6343::setCalibrationMode(int exitOrEnter) {
- char tx[1];
- int delay = 0;
-
- tx[0] = exitOrEnter;
-
- if (exitOrEnter == HMC6343_EXIT_CALIB) {
- delay = 50;
- } else if (exitOrEnter == HMC6343_ENTER_CALIB) {
- delay = 1;
- }
-
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
- wait_ms(delay);
-}
-
-
-int HMC6343::getSlaveAddress(void) {
- return read(HMC6343_SLAVE_ADDR);
-}
-
-int HMC6343::getOffset(int axis) {
-
- char rx[2] = {0x00, 0x00};
-
- if (axis == HMC6343_X_AXIS) {
- rx[0] = read(HMC6343_XOFFSET_MSB);
- rx[1] = read(HMC6343_XOFFSET_LSB);
-
- } else if (axis == HMC6343_Y_AXIS) {
- rx[0] = read(HMC6343_YOFFSET_MSB);
- rx[1] = read(HMC6343_YOFFSET_LSB);
-
- } else {
- rx[0] = read(HMC6343_ZOFFSET_MSB);
- rx[1] = read(HMC6343_ZOFFSET_LSB);
- }
-
- return ((rx[0] << 8) | (rx[1]));
-
-}
-
-
-int HMC6343::getSoftwareVersion(void) {
-
- return read(HMC6343_SOFT_VER);
-
-}
-
-int HMC6343::getOpMode(void) {
-
- char tx[1];
- tx[0] = HMC6343_GET_OPMODE;
-
- char rx[2];
-
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
- wait_ms(1);
- i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 2);
-
- operationMode_ = (rx[1] << 8) | (rx[0]);
-
- return operationMode_;
-
-}
-
-void HMC6343::setOpMode(int mode, int periodicSetReset, int measurementRate) {
-
-}
-
-void HMC6343::writeFloat(int lsb_address, float data) {
- int i = (int)(data * 100);
- if (i <= 1800 && i >= -1800) {
- write(lsb_address, (int)(i & 0x000000FF));
- write(lsb_address + 1, (int)(i >> 8));
- }
-}
-
-float HMC6343::readFloat(int lsb_eprom_address) {
- return ((float)(read(lsb_eprom_address + 1) << 8 | read(lsb_eprom_address)) / 100);
-}
-
-
-void HMC6343::setMagneticDeviation(float data) {
- writeFloat(HMC6343_DEV_LSB, data);
-}
-
-float HMC6343::getMagneticDeviation() {
- return readFloat(HMC6343_DEV_LSB);
-}
-
-void HMC6343::setMagneticVariation(float data) {
- writeFloat(HMC6343_VAR_LSB, data);
-}
-
-float HMC6343::getMagneticVariation() {
- return readFloat(HMC6343_VAR_LSB);
-}
-
-
-
-void HMC6343::write(int address, int data) {
-
- char tx[3];
-
- tx[0] = HMC6343_EEPROM_WRITE;
- tx[1] = address;
- tx[2] = data;
-
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 3);
- wait_ms(1);
-
-}
-
-int HMC6343::read(int address) {
-
- char tx[2];
- char rx[1];
-
- tx[0] = HMC6343_EEPROM_READ;
- tx[1] = address;
-
- i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 2);
- wait_ms(1);
- i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 1);
- wait_ms(1);
-
- return (rx[0]);
-
-}
+/**
+ * @author Aaron Berk
+ * @author Serge Sozonoff
+ * Partially based on the work of Aaron Berk for the HMC6352
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ *
+ * @section DESCRIPTION
+ *
+ * Honeywell HMC6343 tilt compensated digital compass.
+ *
+ * Datasheet:
+ *
+ * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6343.pdf
+ */
+
+/**
+ * Includes
+ */
+#include "HMC6343.h"
+
+template<class TYPE> inline TYPE BIT(const TYPE & x) {
+ return TYPE(1) << x;
+}
+
+template<class TYPE> inline bool IsBitSet(const TYPE & x, const TYPE & y) {
+ return 0 != (x & y);
+}
+
+HMC6343::HMC6343(PinName sda, PinName scl) {
+ i2c_ = new I2C(sda, scl);
+ i2c_->frequency(100000);
+ operationMode_ = getOpMode();
+}
+
+HMC6343::HMC6343(I2C& p_i2c) : i2c_(&p_i2c) {
+ i2c_->frequency(100000);
+ operationMode_ = getOpMode();
+}
+
+void HMC6343::sampleHeading(Heading* p_heading) {
+
+ char tx[1];
+ char rx[6];
+
+ tx[0] = HMC6343_GET_HEADING_DATA;
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+ wait_ms(1);
+
+ i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 6, true);
+
+ p_heading->heading = (double)((((int)rx[0] << 8) | (int)rx[1])) / 10;
+ p_heading->roll = (double)((((int)rx[4] << 8) | (int)rx[5])) / 10;
+ p_heading->pitch = (double)((((int)rx[2] << 8) | (int)rx[3])) / 10;
+}
+
+
+void HMC6343::setReset(void) {
+ char tx[1];
+ tx[0] = HMC6343_RESET;
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+ wait_ms(500);
+}
+
+void HMC6343::setCalibrationMode(int exitOrEnter) {
+ char tx[1];
+ int delay = 0;
+
+ tx[0] = exitOrEnter;
+
+ if (exitOrEnter == HMC6343_EXIT_CALIB) {
+ delay = 50;
+ } else if (exitOrEnter == HMC6343_ENTER_CALIB) {
+ delay = 1;
+ }
+
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+ wait_ms(delay);
+}
+
+int HMC6343::getSlaveAddress(void) {
+ return readEeprom(HMC6343_SLAVE_ADDR);
+}
+
+int HMC6343::getOffset(int axis) {
+
+ char rx[2] = {0x00, 0x00};
+
+ if (axis == HMC6343_X_AXIS) {
+ rx[0] = readEeprom(HMC6343_XOFFSET_MSB);
+ rx[1] = readEeprom(HMC6343_XOFFSET_LSB);
+
+ } else if (axis == HMC6343_Y_AXIS) {
+ rx[0] = readEeprom(HMC6343_YOFFSET_MSB);
+ rx[1] = readEeprom(HMC6343_YOFFSET_LSB);
+
+ } else {
+ rx[0] = readEeprom(HMC6343_ZOFFSET_MSB);
+ rx[1] = readEeprom(HMC6343_ZOFFSET_LSB);
+ }
+
+ return ((rx[0] << 8) | (rx[1]));
+
+}
+
+char HMC6343::getMeasurementRate() {
+ if (IsBitSet(operationMode_, HMC6343_CM_MR_10HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_5HZ)) { return 10; }
+ else if (IsBitSet(operationMode_, HMC6343_CM_MR_5HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_10HZ)) { return 5; }
+ else return 1;
+}
+
+int HMC6343::getSoftwareVersion(void) {
+ return readEeprom(HMC6343_SOFT_VER);
+}
+
+int HMC6343::getOpMode(void) {
+ char tx[1];
+ tx[0] = HMC6343_GET_OPMODE;
+
+ char rx[2];
+
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
+ wait_ms(1);
+ i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 2, true);
+
+ operationMode_ = (rx[1] << 8) | (rx[0]);
+
+ return operationMode_;
+}
+
+bool HMC6343::isOpModeFlagSet(int flag) {
+ return IsBitSet(operationMode_, flag);
+}
+
+void HMC6343::setOpMode(int opMode) {
+ writeShort(HMC6343_OPMOD_REG1, (short)opMode);
+ operationMode_ = getOpMode();
+}
+
+void HMC6343::writeShort(int lsb_address, short data) {
+ writeEeprom(lsb_address, data & 0x00FF);
+ writeEeprom(lsb_address + 1, data >> 8);
+}
+
+short HMC6343::readShort(int lsb_eprom_address) {
+ return (short)(readEeprom(lsb_eprom_address + 1) << 8) | (readEeprom(lsb_eprom_address));
+}
+
+void HMC6343::setMagneticDeviation(float data) {
+ short v;
+ v = (short)(data * 100); // move decimal right two places
+ if (v <= 1800 && v >= -1800) {
+ writeShort(HMC6343_DEV_LSB, v);
+ }
+}
+
+float HMC6343::getMagneticDeviation() {
+ return (float)(readShort(HMC6343_DEV_LSB)) / 100;
+}
+
+void HMC6343::setMagneticVariation(float data) {
+ short v;
+ v = (short)(data * 100); // move decimal right two places
+ if (v <= 1800 && v >= -1800) {
+ writeShort(HMC6343_VAR_LSB, v);
+ }
+}
+
+float HMC6343::getMagneticVariation() {
+ return (float)(readShort(HMC6343_VAR_LSB)) / 100;
+}
+
+void HMC6343::setIIRFilter(short data) {
+ writeShort(HMC6343_IIRF_LSB, data);
+}
+
+short HMC6343::getIIRFilter() {
+ return (short)readShort(HMC6343_IIRF_LSB);
+}
+
+void HMC6343::setMagOffset(int axis, int offset) {
+ if (axis == HMC6343_X_AXIS) {
+ writeShort(HMC6343_XOFFSET_LSB, offset);
+ } else if (axis == HMC6343_Y_AXIS) {
+ writeShort(HMC6343_YOFFSET_LSB, offset);
+ } else {
+ writeShort(HMC6343_ZOFFSET_LSB, offset);
+ }
+}
+
+int HMC6343::getMagOffset(int axis) {
+ if (axis == HMC6343_X_AXIS) {
+ return readShort(HMC6343_XOFFSET_LSB);
+ } else if (axis == HMC6343_Y_AXIS) {
+ return readShort(HMC6343_YOFFSET_LSB);
+ } else {
+ return readShort(HMC6343_ZOFFSET_LSB);
+ }
+}
+
+void HMC6343::writeEeprom(int address, int data) {
+ char tx[3];
+
+ tx[0] = HMC6343_EEPROM_WRITE;
+ tx[1] = address;
+ tx[2] = data;
+
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 3, false);
+ wait_ms(10);
+}
+
+int HMC6343::readEeprom(int address) {
+ char tx[2];
+ char rx[1];
+
+ tx[0] = HMC6343_EEPROM_READ;
+ tx[1] = address;
+
+ i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 2, false);
+ wait_ms(1);
+ i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 1);
+ wait_ms(1);
+
+ return rx[0];
+}
Serge Sozonoff