Mindstorms 3rd Party ROBOTC Drivers RobotC
[Home] [Download] [Submit a bug/suggestion] [ROBOTC Forums] [Blog] [Support this project]

dexterind-compass.h

Go to the documentation of this file.
00001 /*!@addtogroup Dexter_Industries
00002 * @{
00003 * @defgroup DIMC 3D Compass Sensor
00004 * Dexter Industries DIMC 3D Compass Sensor driver
00005 * @{
00006 */
00007 
00008 /*
00009 * $Id: dexterind-compass.h 133 2013-03-10 15:15:38Z xander $
00010 */
00011 
00012 #ifndef __DIMC_H__
00013 #define __DIMC_H__
00014 /** \file dexterind-compass.h
00015 * \brief Dexter Industries IMU Sensor driver
00016 *
00017 * dexterind-compass.h provides an API for the Dexter Industries compass Sensor.\n
00018 *
00019 * Changelog:
00020 * - 0.1: Initial release
00021 *
00022 * Credits:
00023 * - Big thanks to Dexter Industries for providing me with the hardware necessary to write and test this.
00024 *
00025 * License: You may use this code as you wish, provided you give credit where its due.
00026 *
00027 * THIS CODE WILL ONLY WORK WITH ROBOTC VERSION 3.59 AND HIGHER. 
00028 
00029 * \author Xander Soldaat (xander_at_botbench.com)
00030 * \date 18 March 2012
00031 * \version 0.1
00032 * \example dexterind-compass-test1.c
00033 * \example dexterind-compass-test2.c
00034 * \example dexterind-compass-test3.c
00035 */
00036 
00037 #pragma systemFile
00038 
00039 #ifndef __COMMON_H__
00040 #include "common.h"
00041 #endif
00042 
00043 #define DIMCDAT "dimc.dat"
00044 
00045 #define DIMC_I2C_ADDR           0x3C  /*!< Compass I2C address */
00046 
00047 #define DIMC_REG_CONFIG_A       0x00  /*!< 250 dps range */
00048 #define DIMC_REG_CONFIG_B       0x01  /*!< 500 dps range */
00049 #define DIMC_REG_MODE           0x02  /*!< 2000 dps range */
00050 #define DIMC_REG_X_MSB          0x03  /*!< Register MSB X axis */
00051 #define DIMC_REG_X_LSB          0x04  /*!< Register LSB X axis */
00052 #define DIMC_REG_Z_MSB          0x05  /*!< Register MSB Z axis */
00053 #define DIMC_REG_Z_LSB          0x06  /*!< Register LSB Z axis */
00054 #define DIMC_REG_Y_MSB          0x07  /*!< Register MSB Y axis */
00055 #define DIMC_REG_Y_LSB          0x08  /*!< Register LSB Y axis */
00056 #define DIMC_REG_STATUS         0x09  /*!< Status register */
00057 #define DIMC_REG_IDENT_A        0x0A  /*!< Identification Register A */
00058 #define DIMC_REG_IDENT_B        0x0B  /*!< Identification Register B */
00059 #define DIMC_REG_IDENT_C        0x0C  /*!< Identification Register C */
00060 
00061 // HMC5883L status definitions
00062 // See page 16 of HMC5883L.pdf
00063 
00064 #define DIMC_STATUS_LOCK  2           /*!< Data output register lock active */
00065 #define DIMC_STATUS_RDY   1           /*!< Data is ready for reading */
00066 
00067 
00068 // HMC5883L configuration definitions
00069 // See pages 12, 13, 14 of HMC5883L.pdf
00070 
00071 // The Mode Register has 2 possible values for Idle mode.
00072 #define DIMC_MODE_CONTINUOUS  0       /*!< Continuous-Measurement Mode */
00073 #define DIMC_MODE_SINGLE      1       /*!< Single-Measurement Mode (Default) */
00074 #define DIMC_MODE_IDLE_A      2       /*!< Idle Mode */
00075 #define DIMC_MODE_IDLE_B      3       /*!< Idle Mode */
00076 #define DIMC_MODE_MASK        3       /*!< Mask for setting mode */
00077 
00078 // How many samples averaged? Default=1
00079 #define DIMC_CONF_A_SAMPLES_1    0x00 /*!< Number of samples averaged: 1 */
00080 #define DIMC_CONF_A_SAMPLES_2    0x20 /*!< Number of samples averaged: 2 */
00081 #define DIMC_CONF_A_SAMPLES_4    0x40 /*!< Number of samples averaged: 4 */
00082 #define DIMC_CONF_A_SAMPLES_8    0x60 /*!< Number of samples averaged: 8 */
00083 #define DIMC_CONF_A_SAMPLES_MASK 0x60 /*!< Mask for setting sample number */
00084 
00085 // Data output rate for continuous mode. Default=15Hz
00086 #define DIMC_CONF_A_RATE_0_75     0x00 /*!< Data Output Rate: 0.75 Hz */
00087 #define DIMC_CONF_A_RATE_1_5      0x04 /*!< Data Output Rate: 1.5 Hz */
00088 #define DIMC_CONF_A_RATE_3        0x08 /*!< Data Output Rate: 3 Hz */
00089 #define DIMC_CONF_A_RATE_7_5      0x0C /*!< Data Output Rate: 7.5 Hz */
00090 #define DIMC_CONF_A_RATE_15       0x10 /*!< Data Output Rate: 15 Hz */
00091 #define DIMC_CONF_A_RATE_30       0x14 /*!< Data Output Rate: 30 Hz */
00092 #define DIMC_CONF_A_RATE_75       0x18 /*!< Data Output Rate: 75 Hz */
00093 #define DIMC_CONF_A_RATE_RESERVED 0x1C
00094 #define DIMC_CONF_A_RATE_MASK     0x1C /*!< Mask for setting Data Output Rate */
00095 
00096 // Measurement configuration, whether to apply bias. Default=Normal
00097 #define DIMC_CONF_A_BIAS_NORMAL   0x00 /*!< Normal measurement configuration (Default) */
00098 #define DIMC_CONF_A_BIAS_POSITIVE 0x01 /*!< Positive bias configuration for X, Y, and Z axes. */
00099 #define DIMC_CONF_A_BIAS_NEGATIVE 0x02 /*!< Negative bias configuration for X, Y and Z axes. */
00100 #define DIMC_CONF_A_BIAS_RESERVED 0x03
00101 #define DIMC_CONF_A_BIAS_MASK     0x03 /*!< Mask for setting measurement bias */
00102 
00103 // Gain configuration. Default=1.3Ga
00104 #define DIMC_CONF_B_GAIN_0_88 0x00     /*!< Sensor Field Range 0.88 Ga */
00105 #define DIMC_CONF_B_GAIN_1_3  0x20     /*!< Sensor Field Range 1.3 Ga */
00106 #define DIMC_CONF_B_GAIN_1_9  0x40     /*!< Sensor Field Range 1.9 Ga */
00107 #define DIMC_CONF_B_GAIN_2_5  0x60     /*!< Sensor Field Range 2.5 Ga */
00108 #define DIMC_CONF_B_GAIN_4_0  0x80     /*!< Sensor Field Range 4.0 Ga */
00109 #define DIMC_CONF_B_GAIN_4_7  0xA0     /*!< Sensor Field Range 4.7 Ga */
00110 #define DIMC_CONF_B_GAIN_5_6  0xC0     /*!< Sensor Field Range 5.6 Ga */
00111 #define DIMC_CONF_B_GAIN_8_1  0xE0     /*!< Sensor Field Range 8.1 Ga */
00112 #define DIMC_CONF_B_GAIN_MASK 0xE0     /*!< Mask for setting Sensor Field Range */
00113 
00114 // Digital resolution (mG/LSb) for each gain
00115 #define DIMC_GAIN_SCALE_0_88  0.73     /*!< Ramge multiplier for 0.88 Ga */
00116 #define DIMC_GAIN_SCALE_1_3   0.92     /*!< Ramge multiplier for 1.3 Ga */
00117 #define DIMC_GAIN_SCALE_1_9   1.22     /*!< Ramge multiplier for 1.9 Ga */
00118 #define DIMC_GAIN_SCALE_2_5   1.52     /*!< Ramge multiplier for 2.5 Ga */
00119 #define DIMC_GAIN_SCALE_4_0   2.27     /*!< Ramge multiplier for 4.0 Ga */
00120 #define DIMC_GAIN_SCALE_4_7   2.56     /*!< Ramge multiplier for 4.7 Ga */
00121 #define DIMC_GAIN_SCALE_5_6   3.03     /*!< Ramge multiplier for 5.6 Ga */
00122 #define DIMC_GAIN_SCALE_8_1   4.35     /*!< Ramge multiplier for 8.1 Ga */
00123 
00124 tByteArray DIMC_I2CRequest;    /*!< Array to hold I2C command data */
00125 tByteArray DIMC_I2CReply;      /*!< Array to hold I2C reply data */
00126 
00127 bool DIMCinit(tSensors link, ubyte range, bool lpfenable=true);
00128 bool DIMCreadAxes(tSensors link, int &_x, int &_y, int &_z);
00129 float DIMCreadHeading(tSensors link);
00130 void DIMCstartCal(tSensors link);
00131 void DIMCstopCal(tSensors link);
00132 void _DIMCreadCalVals();
00133 void _DIMCwriteCalVals();
00134 
00135 bool DIMCcalibrating[4] = {false, false, false, false };
00136 bool DIMCcalibrationDataLoaded = false;
00137 int DIMCminVals[4][3] = {{32767, 32767, 32767}, {32767, 32767, 32767}, {32767, 32767, 32767}, {32767, 32767, 32767}};
00138 int DIMCmaxVals[4][3] = {{0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}};
00139 int DIMCoffsets[4][3] = {{0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}};
00140 
00141 /**
00142 * Configure the Compass
00143 * @param link the port number
00144 * @return true if no error occured, false if it did
00145 */
00146 bool DIMCinit(tSensors link)
00147 {
00148   memset(DIMC_I2CRequest, 0, sizeof(DIMC_I2CRequest));
00149 
00150   // Setup the size and address, same for all requests.
00151   DIMC_I2CRequest[0] = 3;    // Sending address, register, value. Optional, defaults to true
00152   DIMC_I2CRequest[1] = DIMC_I2C_ADDR; // I2C Address of Compass.
00153 
00154   // Write CONFIG_A
00155   // Set to 15Hz sample rate and a 8 sample average.
00156   DIMC_I2CRequest[2] = DIMC_REG_CONFIG_A;
00157   DIMC_I2CRequest[3] = DIMC_CONF_A_SAMPLES_8 + DIMC_CONF_A_RATE_15;
00158   if (!writeI2C(link, DIMC_I2CRequest))
00159     return false;
00160 
00161   // Write CONFIG_B
00162   // Set gain to 1.9
00163   DIMC_I2CRequest[2] = DIMC_REG_CONFIG_B;
00164   DIMC_I2CRequest[3] = DIMC_CONF_B_GAIN_1_3;
00165   if (!writeI2C(link, DIMC_I2CRequest))
00166     return false;
00167 
00168   // Write REG_MODE
00169   // Set to continuous mode
00170   ////////////////////////////////////////////////////////////////////////////
00171   DIMC_I2CRequest[2] = DIMC_REG_MODE;           // Register address of CTRL_REG3
00172   DIMC_I2CRequest[3] = DIMC_MODE_CONTINUOUS;    // No interrupts.  Date ready.
00173   return writeI2C(link, DIMC_I2CRequest);
00174 }
00175 
00176 
00177 /**
00178 * Read all three axes of the Compass
00179 * @param link the port number
00180 * @param _x data for x axis in degrees per second
00181 * @param _y data for y axis in degrees per second
00182 * @param _z data for z axis in degrees per second
00183 * @return true if no error occured, false if it did
00184 */
00185 bool DIMCreadAxes(tSensors link, int &_x, int &_y, int &_z)
00186 {
00187   if (!DIMCcalibrationDataLoaded)
00188     _DIMCreadCalVals();
00189 
00190   DIMC_I2CRequest[0] = 2;                   // Message size
00191   DIMC_I2CRequest[1] = DIMC_I2C_ADDR;  // I2C Address
00192   DIMC_I2CRequest[2] = DIMC_REG_X_MSB;            // Register address
00193 
00194   if (!writeI2C(link, DIMC_I2CRequest, DIMC_I2CReply, 6)) {
00195     writeDebugStreamLine("error write");
00196     return false;
00197   }
00198 
00199   _x = (DIMC_I2CReply[0]<<8) + DIMC_I2CReply[1];
00200   _z = (DIMC_I2CReply[2]<<8) + DIMC_I2CReply[3];
00201   _y = (DIMC_I2CReply[4]<<8) + DIMC_I2CReply[5];
00202 
00203   if (DIMCcalibrating[link])
00204   {
00205     DIMCminVals[link][0] = min2(_x, DIMCminVals[link][0]);
00206     DIMCminVals[link][1] = min2(_y, DIMCminVals[link][1]);
00207     DIMCminVals[link][2] = min2(_z, DIMCminVals[link][2]);
00208 
00209     DIMCmaxVals[link][0] = max2(_x, DIMCmaxVals[link][0]);
00210     DIMCmaxVals[link][1] = max2(_y, DIMCmaxVals[link][1]);
00211     DIMCmaxVals[link][2] = max2(_z, DIMCmaxVals[link][2]);
00212   }
00213 
00214   _x -= DIMCoffsets[link][0];
00215   _y -= DIMCoffsets[link][1];
00216   _z -= DIMCoffsets[link][2];
00217 
00218   return true;
00219 }
00220 
00221 
00222 /**
00223 * Read the current heading
00224 * @return the heading in degrees.
00225 */
00226 float DIMCreadHeading(tSensors link)
00227 {
00228   float angle;
00229   int fx,fy,fz;
00230   DIMCreadAxes(link, fx, fy, fz);
00231 
00232   angle = atan2(fx, fy);
00233   if (angle < 0) angle += 2*PI;
00234   return angle * (180/PI);
00235 }
00236 
00237 
00238 /**
00239 * Start calibration.  The robot should be made to rotate
00240 * about its axis at least twice to get an accurate result.
00241 * Stop the calibration with DIMCstopCal()
00242 */
00243 void DIMCstartCal(tSensors link)
00244 {
00245   DIMCcalibrating[link] = true;
00246 }
00247 
00248 
00249 /**
00250 * Stop calibration.  The appropriate offsets will be calculated for all
00251 * the axes.
00252 */
00253 void DIMCstopCal(tSensors link)
00254 {
00255   DIMCcalibrating[link] = false;
00256   DIMCoffsets[link][0] = ((DIMCmaxVals[link][0] - DIMCminVals[link][0]) / 2) + DIMCminVals[link][0];
00257   DIMCoffsets[link][1] = ((DIMCmaxVals[link][1] - DIMCminVals[link][1]) / 2) + DIMCminVals[link][1];
00258   DIMCoffsets[link][2] = ((DIMCmaxVals[link][2] - DIMCminVals[link][2]) / 2) + DIMCminVals[link][2];
00259   _DIMCwriteCalVals();
00260 }
00261 
00262 
00263 /**
00264  * Write the calibration values to a data file.
00265  *
00266  * Note: this is an internal function and should not be called directly
00267  */
00268 void _DIMCwriteCalVals()
00269 {
00270   TFileHandle hFileHandle;
00271   TFileIOResult nIoResult;
00272   short nFileSize = sizeof(DIMCoffsets);
00273 
00274   // Delete the old data file and open a new one for writing
00275   Delete(DIMCDAT, nIoResult);
00276   OpenWrite(hFileHandle, nIoResult, DIMCDAT, nFileSize);
00277   if (nIoResult != ioRsltSuccess)
00278   {
00279     Close(hFileHandle, nIoResult);
00280     eraseDisplay();
00281     nxtDisplayTextLine(3, "W:can't cal file");
00282     PlaySound(soundException);
00283     while(bSoundActive) EndTimeSlice();
00284     wait1Msec(5000);
00285     StopAllTasks();
00286   }
00287 
00288         for (int i = 0; i < 4; i++)
00289         {
00290     for (int j = 0; j < 3; j++)
00291     {
00292                   WriteShort(hFileHandle, nIoResult, DIMCoffsets[i][j]);
00293                   if (nIoResult != ioRsltSuccess)
00294                   {
00295               eraseDisplay();
00296                     nxtDisplayTextLine(3, "can't write lowval");
00297                     PlaySound(soundException);
00298                     while(bSoundActive) EndTimeSlice();
00299                     wait1Msec(5000);
00300                     StopAllTasks();
00301             }
00302           }
00303         }
00304 
00305   // Close the file
00306   Close(hFileHandle, nIoResult);
00307   if (nIoResult != ioRsltSuccess)
00308   {
00309     eraseDisplay();
00310     nxtDisplayTextLine(3, "Can't close");
00311     PlaySound(soundException);
00312     while(bSoundActive) EndTimeSlice();
00313     wait1Msec(5000);
00314     StopAllTasks();
00315   }
00316 }
00317 
00318 
00319 
00320 /**
00321  * Read the calibration values from a data file.
00322  *
00323  * Note: this is an internal function and should not be called directly
00324  */
00325 void _DIMCreadCalVals()
00326 {
00327   TFileHandle hFileHandle;
00328   TFileIOResult nIoResult;
00329   short nFileSize;
00330 
00331   // Open the data file for reading
00332   DIMCcalibrationDataLoaded = true;
00333   OpenRead(hFileHandle, nIoResult, DIMCDAT, nFileSize);
00334   if (nIoResult != ioRsltSuccess)
00335   {
00336     Close(hFileHandle, nIoResult);
00337     // Assign default values
00338                 memset(DIMCoffsets, 0, sizeof(DIMCoffsets));
00339                 _DIMCwriteCalVals();
00340     return;
00341   }
00342 
00343   for (int i = 0; i < 4; i++)
00344   {
00345     for (int j = 0; j < 3; j++)
00346     {
00347                   ReadShort(hFileHandle, nIoResult, DIMCoffsets[i][j]);
00348                   // writeDebugStream("R offsets[%d][%d]:", i, j);
00349                   // writeDebugStreamLine(" %d", DIMCoffsets[i][j]);
00350                   if (nIoResult != ioRsltSuccess)
00351                   {
00352                     memset(DIMCoffsets, 0, sizeof(DIMCoffsets));
00353                           _DIMCwriteCalVals();
00354                           return;
00355                         }
00356           }
00357         }
00358 
00359   Close(hFileHandle, nIoResult);
00360 }
00361 
00362 
00363 
00364 
00365 
00366 #endif // __DIMC_H__
00367 
00368 /*
00369 * $Id: dexterind-compass.h 133 2013-03-10 15:15:38Z xander $
00370 */
00371 /* @} */
00372 /* @} */