Complete LIS3MDL implementation.

Author: DavidEGrayson

lis3mdl.cpp

@@ -0,0 +1,46 @@
+#include "lis3mdl.h"
+#include <stdexcept>
+
+void lis3mdl::handle::open(const comm_config & config)
+{
+  if (!config.use_sensor)
+  {
+    throw std::runtime_error("LIS3MDL configuration is null.");
+  }
+
+  this->config = config;
+  i2c.open(config.i2c_bus_name);
+}
+
+void lis3mdl::handle::write_reg(reg_addr addr, uint8_t value)
+{
+  i2c.write_two_bytes(config.i2c_address, addr, value);
+}
+
+void lis3mdl::handle::enable()
+{
+  if (config.device == LIS3MDL)
+  {
+    // OM = 11 (ultra-high-performance mode for X and Y); DO = 100 (10 Hz ODR)
+    write_reg(CTRL_REG1, 0b01110000);
+
+    // FS = 00 (+/- 4 gauss full scale)
+    write_reg(CTRL_REG2, 0b00000000);
+
+    // MD = 00 (continuous-conversion mode)
+    write_reg(CTRL_REG3, 0b00000000);
+
+    // OMZ = 11 (ultra-high-performance mode for Z)
+    write_reg(CTRL_REG4, 0b00001100);
+  }
+}
+
+void lis3mdl::handle::read()
+{
+  uint8_t block[6];
+  i2c.write_byte_and_read(config.i2c_address,
+    0x80 | OUT_X_L, block, sizeof(block));
+  m[0] = (int16_t)(block[0] | block[1] << 8);
+  m[1] = (int16_t)(block[2] | block[3] << 8);
+  m[2] = (int16_t)(block[4] | block[5] << 8);
+}

lis3mdl.h

@@ -18,7 +18,27 @@ namespace lis3mdl
 
   enum reg_addr
   {
-    WHO_AM_I = 0x0F,
+    WHO_AM_I    = 0x0F,
+
+    CTRL_REG1   = 0x20,
+    CTRL_REG2   = 0x21,
+    CTRL_REG3   = 0x22,
+    CTRL_REG4   = 0x23,
+    CTRL_REG5   = 0x24,
+
+    STATUS_REG  = 0x27,
+    OUT_X_L     = 0x28,
+    OUT_X_H     = 0x29,
+    OUT_Y_L     = 0x2A,
+    OUT_Y_H     = 0x2B,
+    OUT_Z_L     = 0x2C,
+    OUT_Z_H     = 0x2D,
+    TEMP_OUT_L  = 0x2E,
+    TEMP_OUT_H  = 0x2F,
+    INT_CFG     = 0x30,
+    INT_SRC     = 0x31,
+    INT_THS_L   = 0x32,
+    INT_THS_H   = 0x33,
   };
 
   struct comm_config {
@@ -33,9 +53,16 @@ namespace lis3mdl
   public:
     void open(const comm_config &);
 
+    void enable();
+
+    void write_reg(reg_addr addr, uint8_t value);
+
+    void read();
+
+    int32_t m[3];
+
   protected:
     i2c_bus i2c;
-    i2c_addr address;
-    device_type device;
+    comm_config config;
   };
 };