registers.h

Go to the documentation of this file.

#ifndef UM7_REGISTERS_H_
#define UM7_REGISTERS_H_

#if __APPLE__
#include <machine/endian.h>
#else
#include <endian.h>
#endif

#include <math.h>
#include <stdint.h>
#include <string.h>

#include <string>
#include <stdexcept>

#include "um7/firmware_registers.h"

#define TO_RADIANS (M_PI / 180.0)
#define TO_DEGREES (180.0 / M_PI)

// This excludes the command registers, which are always sent
// and received with no data.
#define NUM_REGISTERS (DATA_REG_START_ADDRESS + DATA_ARRAY_SIZE)


namespace um7
{

inline void memcpy_network(void* dest, void* src, size_t count)
{
#if __BYTE_ORDER == __LITTLE_ENDIAN
  uint8_t* d = reinterpret_cast<uint8_t*>(dest);
  uint8_t* s = reinterpret_cast<uint8_t*>(src);
  for (uint8_t i = 0; i < count; i++)
  {
    d[i] = s[count - (i+1)];
  }
#else
  // Copy bytes without reversing.
  #warning Big-endian implementation is untested.
  memcpy(dest, src, count);
#endif
}

class Registers;

class Accessor_
{
public:
  Accessor_(Registers* registers, uint8_t register_index,
            uint8_t register_width, uint8_t array_length)
    : index(register_index), width(register_width),
      length(array_length), registers_(registers)
  {}

  void* raw() const;

  const uint8_t index;

  const uint8_t width;

  const uint16_t length;

private:
  Registers* registers_;
};

template<typename RegT>
class Accessor : public Accessor_
{
  public:
    Accessor(Registers* registers, uint8_t register_index, uint8_t array_length = 0, double scale_factor = 1.0)
      : Accessor_(registers, register_index, sizeof(RegT), array_length), scale_(scale_factor)
    {}

    RegT get(uint8_t field) const
    {
      RegT* raw_ptr = reinterpret_cast<RegT*>(raw());
      RegT value;
      memcpy_network(&value, raw_ptr + field, sizeof(value));
      return value;
    }

    double get_scaled(uint16_t field) const
    {
      return get(field) * scale_;
    }

    void set(uint8_t field, RegT value) const
    {
      RegT* raw_ptr = reinterpret_cast<RegT*>(raw());
      memcpy_network(raw_ptr + field, &value, sizeof(value));
    }

    void set_scaled(uint16_t field, double value) const
    {
      set(field, value / scale_);
    }

  private:
    const double scale_;
};

class Registers
{
  public:
    Registers() :
      gyro_raw(this, DREG_GYRO_RAW_XY, 3),
      accel_raw(this, DREG_ACCEL_RAW_XY, 3),
      mag_raw(this, DREG_MAG_RAW_XY, 3),
      gyro(this, DREG_GYRO_PROC_X, 3, 1.0 * TO_RADIANS),
      accel(this, DREG_ACCEL_PROC_X, 3, 9.80665),
      mag(this, DREG_MAG_PROC_X, 3, 1.0),
      euler(this, DREG_EULER_PHI_THETA, 3, 0.0109863 * TO_RADIANS),
      quat(this, DREG_QUAT_AB, 4, 0.0000335693),
      temperature(this, DREG_TEMPERATURE, 1),
      communication(this, CREG_COM_SETTINGS, 1),
      comrate2(this, CREG_COM_RATES2, 1),
      comrate4(this, CREG_COM_RATES4, 1),
      comrate5(this, CREG_COM_RATES5, 1),
      comrate6(this, CREG_COM_RATES6, 1),
      misc_config(this, CREG_MISC_SETTINGS, 1),
      status(this, CREG_COM_RATES6, 1),
      mag_bias(this, CREG_MAG_BIAS_X, 3),
      cmd_zero_gyros(this, CHR_ZERO_GYROS),
      cmd_reset_ekf(this, CHR_RESET_EKF),
      cmd_set_mag_ref(this, CHR_SET_MAG_REFERENCE)
    {
      memset(raw_, 0, sizeof(raw_));
    }

    // Data
    const Accessor<int16_t> gyro_raw, accel_raw, euler, mag_raw, quat;

    const Accessor<float> gyro, accel, mag, temperature;

    // Configs
    const Accessor<uint32_t> communication, misc_config, status, comrate2,
                            comrate4, comrate5, comrate6;

    const Accessor<float>  mag_bias;

    // Commands
    const Accessor<uint32_t> cmd_zero_gyros, cmd_reset_ekf, cmd_set_mag_ref;

    void write_raw(uint8_t register_index, std::string data)
    {
      if ((register_index - 1) + (data.length()/4 - 1) >= NUM_REGISTERS)
      {
        throw std::range_error("Index and length write beyond boundaries of register array.");
      }
      memcpy(&raw_[register_index], data.c_str(), data.length());
    }

  private:
    uint32_t raw_[NUM_REGISTERS];

  friend class Accessor_;
};
}  // namespace um7

#endif  // UM7_REGISTERS_H_