registers.h
Go to the documentation of this file.
00001 
00036 #ifndef UM6_REGISTERS_H
00037 #define UM6_REGISTERS_H
00038 
00039 #if __APPLE__
00040 #include <machine/endian.h>
00041 #else
00042 #include <endian.h>
00043 #endif
00044 
00045 #include <math.h>
00046 #include <stdint.h>
00047 #include <string.h>
00048 
00049 #include <stdexcept>
00050 #include <string>
00051 
00052 #include "um6/firmware_registers.h"
00053 
00054 #define TO_RADIANS (M_PI / 180.0)
00055 #define TO_DEGREES (180.0 / M_PI)
00056 
00057 // This excludes the command registers, which are always sent
00058 // and received with no data.
00059 #define NUM_REGISTERS (DATA_REG_START_ADDRESS + DATA_ARRAY_SIZE)
00060 
00061 
00062 namespace um6
00063 {
00064 
00065 inline void memcpy_network(void* dest, void* src, size_t count)
00066 {
00067 #if __BYTE_ORDER == __LITTLE_ENDIAN
00068   uint8_t* d = reinterpret_cast<uint8_t*>(dest);
00069   uint8_t* s = reinterpret_cast<uint8_t*>(src);
00070   for (uint8_t i = 0; i < count; i++)
00071   {
00072     d[i] = s[count - (i+1)];
00073   }
00074 #else
00075   // Copy bytes without reversing.
00076 #warning Big-endian implementation is untested.
00077   memcpy(dest, src, count);
00078 #endif
00079 }
00080 
00081 class Registers;
00082 
00093 class Accessor_
00094 {
00095 public:
00096   Accessor_(Registers* registers, uint8_t register_index,
00097             uint8_t register_width, uint8_t array_length)
00098     : index(register_index), width(register_width),
00099       length(array_length), registers_(registers)
00100   {}
00101 
00102   void* raw() const;
00103 
00107   const uint8_t index;
00108 
00111   const uint8_t width;
00112 
00116   const uint16_t length;
00117 
00118 private:
00119   Registers* registers_;
00120 };
00121 
00122 template<typename RegT>
00123 class Accessor : public Accessor_
00124 {
00125 public:
00126   Accessor(Registers* registers, uint8_t register_index, uint8_t array_length = 0, double scale_factor = 1.0)
00127     : Accessor_(registers, register_index, sizeof(RegT), array_length), scale_(scale_factor)
00128   {}
00129 
00130   RegT get(uint8_t field) const
00131   {
00132     RegT* raw_ptr = reinterpret_cast<RegT*>(raw());
00133     RegT value;
00134     memcpy_network(&value, raw_ptr + field, sizeof(value));
00135     return value;
00136   }
00137 
00138   double get_scaled(uint16_t field) const
00139   {
00140     return get(field) * scale_;
00141   }
00142 
00143   void set(uint8_t field, RegT value) const
00144   {
00145     RegT* raw_ptr = reinterpret_cast<RegT*>(raw());
00146     memcpy_network(raw_ptr + field, &value, sizeof(value));
00147   }
00148 
00149   void set_scaled(uint16_t field, double value) const
00150   {
00151     set(field, value / scale_);
00152   }
00153 
00154 private:
00155   const double scale_;
00156 };
00157 
00158 class Registers
00159 {
00160 public:
00161   Registers() :
00162     gyro_raw(this, UM6_GYRO_RAW_XY, 3),
00163     accel_raw(this, UM6_ACCEL_RAW_XY, 3),
00164     mag_raw(this, UM6_MAG_RAW_XY, 3),
00165     gyro(this, UM6_GYRO_PROC_XY, 3, 0.0610352 * TO_RADIANS),
00166     accel(this, UM6_ACCEL_PROC_XY, 3, 0.00179639),
00167     mag(this, UM6_MAG_PROC_XY, 3, 0.000305176),
00168     euler(this, UM6_EULER_PHI_THETA, 3, 0.0109863 * TO_RADIANS),
00169     quat(this, UM6_QUAT_AB, 4, 0.0000335693),
00170     covariance(this, UM6_ERROR_COV_00, 16),
00171     temperature(this, UM6_TEMPERATURE, 1),
00172     communication(this, UM6_COMMUNICATION, 1),
00173     misc_config(this, UM6_MISC_CONFIG, 1),
00174     status(this, UM6_STATUS, 1),
00175     mag_ref(this, UM6_MAG_REF_X, 3),
00176     accel_ref(this, UM6_ACCEL_REF_X, 3),
00177     gyro_bias(this, UM6_GYRO_BIAS_XY, 3),
00178     accel_bias(this, UM6_ACCEL_BIAS_XY, 3),
00179     mag_bias(this, UM6_MAG_BIAS_XY, 3),
00180     cmd_zero_gyros(this, UM6_ZERO_GYROS),
00181     cmd_reset_ekf(this, UM6_RESET_EKF),
00182     cmd_set_accel_ref(this, UM6_SET_ACCEL_REF),
00183     cmd_set_mag_ref(this, UM6_SET_MAG_REF)
00184   {
00185     memset(raw_, 0, sizeof(raw_));
00186   }
00187 
00188   // Data
00189   const Accessor<int16_t> gyro_raw, accel_raw, mag_raw,
00190         gyro, accel, mag, euler, quat;
00191   const Accessor<float> covariance, temperature;
00192 
00193   // Configs
00194   const Accessor<uint32_t> communication, misc_config, status;
00195   const Accessor<float> mag_ref, accel_ref;
00196   const Accessor<int16_t> gyro_bias, accel_bias, mag_bias;
00197 
00198   // Commands
00199   const Accessor<uint32_t> cmd_zero_gyros, cmd_reset_ekf,
00200         cmd_set_accel_ref, cmd_set_mag_ref;
00201 
00202   void write_raw(uint8_t register_index, std::string data)
00203   {
00204     if ((register_index - 1) + (data.length()/4 - 1) >= NUM_REGISTERS)
00205     {
00206       throw std::range_error("Index and length write beyond boundaries of register array.");
00207     }
00208     memcpy(&raw_[register_index], data.c_str(), data.length());
00209   }
00210 
00211 private:
00212   uint32_t raw_[NUM_REGISTERS];
00213 
00214   friend class Accessor_;
00215 };
00216 }  // namespace um6
00217 
00218 #endif  // UM6_REGISTERS_H


um6
Author(s): Mike Purvis
autogenerated on Thu Jun 6 2019 19:02:20