<|start_filename|>prog_options.h<|end_filename|>
#pragma once

#include <string>

struct prog_options
{
  bool show_help = false;
  bool show_version = false;
  std::string mode;
  std::string output_mode;
  std::string i2c_bus_name;
};

void print_command_line_options_desc();

prog_options get_prog_options(int argc, char ** argv);



<|start_filename|>pacer.h<|end_filename|>
#pragma once

#include <stdint.h>
#include <unistd.h>
#include <sys/timerfd.h>
#include "exceptions.h"

class pacer
{
public:

  pacer() { }

  pacer(const pacer &) = delete;

  pacer operator=(const pacer &) = delete;

  pacer(pacer &&) = delete;

  pacer operator=(pacer &&) = delete;

  ~pacer()
  {
    close();
  }

  void set_period_ns(uint32_t nanoseconds)
  {
    if (fd == -1)
    {
      fd = timerfd_create(CLOCK_MONOTONIC, 0);
      if (fd == -1)
      {
        throw posix_error("Failed to create timerfd");
      }
    }

    struct itimerspec spec = { 0 };
    spec.it_value.tv_nsec = 1;
    spec.it_interval.tv_nsec = 20000000;
    int result = timerfd_settime(fd, 0, &spec, NULL);
    if (result == -1)
    {
      throw posix_error("Failed to set timerfd interval");
    }
  }

  uint64_t pace()
  {
    uint64_t expirations = 0;
    ssize_t result = read(fd, &expirations, sizeof(expirations));
    if (result != 8)
    {
      throw std::runtime_error("Failed to read from timer.");
    }
    return expirations;
  }

  void close()
  {
    if (fd != -1)
    {
      ::close(fd);
      fd = -1;
    }
  }

private:
  int fd = -1;
};


<|start_filename|>l3g.cpp<|end_filename|>
#include "l3g.h"
#include <stdexcept>

void l3g::handle::open(const comm_config & config)
{
  if (!config.use_sensor)
  {
    throw std::runtime_error("L3G configuration is null.");
  }

  this->config = config;
  i2c.open(config.i2c_bus_name);
}

// Turns on the gyro and places it in normal mode.
void l3g::handle::enable()
{
  write_reg(CTRL_REG1, 0b00001111); // Normal power mode, all axes enabled
  write_reg(CTRL_REG4, 0b00100000); // 2000 dps full scale
}

void l3g::handle::write_reg(uint8_t reg, uint8_t value)
{
  i2c.write_two_bytes(config.i2c_address, reg, value);
}

uint8_t l3g::handle::read_reg(uint8_t reg)
{
  return i2c.write_byte_and_read_byte(config.i2c_address, reg);
}

void l3g::handle::read()
{
  uint8_t block[6];
  i2c.write_byte_and_read(config.i2c_address,
    0x80 | OUT_X_L, block, sizeof(block));

  g[0] = (int16_t)(block[1] << 8 | block[0]);
  g[1] = (int16_t)(block[3] << 8 | block[2]);
  g[2] = (int16_t)(block[5] << 8 | block[4]);
}


<|start_filename|>lis3mdl.h<|end_filename|>
#pragma once

#include <i2c_bus.h>
#include <cstdint>

namespace lis3mdl
{
  enum device_type
  {
    LIS3MDL = 0x3D,
  };

  enum i2c_addr
  {
    SA1_LOW_ADDR = 0x1C,
    SA1_HIGH_ADDR = 0x1E,
  };

  enum reg_addr
  {
    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 {
    bool use_sensor = false;
    device_type device;
    std::string i2c_bus_name;
    i2c_addr i2c_address;
  };

  class handle
  {
  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;
    comm_config config;
  };
};


<|start_filename|>version.h<|end_filename|>
#define VERSION "3.0.0"


<|start_filename|>.dir-locals.el<|end_filename|>
; This file causes the Emacs text editor to follow our indentation style.

(
  (c-mode . ((c-basic-offset . 2) (tab-width . 2) (indent-tabs-mode . nil) (c-file-style . "linux")) )
  (c++-mode . ((c-basic-offset . 2) (tab-width . 2) (indent-tabs-mode . nil) (c-file-style . "linux")) )
  (nil . ((fill-column . 80)))
)


<|start_filename|>sensor_set.h<|end_filename|>
#pragma once

struct sensor_set
{
  bool mag = false;
  bool acc = false;
  bool gyro = false;

