Revision 10:c4745ddaaf6a, committed 2014-04-03

Comitter:

krobertson

Date:

Thu Apr 03 17:24:53 2014 +0000

Parent:

9:da906eeac51e

Child:

11:97625c27ab90

Commit message:

added compass

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/compass.cpp	Thu Apr 03 17:24:53 2014 +0000
@@ -0,0 +1,298 @@
+#include <compass.h>
+#include <math.h>
+
+SPI spi(p5, p6, p7); // mosi, miso, sclk
+DigitalOut cs(p8);
+
+// Defines ////////////////////////////////////////////////////////////////
+
+#define D_WHO_ID    0x49
+#define DLM_WHO_ID  0x3C
+
+// Constructors ////////////////////////////////////////////////////////////////
+
+Compass::Compass(void){
+    //These values lead to an assumed magnetometer bias of 0.
+    m_min = (Compass::vector<int16_t>){-32767, -32767, -32767};
+    m_max = (Compass::vector<int16_t>){+32767, +32767, +32767};
+    //automatically determine which device is being used
+    _device = device_auto;
+    //initialize chip select to high (not active)
+    cs=1;
+}
+
+bool Compass::init(deviceType device){
+    // Setup the spi for 8 bit data, low steady state clock,
+    // rising edge capture, with a 1MHz clock rate
+    spi.format(8,0);
+    spi.frequency(1000000);
+  
+    // determine device type if necessary
+    if (device == device_auto){
+        if (testReg(WHO_AM_I) == D_WHO_ID){
+            // device responds with D ID; it's a D.
+            device = device_D;
+        }else{
+            // device hasn't responded meaningfully, so give up
+            return false;
+        }
+    }
+
+    _device = device;
+
+    // set device addresses and translated register addresses
+    switch (device){
+        case device_D:
+            translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
+            translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
+            translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
+            translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
+            translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
+            translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
+        break;
+    }
+    return true;
+}
+
+/*
+Enables the Compass's accelerometer and magnetometer. Also:
+- Sets sensor full scales (gain) to default power-on values, which are
+  +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
+  (+/- 4 gauss on LSM303D).
+- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
+  ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
+  settings for which the electrical characteristics are specified in
+  the datasheets.)
+- Enables high resolution modes (if available).
+Note that this function will also reset other settings controlled by
+the registers it writes to.
+*/
+void Compass::enableDefault(void){
+    if (_device == device_D){
+        // Accelerometer
+        writeReg(CTRL0, 0x40);
+        writeReg(CTRL2, 0x00);
+
+        // 0x57 = 0b01010111
+        // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
+        writeReg(CTRL1, 0xA7);
+
+        // Magnetometer
+
+        // 0x64 = 0b01100100
+        // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
+        writeReg(CTRL5, 0x74);
+
+        // 0x20 = 0b00100000
+        // MFS = 01 (+/- 4 gauss full scale)
+        writeReg(CTRL6, 0x20);
+
+        // 0x00 = 0b00000000
+        // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
+        writeReg(CTRL7, 0x00);
+    }
+}
+
+// Writes an accelerometer register
+void Compass::writeAccReg(regAddr reg, int value){
+    cs = 0;
+    spi.write(reg);
+    spi.write(value);
+    cs = 1;
+}
+
+// Reads an accelerometer register
+int Compass::readAccReg(regAddr reg){
+    int value;
+    cs = 0;
+    spi.write(reg | 0x80);
+    value = spi.write(0x00);
+    cs = 1;
+    return value;
+}
+
+// Writes a magnetometer register
+void Compass::writeMagReg(regAddr reg, int value){
+    cs = 0;
+    spi.write(reg);
+    spi.write(value);
+    cs = 1; 
+}
+
+// Reads a magnetometer register
+int Compass::readMagReg(regAddr reg){
+    int value;
+
+    // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)
+    if (reg < 0){
+        reg = translated_regs[-reg];
+    }
+    cs = 0;
+    spi.write(reg | (0x80));
+    value = spi.write(0x00);
+    cs = 1;
+
+    return value;
+}
+
+void Compass::writeReg(regAddr reg, int value){
+  // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M
+    if (_device == device_D || reg < CTRL_REG1_A){
+        writeMagReg(reg, value);
+    }
+    else{
+        writeAccReg(reg, value);
+    }
+}
+
+// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
+// To read those two registers, use readMagReg() instead.
+int Compass::readReg(regAddr reg){
+    // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M
+    if (_device == device_D || reg < CTRL_REG1_A){
+        return readMagReg(reg);
+    }
+    else{
+        return readAccReg(reg);
+    }
+}
+
+// Reads the 3 accelerometer channels and stores them in vector a
+void Compass::readAcc(void){
+    char reg = OUT_X_L_A | (3 << 6);
+    char valuesAcc[6];
+    cs = 0;
+    spi.write(reg);
+    for(int i=0;i<6;i++){
+        valuesAcc[i] = spi.write(0x00);
+    }
+    cs = 1;
+    
+    // combine high and low bytes
+    // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
+    // (12-bit resolution, left-aligned). The D has 16-bit resolution
+    a.x = (int16_t)(valuesAcc[1] << 8 | valuesAcc[0]);
+    a.y = (int16_t)(valuesAcc[3] << 8 | valuesAcc[2]);
+    a.z = (int16_t)(valuesAcc[5] << 8 | valuesAcc[4]);
+}
+
+// Reads the 3 magnetometer channels and stores them in vector m
+void Compass::readMag(void){
+    int reg;
+    int values[6];
+    
+    reg = 0xC8;
+    cs = 0;
+    spi.write(reg);
+    for(int i=0;i<6;i++){
+        values[i] = spi.write(0x00);
+    }
+    cs = 1;
+
+    char xlm, xhm, ylm, yhm, zlm, zhm;
+
+    //if (_device == device_D){
+        /// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
+        xlm = values[0];
+        xhm = values[1];
+        ylm = values[2];
+        yhm = values[3];
+        zlm = values[4];
+        zhm = values[5];
+   // }
+    // combine high and low bytes
+    m.x = (int16_t)(xhm << 8 | xlm);
+    m.y = (int16_t)(yhm << 8 | ylm);
+    m.z = (int16_t)(zhm << 8 | zlm);
+}
+
+// Reads all 6 channels of the LSM303 and stores them in the object variables
+void Compass::read(void){
+    readAcc();
+    readMag();
+}
+
+/*
+Returns the angular difference in the horizontal plane between a
+default vector and north, in degrees.
+
+The default vector here is chosen to point along the surface of the
+PCB, in the direction of the top of the text on the silkscreen.
+This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
+the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
+*/
+float Compass::get_heading(void)
+{
+    if (_device == device_D){
+        vector<int> params = {1,0,0};
+        return get_heading(params);
+    }else{
+        return get_heading((vector<int>){0, -1, 0});
+    }
+}
+
+/*
+Returns the angular difference in the horizontal plane between the
+"from" vector and north, in degrees.
+
+Description of heading algorithm:
+Shift and scale the magnetic reading based on calibration data to find
+the North vector. Use the acceleration readings to determine the Up
+vector (gravity is measured as an upward acceleration). The cross
+product of North and Up vectors is East. The vectors East and North
+form a basis for the horizontal plane. The From vector is projected
+into the horizontal plane and the angle between the projected vector
+and horizontal north is returned.
+*/
+template <typename T> float Compass::get_heading(vector<T> from){
+    vector<int32_t> temp_m = {m.x, m.y, m.z};
+
+    // subtract offset (average of min and max) from magnetometer readings
+    temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
+    temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
+    temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
+
+    // compute E and N
+    vector<float> E;
+    vector<float> N;
+    vector_cross(&temp_m, &a, &E);
+    vector_normalize(&E);
+    vector_cross(&a, &E, &N);
+    vector_normalize(&N);
+
+    // compute heading
+    float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / 3.14159265359;
+    if (heading < 0) heading += 360;
+    return heading;
+}
+
+template <typename Ta, typename Tb, typename To> void Compass::vector_cross(const vector<Ta> *a,const vector<Tb> *b, vector<To> *out){
+    out->x = (a->y * b->z) - (a->z * b->y);
+    out->y = (a->z * b->x) - (a->x * b->z);
+    out->z = (a->x * b->y) - (a->y * b->x);
+}
+
+template <typename Ta, typename Tb> float Compass::vector_dot(const vector<Ta> *a, const vector<Tb> *b){
+    return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
+}
+
+void Compass::vector_normalize(vector<float> *a){
+    float mag = sqrt(vector_dot(a, a));
+    a->x /= mag;
+    a->y /= mag;
+    a->z /= mag;
+}
+
+// Private Methods //////////////////////////////////////////////////////////////
+
+int Compass::testReg(regAddr reg){
+    // Select the device by seting chip select low
+    cs = 0;
+    // Send 0x8f, the command to read the WHOAMI register
+    spi.write(reg | 0x80);
+    // Send a dummy byte to receive the contents of the WHOAMI register
+    int whoami = spi.write(0x00);
+    // Deselect the device
+    cs = 1;    
+    return whoami;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/adapt/compass.h	Thu Apr 03 17:24:53 2014 +0000
@@ -0,0 +1,241 @@
+/**
+* \brief Adaptor for the LSM303 integrated compass and accelerometer
+**/
+
+#ifndef _COMPASS_H_
+#define _COMPASS_H_
+
+#include "mbed.h"
+#include <SPI.h>
+#include <stdint.h>
+
+typedef unsigned char byte;   
+
+class Compass{
+  public:
+
+    template <typename T> struct vector{
+        T x, y, z;
+    };
+
+    /**
+    * \brief LSM303 devices
+    **/
+    enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto };
+
+    /**
+    * \brief register addresses
+    **/
+    enum regAddr{
+      TEMP_OUT_L        = 0x05, // D
+      TEMP_OUT_H        = 0x06, // D
+
+      STATUS_M          = 0x07, // D
+
+      INT_CTRL_M        = 0x12, // D
+      INT_SRC_M         = 0x13, // D
+      INT_THS_L_M       = 0x14, // D
+      INT_THS_H_M       = 0x15, // D
+
+      OFFSET_X_L_M      = 0x16, // D
+      OFFSET_X_H_M      = 0x17, // D
+      OFFSET_Y_L_M      = 0x18, // D
+      OFFSET_Y_H_M      = 0x19, // D
+      OFFSET_Z_L_M      = 0x1A, // D
+      OFFSET_Z_H_M      = 0x1B, // D
+      REFERENCE_X       = 0x1C, // D
+      REFERENCE_Y       = 0x1D, // D
+      REFERENCE_Z       = 0x1E, // D
+
+      CTRL0             = 0x1F, // D
+      CTRL1             = 0x20, // D
+      CTRL_REG1_A       = 0x20, // DLH, DLM, DLHC
+      CTRL2             = 0x21, // D
+      CTRL_REG2_A       = 0x21, // DLH, DLM, DLHC
+      CTRL3             = 0x22, // D
+      CTRL_REG3_A       = 0x22, // DLH, DLM, DLHC
+      CTRL4             = 0x23, // D
+      CTRL_REG4_A       = 0x23, // DLH, DLM, DLHC
+      CTRL5             = 0x24, // D
+      CTRL_REG5_A       = 0x24, // DLH, DLM, DLHC
+      CTRL6             = 0x25, // D
+      CTRL_REG6_A       = 0x25, // DLHC
+      HP_FILTER_RESET_A = 0x25, // DLH, DLM
+      CTRL7             = 0x26, // D
+      REFERENCE_A       = 0x26, // DLH, DLM, DLHC
+      STATUS_A          = 0x27, // D
+      STATUS_REG_A      = 0x27, // DLH, DLM, DLHC
+
+      OUT_X_L_A         = 0x28,
+      OUT_X_H_A         = 0x29,
+      OUT_Y_L_A         = 0x2A,
+      OUT_Y_H_A         = 0x2B,
+      OUT_Z_L_A         = 0x2C,
+      OUT_Z_H_A         = 0x2D,
+
+      FIFO_CTRL         = 0x2E, // D
+      FIFO_CTRL_REG_A   = 0x2E, // DLHC
+      FIFO_SRC          = 0x2F, // D
+      FIFO_SRC_REG_A    = 0x2F, // DLHC
+
+      IG_CFG1           = 0x30, // D
+      INT1_CFG_A        = 0x30, // DLH, DLM, DLHC
+      IG_SRC1           = 0x31, // D
+      INT1_SRC_A        = 0x31, // DLH, DLM, DLHC
+      IG_THS1           = 0x32, // D
+      INT1_THS_A        = 0x32, // DLH, DLM, DLHC
+      IG_DUR1           = 0x33, // D
+      INT1_DURATION_A   = 0x33, // DLH, DLM, DLHC
+      IG_CFG2           = 0x34, // D
+      INT2_CFG_A        = 0x34, // DLH, DLM, DLHC
+      IG_SRC2           = 0x35, // D
+      INT2_SRC_A        = 0x35, // DLH, DLM, DLHC
+      IG_THS2           = 0x36, // D
+      INT2_THS_A        = 0x36, // DLH, DLM, DLHC
+      IG_DUR2           = 0x37, // D
+      INT2_DURATION_A   = 0x37, // DLH, DLM, DLHC
+
+      CLICK_CFG         = 0x38, // D
+      CLICK_CFG_A       = 0x38, // DLHC
+      CLICK_SRC         = 0x39, // D
+      CLICK_SRC_A       = 0x39, // DLHC
+      CLICK_THS         = 0x3A, // D
+      CLICK_THS_A       = 0x3A, // DLHC
+      TIME_LIMIT        = 0x3B, // D
+      TIME_LIMIT_A      = 0x3B, // DLHC
+      TIME_LATENCY      = 0x3C, // D
+      TIME_LATENCY_A    = 0x3C, // DLHC
+      TIME_WINDOW       = 0x3D, // D
+      TIME_WINDOW_A     = 0x3D, // DLHC
+
+      Act_THS           = 0x3E, // D
+      Act_DUR           = 0x3F, // D
+
+      CRA_REG_M         = 0x00, // DLH, DLM, DLHC
+      CRB_REG_M         = 0x01, // DLH, DLM, DLHC
+      MR_REG_M          = 0x02, // DLH, DLM, DLHC
+
+      SR_REG_M          = 0x09, // DLH, DLM, DLHC
+      IRA_REG_M         = 0x0A, // DLH, DLM, DLHC
+      IRB_REG_M         = 0x0B, // DLH, DLM, DLHC
+      IRC_REG_M         = 0x0C, // DLH, DLM, DLHC
+
+      WHO_AM_I_M        = 0x0F, // DLM
+      WHO_AM_I          = 0x0F, // D
+
+      TEMP_OUT_H_M      = 0x31, // DLHC
+      TEMP_OUT_L_M      = 0x32, // DLHC
+
+
+      // dummy addresses for registers in different locations on different devices;
+      // the library translates these based on device type
+      // value with sign flipped is used as index into translated_regs array
+
+      OUT_X_H_M         = -1,
+      OUT_X_L_M         = -2,
+      OUT_Y_H_M         = -3,
+      OUT_Y_L_M         = -4,
+      OUT_Z_H_M         = -5,
+      OUT_Z_L_M         = -6,
+      // update dummy_reg_count if registers are added here!
+
+      // device-specific register addresses
+      DLH_OUT_X_H_M     = 0x03,
+      DLH_OUT_X_L_M     = 0x04,
+      DLH_OUT_Y_H_M     = 0x05,
+      DLH_OUT_Y_L_M     = 0x06,
+      DLH_OUT_Z_H_M     = 0x07,
+      DLH_OUT_Z_L_M     = 0x08,
+
+      DLM_OUT_X_H_M     = 0x03,
+      DLM_OUT_X_L_M     = 0x04,
+      DLM_OUT_Z_H_M     = 0x05,
+      DLM_OUT_Z_L_M     = 0x06,
+      DLM_OUT_Y_H_M     = 0x07,
+      DLM_OUT_Y_L_M     = 0x08,
+
+      DLHC_OUT_X_H_M    = 0x03,
+      DLHC_OUT_X_L_M    = 0x04,
+      DLHC_OUT_Z_H_M    = 0x05,
+      DLHC_OUT_Z_L_M    = 0x06,
+      DLHC_OUT_Y_H_M    = 0x07,
+      DLHC_OUT_Y_L_M    = 0x08,
+
+      D_OUT_X_L_M       = 0x08,
+      D_OUT_X_H_M       = 0x09,
+      D_OUT_Y_L_M       = 0x0A,
+      D_OUT_Y_H_M       = 0x0B,
+      D_OUT_Z_L_M       = 0x0C,
+      D_OUT_Z_H_M       = 0x0D
+    };
+
+    /**
+    * \brief accelerometer readings
+    **/
+    vector<int16_t> a;
+    /**
+    * \brief magnetometer readings
+    **/
+    vector<int16_t> m;
+    /**
+    * \brief maximum magnetometer values (calibration values)
+    **/
+    vector<int16_t> m_max;
+    /**
+    * \brief minimum magnetometer values (calibration values)
+    **/
+    vector<int16_t> m_min;
+
+    /**
+    * \brief Compass constructor
+    **/
+    Compass(void);
+
+    /**
+    * \brief setup SPI, determine device type, and setup register addresses for device
+    **/
+    bool init(deviceType device = device_auto);
+    
+    /**
+    * \brief get the device type
+    **/
+    int getDeviceType(void) { return _device; }
+
+    /**
+    * \brief enable default settings (writes default values to control registers)
+    **/
+    void enableDefault(void);
+
+    /**
+    * \brief write to an accelerometer register
+    **/
+    void writeAccReg(regAddr reg, int value);
+    int readAccReg(regAddr reg);
+    void writeMagReg(regAddr reg, int value);
+    int readMagReg(regAddr reg);
+
+    void writeReg(regAddr reg, int value);
+    int readReg(regAddr reg);
+
+    void readAcc(void);
+    void readMag(void);
+    void read(void);
+
+    float get_heading(void);
+    template <typename T> float get_heading(vector<T> from);
+
+    // vector functions
+    template <typename Ta, typename Tb, typename To> static void vector_cross(const vector<Ta> *a, const vector<Tb> *b, vector<To> *out);
+    template <typename Ta, typename Tb> static float vector_dot(const vector<Ta> *a,const vector<Tb> *b);
+    static void vector_normalize(vector<float> *a);
+
+    deviceType _device; // chip type (DLH, DLM, or DLHC)
+
+private:  
+    static const int dummy_reg_count = 6;
+    regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used
+
+    int testReg(regAddr reg);
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleCompass.cpp	Thu Apr 03 17:24:53 2014 +0000
@@ -0,0 +1,16 @@
+#include "handleCompass.h"
+
+compassHandle::compassHandle(){
+    heading = 0;
+    setup();
+}
+
+void compassHandle::setup(){
+    compass.init();
+    compass.enableDefault();
+}
+
+void compassHandle::run(){
+    compass.read();
+    heading = compass.get_heading();
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/handle/handleCompass.h	Thu Apr 03 17:24:53 2014 +0000
@@ -0,0 +1,34 @@
+#ifndef _HANDLECOMPASS_H_
+#define _HANDLECOMPASS_H_
+
+#include "adapt/compass.h"
+
+class compassHandle {
+public:
+    /**
+    * \brief the last read compass heading
+    **/
+    float heading;
+    
+    /**
+    * \brief constructor for the compass handle.  Sets up the compass for reading.
+    **/
+    compassHandle();
+    
+    /**
+    * \brief sets up the compass for reading headings
+    **/
+    void setup();
+    
+    /**
+    * \brief gets an updated heading
+    **/
+    void run();
+private:
+    /**
+    * \brief reference to the compass adaptor
+    **/
+    Compass compass;
+};
+
+#endif
\ No newline at end of file