registers.h
Go to the documentation of this file.
00001 
00036 #ifndef UM7_REGISTERS_H_
00037 #define UM7_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 <string>
00050 #include <stdexcept>
00051 
00052 #include "um7/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 um7
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, DREG_GYRO_RAW_XY, 3),
00163       accel_raw(this, DREG_ACCEL_RAW_XY, 3),
00164       mag_raw(this, DREG_MAG_RAW_XY, 3),
00165       gyro(this, DREG_GYRO_PROC_X, 3, 1.0 * TO_RADIANS),
00166       accel(this, DREG_ACCEL_PROC_X, 3, 9.80665),
00167       mag(this, DREG_MAG_PROC_X, 3, 1.0),
00168       euler(this, DREG_EULER_PHI_THETA, 3, 0.0109863 * TO_RADIANS),
00169       quat(this, DREG_QUAT_AB, 4, 0.0000335693),
00170       temperature(this, DREG_TEMPERATURE, 1),
00171       communication(this, CREG_COM_SETTINGS, 1),
00172       comrate2(this, CREG_COM_RATES2, 1),
00173       comrate4(this, CREG_COM_RATES4, 1),
00174       comrate5(this, CREG_COM_RATES5, 1),
00175       comrate6(this, CREG_COM_RATES6, 1),
00176       misc_config(this, CREG_MISC_SETTINGS, 1),
00177       status(this, CREG_COM_RATES6, 1),
00178       mag_bias(this, CREG_MAG_BIAS_X, 3),
00179       cmd_zero_gyros(this, CHR_ZERO_GYROS),
00180       cmd_reset_ekf(this, CHR_RESET_EKF),
00181       cmd_set_mag_ref(this, CHR_SET_MAG_REFERENCE)
00182     {
00183       memset(raw_, 0, sizeof(raw_));
00184     }
00185 
00186     // Data
00187     const Accessor<int16_t> gyro_raw, accel_raw, euler, mag_raw, quat;
00188 
00189     const Accessor<float> gyro, accel, mag, temperature;
00190 
00191     // Configs
00192     const Accessor<uint32_t> communication, misc_config, status, comrate2,
00193                             comrate4, comrate5, comrate6;
00194 
00195     const Accessor<float>  mag_bias;
00196 
00197     // Commands
00198     const Accessor<uint32_t> cmd_zero_gyros, cmd_reset_ekf, cmd_set_mag_ref;
00199 
00200     void write_raw(uint8_t register_index, std::string data)
00201     {
00202       if ((register_index - 1) + (data.length()/4 - 1) >= NUM_REGISTERS)
00203       {
00204         throw std::range_error("Index and length write beyond boundaries of register array.");
00205       }
00206       memcpy(&raw_[register_index], data.c_str(), data.length());
00207     }
00208 
00209   private:
00210     uint32_t raw_[NUM_REGISTERS];
00211 
00212   friend class Accessor_;
00213 };
00214 }  // namespace um7
00215 
00216 #endif  // UM7_REGISTERS_H_


um7
Author(s): Mike Purvis , Alex Brown
autogenerated on Thu Jun 6 2019 19:34:53