  operator bool()
  {
    return mag || acc || gyro;
  }

  inline friend sensor_set operator-(const sensor_set &, const sensor_set &);
};

inline sensor_set operator-(
  const sensor_set & c1,
  const sensor_set & c2)
{
  sensor_set r;
  r.mag = c1.mag && !c2.mag;
  r.acc = c1.acc && !c2.acc;
  r.gyro = c1.gyro && !c2.gyro;
  return r;
}


<|start_filename|>vector.h<|end_filename|>
#ifndef _version_h
#define _version_h

#include <stdint.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
typedef Eigen::Vector3f vector;
typedef Eigen::Vector3i int_vector;
typedef Eigen::Matrix3f matrix;
typedef Eigen::Quaternionf quaternion;

static inline vector vector_from_ints(int32_t (*ints)[3])
{
    return vector((float)(*ints)[0], (float)(*ints)[1], (float)(*ints)[2]);
}

#endif


<|start_filename|>minimu9.cpp<|end_filename|>
#include "vector.h"
#include "minimu9.h"
#include "exceptions.h"
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <wordexp.h>

minimu9::comm_config minimu9::auto_detect(const std::string & i2c_bus_name)
{
  i2c_bus bus(i2c_bus_name.c_str());
  minimu9::comm_config config;

  // Detect LSM6 devices.
  {
    auto addrs = { lsm6::SA0_LOW_ADDR, lsm6::SA0_HIGH_ADDR };
    for (uint8_t addr : addrs)
    {
      int result = bus.try_write_byte_and_read_byte(addr, lsm6::WHO_AM_I);
      if (result == lsm6::LSM6DS33)
      {
        config.lsm6.use_sensor = true;
        config.lsm6.device = (lsm6::device_type)result;
        config.lsm6.i2c_bus_name = i2c_bus_name;
        config.lsm6.i2c_address = (lsm6::i2c_addr)addr;
        break;
      }
    }
  }

  // Detect LIS3MDL devices.
  {
    auto addrs = { lis3mdl::SA1_LOW_ADDR, lis3mdl::SA1_HIGH_ADDR };
    for (uint8_t addr : addrs)
    {
      int result = bus.try_write_byte_and_read_byte(addr, lis3mdl::WHO_AM_I);
      if (result == lis3mdl::LIS3MDL)
      {
        config.lis3mdl.use_sensor = true;
        config.lis3mdl.device = (lis3mdl::device_type)result;
        config.lis3mdl.i2c_bus_name = i2c_bus_name;
        config.lis3mdl.i2c_address = (lis3mdl::i2c_addr)addr;
        break;
      }
    }
  }

  // Detect L3G devices.
  {
    auto addrs = {
      l3g::L3GD20_SA0_LOW_ADDR,
      l3g::L3GD20_SA0_HIGH_ADDR,
      l3g::L3G4200D_SA0_LOW_ADDR,
      l3g::L3G4200D_SA0_HIGH_ADDR,
    };
    for (uint8_t addr : addrs)
    {
      int result = bus.try_write_byte_and_read_byte(addr, l3g::WHO_AM_I);
      if (result == l3g::L3G4200D || result == l3g::L3GD20
        || result == l3g::L3GD20H)
      {
        config.l3g.use_sensor = true;
        config.l3g.device = (l3g::device_type)result;
        config.l3g.i2c_bus_name = i2c_bus_name;
        config.l3g.i2c_address = (l3g::i2c_addr)addr;
        break;
      }
    }
  }

  // Detect LSM303 devices.
  {
    auto & c = config.lsm303;
    if (lsm303::LSM303D == bus.try_write_byte_and_read_byte(
        lsm303::LSM303D_SA0_HIGH_ADDR, lsm303::WHO_AM_I))
    {
      c.use_sensor = true;
      c.device = lsm303::LSM303D;
      c.i2c_bus_name = i2c_bus_name;
      c.i2c_address_acc = c.i2c_address_mag =
        lsm303::LSM303D_SA0_HIGH_ADDR;
    }
    else if (lsm303::LSM303D == bus.try_write_byte_and_read_byte(
        lsm303::LSM303D_SA0_LOW_ADDR, lsm303::WHO_AM_I))
    {
      c.use_sensor = true;
      c.device = lsm303::LSM303D;
      c.i2c_bus_name = i2c_bus_name;
      c.i2c_address_acc = c.i2c_address_mag =
        lsm303::LSM303D_SA0_LOW_ADDR;
    }
    // Remaining possibilities: LSM303DLHC, LSM303DLM, or LSM303DLH.
    //
    // LSM303DLHC seems to respond to WHO_AM_I request the same way as DLM, even
    // though this register isn't documented in its datasheet, and LSM303DLH does
    // not have a WHO_AM_I.
    //
    // Lets assume that if it's a LSM303DLM and LSM303DLH then SA0 is low,
    // because that is how the Pololu boards pull that pin and this lets us
    // distinguish between the LSM303DLHC and those other chips.
    else if (bus.try_write_byte_and_read_byte(
        lsm303::LSM303_NON_D_ACC_SA0_HIGH_ADDR, lsm303::CTRL_REG1_A) >= 0)
    {
      // There's an accelerometer on a chip with SA0 high, so by the
      // logic above, guess that it's an LSM303DLHC.
      c.use_sensor = true;
      c.device = lsm303::LSM303DLHC;
      c.i2c_bus_name = i2c_bus_name;
      c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_HIGH_ADDR;
      c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR;
    }
    // Remaining possibilities: LSM303DLM or LSM303DLH, SA0 assumed low.
    else if (bus.try_write_byte_and_read_byte(
        lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR, lsm303::CTRL_REG1_A) >= 0)
    {
      // Found the acceleromter for an LSM303DLM or LSM303DLH.
      // Use the WHO_AM_I register to distinguish the two.

      int result = bus.try_write_byte_and_read_byte(
        lsm303::LSM303_NON_D_MAG_ADDR, lsm303::WHO_AM_I_M);

      if (result == lsm303::LSM303DLM)
      {
        // Detected LSM303DLM with SA0 low.
        c.use_sensor = true;
        c.device = lsm303::LSM303DLM;
        c.i2c_bus_name = i2c_bus_name;
        c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR;
        c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR;
      }
      else
      {
        // Guess that it's an LSM303DLH with SA0 low.
        c.use_sensor = true;
        c.device = lsm303::LSM303DLH;
        c.i2c_bus_name = i2c_bus_name;
        c.i2c_address_acc = lsm303::LSM303_NON_D_ACC_SA0_LOW_ADDR;
        c.i2c_address_mag = lsm303::LSM303_NON_D_MAG_ADDR;
      }
    }
  }

  return config;
}

sensor_set minimu9::config_sensor_set(const comm_config & config)
{
  sensor_set set;

  if (config.lsm6.use_sensor)
  {
    set.acc = true;
    set.gyro = true;
  }

  if (config.lis3mdl.use_sensor)
  {
    set.mag = true;
  }

  if (config.lsm303.use_sensor)
  {
    set.mag = true;
    set.acc = true;
  }

  if (config.l3g.use_sensor)
  {
    set.gyro = true;
  }

  return set;
}

minimu9::comm_config minimu9::disable_redundant_sensors(
  const comm_config & in, const sensor_set & needed)
{
  comm_config config = in;

  sensor_set missing = needed;

  if (!(missing.acc || missing.gyro))
  {
    config.lsm6.use_sensor = false;
  }
  else if (config.lsm6.use_sensor)
  {
    missing.acc = false;
    missing.gyro = false;
  }

  if (!missing.mag)
  {
    config.lis3mdl.use_sensor = false;
  }
  else if (config.lis3mdl.use_sensor)
  {
    missing.mag = false;
  }

  if (!(missing.mag || missing.acc))
  {
    config.lsm303.use_sensor = false;
  }
  else if (config.lsm303.use_sensor)
  {
    missing.mag = false;
    missing.acc = false;
  }

  if (!missing.gyro)
  {
    config.l3g.use_sensor = false;
  }
  else if (config.l3g.use_sensor)
  {
    missing.gyro = false;
  }

  return config;
}

void minimu9::handle::open(const comm_config & config)
{
  this->config = config;

  if (config.lsm6.use_sensor)
  {
    lsm6.open(config.lsm6);
  }

  if (config.lis3mdl.use_sensor)
  {
    lis3mdl.open(config.lis3mdl);
  }

  if (config.lsm303.use_sensor)
  {
    lsm303.open(config.lsm303);
  }

  if (config.l3g.use_sensor)
  {
    l3g.open(config.l3g);
  }
}

void minimu9::handle::enable()
{
  if (config.lsm6.use_sensor)
  {
    lsm6.enable();
  }

  if (config.lis3mdl.use_sensor)
  {
    lis3mdl.enable();
  }

  if (config.lsm303.use_sensor)
  {
    lsm303.enable();
  }

  if (config.l3g.use_sensor)
  {
    l3g.enable();
  }
}

void minimu9::handle::load_calibration()
{
  wordexp_t expansion_result;
  wordexp("~/.minimu9-ahrs-cal", &expansion_result, 0);

  std::ifstream file(expansion_result.we_wordv[0]);
  if (file.fail())
  {
    throw posix_error("Failed to open calibration file ~/.minimu9-ahrs-cal");
  }

  file >> mag_min(0) >> mag_max(0)
       >> mag_min(1) >> mag_max(1)
       >> mag_min(2) >> mag_max(2);
  if (file.fail() || file.bad())
  {
    throw std::runtime_error("Failed to parse calibration file ~/.minimu9-ahrs-cal");
  }
}

void minimu9::handle::read_mag_raw()
{
  if (config.lis3mdl.use_sensor)
  {
    lis3mdl.read();
    for (int i = 0; i < 3; i++) { m[i] = lis3mdl.m[i]; }
  }
  else if (config.lsm303.use_sensor)
  {
    lsm303.read_mag();
    for (int i = 0; i < 3; i++) { m[i] = lsm303.m[i]; }
  }
  else
  {
    throw std::runtime_error("No magnetometer to read.");
  }
}

void minimu9::handle::read_acc_raw()
{
  if (config.lsm6.use_sensor)
  {
    lsm6.read_acc();
    for (int i = 0; i < 3; i++) { a[i] = lsm6.a[i]; }
  }
  else if (config.lsm303.use_sensor)
  {
    lsm303.read_acc();
    for (int i = 0; i < 3; i++) { a[i] = lsm303.a[i]; }
  }
  else
  {
    throw std::runtime_error("No accelerometer to read.");
  }
}

void minimu9::handle::read_gyro_raw()
{
  if (config.lsm6.use_sensor)
  {
    lsm6.read_gyro();
    for (int i = 0; i < 3; i++) { g[i] = lsm6.g[i]; }
  }
  else if (config.l3g.use_sensor)
  {
    l3g.read();
    for (int i = 0; i < 3; i++) { g[i] = l3g.g[i]; }
  }
  else
  {
    throw std::runtime_error("No gyro to read.");
  }
}

float minimu9::handle::get_acc_scale() const
{
  // Info about linear acceleration sensitivity from datasheets:
  // LSM303DLM: at FS = 8 g, 3.9 mg/digit (12-bit reading)
  // LSM303DLHC: at FS = 8 g, 4 mg/digit (12-bit reading probably an approximation)
  // LSM303DLH: at FS = 8 g, 3.9 mg/digit (12-bit reading)
  // LSM303D: at FS = 8 g, 0.244 mg/LSB (16-bit reading)
  // LSM6DS33: at FS = 8 g, 0.244 mg/LSB (16-bit reading)
  return 0.000244;
}

float minimu9::handle::get_gyro_scale() const
{
  // Info about sensitivity from datasheets:
  // L3G4200D, FS = 2000 dps: 70 mdps/digit
  // L3GD20,   FS = 2000 dps: 70 mdps/digit
  // L3GD20H,  FS = 2000 dps: 70 mdps/digit
  // LSM6DS33, FS = 2000 dps: 70 mdps/digit
  return 0.07 * 3.14159265 / 180;
}

void minimu9::handle::measure_offsets()
{
  // LSM303 accelerometer: 8 g sensitivity.  3.8 mg/digit; 1 g = 256.
  gyro_offset = vector::Zero();
  const int sampleCount = 32;
  for(int i = 0; i < sampleCount; i++)
  {
    read_gyro_raw();
    gyro_offset += vector_from_ints(&g);
    usleep(20 * 1000);
  }
  gyro_offset /= sampleCount;
}

vector minimu9::handle::read_mag()
{
  read_mag_raw();

  vector v;
  v(0) = (float)(m[0] - mag_min(0)) / (mag_max(0) - mag_min(0)) * 2 - 1;
  v(1) = (float)(m[1] - mag_min(1)) / (mag_max(1) - mag_min(1)) * 2 - 1;
  v(2) = (float)(m[2] - mag_min(2)) / (mag_max(2) - mag_min(2)) * 2 - 1;
  return v;
}

vector minimu9::handle::read_acc()
{
  read_acc_raw();
  return vector_from_ints(&a) * get_acc_scale();
}

vector minimu9::handle::read_gyro()
{
  read_gyro_raw();
  return (vector_from_ints(&g) - gyro_offset) * get_gyro_scale();
}


<|start_filename|>l3g.h<|end_filename|>
#pragma once

#include "i2c_bus.h"

namespace l3g
{
  enum device_type
  {
    L3G4200D = 0xD3,
    L3GD20 = 0xD4,
    L3GD20H = 0xD7,
  };

