sensor.cpp

00001 #include "sensor.h"
00002 
00003 
00004 extern Serial pc;
00005 I2C i2c(p9, p10);
00006 
00007 char set_i2c_pointer(char addr, char reg)
00008 {
00009     if (i2c.write(addr) == 0)
00010     {
00011         if (DEBUG)
00012             pc.printf("Could not write device address (set_i2c_pointer)\r\n");
00013         return 0;
00014     }
00015     if (i2c.write(reg) == 0)
00016     {
00017         if (DEBUG)
00018             pc.printf("Could not write reg address (set_i2c_pointer)\r\n");
00019         return 0;
00020     }
00021     return 1;
00022 }   
00023 
00024 int read(char addr, char reg, char *buf, int n)
00025 {
00026     i2c.start();
00027     if (set_i2c_pointer(addr, reg) == 0)
00028     {
00029         if (DEBUG)
00030             pc.printf("Could not set i2c pointer (read)\r\n");
00031         return 0;
00032     }
00033     if (i2c.read(addr|1, buf, n, true) != 0)
00034     {
00035         if (DEBUG)
00036             pc.printf("Could not execute read sequence (read)\r\n");
00037         return 0;
00038     }
00039     i2c.stop();
00040     return n;
00041 }
00042 
00043 int write(char addr, char reg, char *buf, int n)
00044 {
00045     i2c.start();
00046     if (set_i2c_pointer(addr, reg) == 0)
00047     {
00048         if (DEBUG)
00049             pc.printf("Could not set i2c pointer (write)\r\n");
00050         return 0;
00051     }
00052     for (int i=0; i<n; i++)
00053     {
00054         if (i2c.write(buf[i]) == 0)
00055         {   
00056             i2c.stop();
00057             if (DEBUG)
00058                 pc.printf("Only sent %i/%i bytes (write)\r\n", i, n);
00059             return i;
00060         }
00061     }
00062     i2c.stop();
00063     return n;
00064     
00065 }
00066 
00067 int read_accelerometer(struct sensor* s)
00068 {
00069     int ret = read(accel_w, ACCEL_X, s->raw_data, 6);
00070     if (ret == 0)
00071     {
00072         pc.printf("Error, could not read (read_accelerometer)\r\n");
00073         return 0;
00074     }
00075     int16_t axlsb = (int16_t) s->raw_data[0];
00076     int16_t axmsb = (int16_t) s->raw_data[1];
00077     int16_t aylsb = (int16_t) s->raw_data[2];
00078     int16_t aymsb = (int16_t) s->raw_data[3];
00079     int16_t azlsb = (int16_t) s->raw_data[4];
00080     int16_t azmsb = (int16_t) s->raw_data[5];
00081 
00082     s->ax = ((axmsb << 8) + axlsb);
00083     s->ay = ((aymsb << 8) + aylsb);
00084     s->az = ((azmsb << 8) + azlsb);
00085     return 1;
00086 }
00087 
00088 // disable accelerometer to save power
00089 int accelerometer_standby()
00090 {
00091     char power_ctl;
00092     int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
00093     if (ret == 0)
00094     {
00095         if (DEBUG)
00096             pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
00097         return 0;
00098     }
00099     power_ctl &= 0xF7 ;
00100     ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
00101     if (ret == 0)
00102     {
00103         if (DEBUG)
00104             pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
00105         return 0;
00106     }
00107     return 1;
00108 }
00109 
00110 // enable accelerometer for measurements
00111 int accelerometer_measure()
00112 {
00113     char power_ctl;
00114     int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
00115     if (ret == 0)
00116     {
00117         if (DEBUG)
00118             pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
00119         return 0;
00120     }
00121     power_ctl |= 0x8 ;
00122     ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
00123     if (ret == 0)
00124     {
00125         if (DEBUG)
00126             pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
00127         return 0;
00128     }    
00129     return 1;
00130 }
00131 
00132 int gyro_turnon()
00133 {
00134     char power_ctl;
00135     int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
00136     if (ret == 0)
00137     {
00138         if (DEBUG)
00139             pc.printf("Error turning on gyro (gyro_turnon)\r\n");
00140         return 0;
00141     }
00142     power_ctl |= 0x8 ;
00143     ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
00144     if (ret == 0)
00145     {
00146         if (DEBUG)
00147             pc.printf("Error turning on gyro (gyro_turnon)\r\n");
00148         return 0;
00149     }    
00150     return 1;
00151 }
00152 
00153 int gyro_turnoff()
00154 {
00155     char power_ctl;
00156     int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
00157     if (ret == 0)
00158     {
00159         if (DEBUG)
00160             pc.printf("Error turning off gyro (gyro_turnoff)\r\n");
00161         return 0;
00162     }
00163     power_ctl &= 0xF7 ;
00164     ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
00165     if (ret == 0)
00166     {
00167         if (DEBUG)
00168             pc.printf("Error turning off gyro (gyro_turnoff)\r\n");
00169         return 0;
00170     }
00171     return 1;
00172 }
00173 
00174 int read_gyro(struct sensor* s)
00175 {
00176     int ret = read(gyro_w, GYRO_X, s->raw_data, 6);
00177     if (ret == 0)
00178     {
00179         pc.printf("Error, could not read (read_gyro)\r\n");
00180         return 0;
00181     }
00182     int16_t gxlsb = (int16_t) s->raw_data[0];
00183     int16_t gxmsb = (int16_t) s->raw_data[1];
00184     int16_t gylsb = (int16_t) s->raw_data[2];
00185     int16_t gymsb = (int16_t) s->raw_data[3];
00186     int16_t gzlsb = (int16_t) s->raw_data[4];
00187     int16_t gzmsb = (int16_t) s->raw_data[5];
00188 
00189     s->gx = ((gxmsb << 8) + gxlsb);
00190     s->gy = ((gymsb << 8) + gylsb);
00191     s->gz = ((gzmsb << 8) + gzlsb);
00192     return 1;
00193 }
00194 int read_compass(void){return 0;}
00195 int read_barometer(void){return 0;}
00196 
00197 int config_accelerometer(void)
00198 {
00199     // take accelerometer out of standby mode
00200     int ret = accelerometer_measure();
00201     if (ret == 0)
00202     {
00203         if (DEBUG)
00204             pc.printf("Error starting up accelerometer\r\n");
00205         return 0;
00206     }
00207     return 8;
00208 }
00209 int config_gyro()
00210 {
00211     // turn on the gyro via i2c
00212     int ret = gyro_turnon();
00213     if (ret == 0)
00214     {
00215         if (DEBUG)
00216             pc.printf("Error starting up gyro\r\n");
00217         return 0;
00218     }
00219     return 4;
00220 }
00221 int config_compass(void){return 2;}
00222 int config_barometer(void){return 1;}
00223 
00224 int config_gy80(struct config *c)
00225 {
00226     // return value is a 4-bit number: AGCB, indicating
00227     // the return values of accel, gyro, compass, and barometer
00228     i2c.frequency(c->frequency);
00229     int ret = config_accelerometer();
00230     ret |= config_gyro();
00231     ret |= config_compass();
00232     ret |= config_barometer();
00233     return ret;
00234 }