  enum i2c_addr
  {
    L3GD20_SA0_LOW_ADDR = 0x6B,
    L3GD20_SA0_HIGH_ADDR = 0x6A,
    L3G4200D_SA0_LOW_ADDR = 0x68,
    L3G4200D_SA0_HIGH_ADDR = 0x69,
  };

  // For now, we use the old register names instead of the new ones that the
  // L3GD20H has.
  enum reg_addr
  {
    WHO_AM_I = 0x0F,
    CTRL_REG1 = 0x20,
    CTRL_REG2 = 0x21,
    CTRL_REG3 = 0x22,
    CTRL_REG4 = 0x23,
    CTRL_REG5 = 0x24,
    REFERENCE = 0x25,
    OUT_TEMP = 0x26,
    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,
    FIFO_CTRL_REG = 0x2E,
    FIFO_SRC_REG = 0x2F,
    INT1_CFG = 0x30,
    INT1_SRC = 0x31,
    INT1_THS_XH = 0x32,
    INT1_THS_XL = 0x33,
    INT1_THS_YH = 0x34,
    INT1_THS_YL = 0x35,
    INT1_THS_ZH = 0x36,
    INT1_THS_ZL = 0x37,
    INT1_DURATION = 0x38,
    LOW_ODR = 0x39,
  };

  struct comm_config {
    bool use_sensor = false;
    device_type device;
    std::string i2c_bus_name;
    i2c_addr i2c_address;
  };

  class handle
  {
  public:
    void open(const comm_config &);

    // gyro angular velocity readings
    int32_t g[3];

    void enable();

    void write_reg(uint8_t reg, uint8_t value);
    uint8_t read_reg(uint8_t reg);
    void read();

  protected:
    i2c_bus i2c;
    comm_config config;
  };
};


<|start_filename|>lsm6.cpp<|end_filename|>
#include "lsm6.h"
#include <stdexcept>

void lsm6::handle::open(const comm_config & config)
{
  if (!config.use_sensor)
  {
    throw std::runtime_error("LSM6 configuration is null.");
  }

  this->config = config;
  i2c.open(config.i2c_bus_name);
}

void lsm6::handle::enable()
{
  if (config.device == LSM6DS33)
  {
    //// LSM6DS33 gyro

    // ODR = 1000 (1.66 kHz (high performance))
    // FS_G = 11 (2000 dps)
    write_reg(CTRL2_G, 0b10001100);

    // defaults
    write_reg(CTRL7_G, 0b00000000);

    //// LSM6DS33 accelerometer

    // ODR = 1000 (1.66 kHz (high performance))
    // FS_XL = 11 (8 g full scale)
    // BW_XL = 00 (400 Hz filter bandwidth)
    write_reg(CTRL1_XL, 0b10001100);

    //// common

    // IF_INC = 1 (automatically increment address register)
    write_reg(CTRL3_C, 0b00000100);
  }
  else
  {
    throw std::runtime_error("Cannot enable unknown LSM6 device.");
  }
}

void lsm6::handle::write_reg(reg_addr addr, uint8_t value)
{
  i2c.write_two_bytes(config.i2c_address, addr, value);
}

void lsm6::handle::read_gyro()
{
  uint8_t block[6];
  i2c.write_byte_and_read(config.i2c_address,
    OUTX_L_G, block, sizeof(block));
  g[0] = (int16_t)(block[0] | block[1] << 8);
  g[1] = (int16_t)(block[2] | block[3] << 8);
  g[2] = (int16_t)(block[4] | block[5] << 8);
}

void lsm6::handle::read_acc()
{
  uint8_t block[6];
  i2c.write_byte_and_read(config.i2c_address,
    OUTX_L_XL, block, sizeof(block));
  a[0] = (int16_t)(block[0] | block[1] << 8);
  a[1] = (int16_t)(block[2] | block[3] << 8);
  a[2] = (int16_t)(block[4] | block[5] << 8);
}



<|start_filename|>exceptions.h<|end_filename|>
#ifndef _AHRS_EXCEPTIONS_H
#define _AHRS_EXCEPTIONS_H

#include <cerrno>
#include <system_error>
#include <string>

static inline std::system_error posix_error()
{
    return std::system_error(errno, std::system_category());
}

static inline std::system_error posix_error(const char * what)
{
    return std::system_error(errno, std::system_category(), what);
}

static inline std::system_error posix_error(const std::string & what)
{
    return std::system_error(errno, std::system_category(), what);
}

#endif


<|start_filename|>lsm303.cpp<|end_filename|>
#include "lsm303.h"
#include <stdexcept>

// Relevant Pololu products:
//
// #1250  LSM303DLH                  SA0_A pulled to GND, accessible via.
// #1264  LSM303DLH + L3G4200D (v0)  SA0_A pulled to GND, accessible thru-hole.
// #1265  LSM303DLM + L3G4200D (v1)  SA0_A pulled to GND, accessible thru-hole.
// #1268  LSM303DLHC + L3GD20  (v2)  (chip has no SA0 line)
// #1273  LSM303DLM                  SA0_A pulled to GND, accessible via.
// #2124  LSM303DLHC                 (chip has no SA0 line)
// #2127  LSM303D                    SA0 pulled to VDD, accessible thru-hole.
// #2468  LSM303D + L3GD20H (v3)     SA0 pulled to VDD, accessible thru-hole

void lsm303::handle::open(const comm_config & config)
{
  if (!config.use_sensor)
  {
    throw std::runtime_error("LSM303 configuration is null.");
  }

  this->config = config;
  i2c.open(config.i2c_bus_name);
}

uint8_t lsm303::handle::read_mag_reg(uint8_t reg)
{
  return i2c.write_byte_and_read_byte(config.i2c_address_mag, reg);
}

uint8_t lsm303::handle::read_acc_reg(uint8_t reg)
{
  return i2c.write_byte_and_read_byte(config.i2c_address_acc, reg);
}

void lsm303::handle::write_mag_reg(uint8_t reg, uint8_t value)
{
  i2c.write_two_bytes(config.i2c_address_mag, reg, value);
}

void lsm303::handle::write_acc_reg(uint8_t reg, uint8_t value)
{
  i2c.write_two_bytes(config.i2c_address_acc, reg, value);
}

// Turns on the LSM303's accelerometer and magnetometers and places them in normal
// mode.
void lsm303::handle::enable()
{
  if (config.device == LSM303D)
  {
    //// LSM303D Accelerometer

    // AODR = 0101 (50 Hz ODR)
    // AZEN = AYEN = AXEN = 1 (all axes enabled)
    write_acc_reg(CTRL1, 0b01010111);

    // AFS = 011 (8 g full scale)
    write_acc_reg(CTRL2, 0b00011000);

    //// LSM303D Magnetometer
    // M_RES = 11 (high resolution mode)
    // M_ODR = 001 (6.25 Hz ODR)
    write_mag_reg(CTRL5, 0b01100100);

    // MFS = 01 (4 gauss full scale)
    write_mag_reg(CTRL6, 0b00100000);

    // MLP = 0 (low power mode off)
    // MD = 00 (continuous-conversion mode)
    write_mag_reg(CTRL7, 0b00000000);
  }
  else if (config.device == LSM303DLHC)
  {
    //// LSM303DLHC Accelerometer

    // ODR = 0100 (50 Hz ODR)
    // LPen = 0 (normal mode)
    // Zen = Yen = Xen = 1 (all axes enabled)
    write_acc_reg(CTRL_REG1_A, 0b01000111);

    // FS = 10 (8 g full scale)
    // HR = 1 (high resolution enable)
    write_acc_reg(CTRL_REG4_A, 0b00101000);

    //// LSM303DLHC Magnetometer

    // DO = 011 (7.5 Hz ODR)
    write_mag_reg(CRA_REG_M, 0b00001100);

    // GN = 001 (+/- 1.3 gauss full scale)
    write_mag_reg(CRB_REG_M, 0b00100000);

    // MD = 00 (continuous-conversion mode)
    write_mag_reg(MR_REG_M, 0b00000000);
  }
  else
  {
    //// LSM303DLM or LSM303DLH Accelerometer

    // FS = 11 (8 g full scale)
    write_acc_reg(CTRL_REG4_A, 0b00110000);

    // PM = 001 (normal mode)
    // DR = 00 (50 Hz ODR)
    // Zen = Yen = Xen = 1 (all axes enabled)
    write_acc_reg(CTRL_REG1_A, 0b00100111);

    //// LSM303DLM or LSM303DLH Magnetometer

    // DO = 011 (7.5 Hz ODR)
    write_mag_reg(CRA_REG_M, 0b00001100);

    // GN = 001 (+/- 1.3 gauss full scale)
    write_mag_reg(CRB_REG_M, 0b00100000);

    // MD = 00 (continuous-conversion mode)
    write_mag_reg(MR_REG_M, 0b00000000);
  }
}

void lsm303::handle::read_acc()
{
  uint8_t block[6];
  i2c.write_byte_and_read(config.i2c_address_acc,
    0x80 | OUT_X_L_A, block, sizeof(block));
  a[0] = (int16_t)(block[0] | block[1] << 8);
  a[1] = (int16_t)(block[2] | block[3] << 8);
  a[2] = (int16_t)(block[4] | block[5] << 8);
}

void lsm303::handle::read_mag()
{
  uint8_t block[6];

  if (config.device == LSM303D)
  {
    // LSM303D: XYZ order, little endian
    i2c.write_byte_and_read(config.i2c_address_mag,
      0x80 | D_OUT_X_L_M, 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);
  }
  else if (config.device == LSM303DLH)
  {
    // LSM303DLH: XYZ order, big endian
    i2c.write_byte_and_read(config.i2c_address_mag,
      0x80 | DLH_OUT_X_H_M, block, sizeof(block));
    m[0] = (int16_t)(block[1] | block[0] << 8);
    m[1] = (int16_t)(block[3] | block[2] << 8);
    m[2] = (int16_t)(block[5] | block[4] << 8);
  }
  else
  {
    // LSM303DLM, LSM303DLHC: XZY order, big endian (and same addresses)
    i2c.write_byte_and_read(config.i2c_address_mag,
      0x80 | DLM_OUT_X_H_M, block, sizeof(block));
    m[0] = (int16_t)(block[1] | block[0] << 8);
    m[1] = (int16_t)(block[5] | block[4] << 8);
    m[2] = (int16_t)(block[3] | block[2] << 8);
  }
}

void lsm303::handle::read()
{
  read_acc();
  read_mag();
}


<|start_filename|>lis3mdl.cpp<|end_filename|>
#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);
  }
  else
  {
    std::runtime_error("Cannot enable unknown LIS3MDL device.");
  }
}

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);
}
