Diff: sensor.cpp

Revision:

2:452dd766d212

Parent:

0:0c627ff4c5ed

Child:

3:f9e18a9cd9af

--- a/sensor.cpp	Wed Oct 30 22:51:33 2013 +0000
+++ b/sensor.cpp	Fri Nov 01 00:48:06 2013 +0000
@@ -1,15 +1,21 @@
 #include "sensor.h"
 
-I2C i2c(PTE1, PTE0);
+
+extern Serial pc;
+I2C i2c(p9, p10);
 
 char set_i2c_pointer(char addr, char reg)
 {
     if (i2c.write(addr) == 0)
     {
+        if (DEBUG)
+            pc.printf("Could not write device address (set_i2c_pointer)\r\n");
         return 0;
     }
     if (i2c.write(reg) == 0)
     {
+        if (DEBUG)
+            pc.printf("Could not write reg address (set_i2c_pointer)\r\n");
         return 0;
     }
     return 1;
@@ -19,9 +25,17 @@
 {
     i2c.start();
     if (set_i2c_pointer(addr, reg) == 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not set i2c pointer (read)\r\n");
         return 0;
+    }
     if (i2c.read(addr|1, buf, n, true) != 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not execute read sequence (read)\r\n");
         return 0;
+    }
     i2c.stop();
     return n;
 }
@@ -30,12 +44,18 @@
 {
     i2c.start();
     if (set_i2c_pointer(addr, reg) == 0)
+    {
+        if (DEBUG)
+            pc.printf("Could not set i2c pointer (write)\r\n");
         return 0;
+    }
     for (int i=0; i<n; i++)
     {
         if (i2c.write(buf[i]) == 0)
         {   
             i2c.stop();
+            if (DEBUG)
+                pc.printf("Only sent %i/%i bytes (write)\r\n", i, n);
             return i;
         }
     }
@@ -44,39 +64,167 @@
     
 }
 
-int read_accelerometer(struct accel* s)
+int read_accelerometer(struct sensor* s)
 {
     int ret = read(accel_w, ACCEL_X, s->raw_data, 6);
     if (ret == 0)
     {
+        pc.printf("Error, could not read (read_accelerometer)\r\n");
+        return 0;
+    }
+    int16_t axlsb = (int16_t) s->raw_data[0];
+    int16_t axmsb = (int16_t) s->raw_data[1];
+    int16_t aylsb = (int16_t) s->raw_data[2];
+    int16_t aymsb = (int16_t) s->raw_data[3];
+    int16_t azlsb = (int16_t) s->raw_data[4];
+    int16_t azmsb = (int16_t) s->raw_data[5];
+
+    s->ax = ((axmsb << 8) + axlsb);
+    s->ay = ((aymsb << 8) + aylsb);
+    s->az = ((azmsb << 8) + azlsb);
+    return 1;
+}
+
+// disable accelerometer to save power
+int accelerometer_standby()
+{
+    char power_ctl;
+    int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
         return 0;
     }
-    int axlsb = (int) s->raw_data[0];
-    int axmsb = (int) s->raw_data[1];
-    int aylsb = (int) s->raw_data[2];
-    int aymsb = (int) s->raw_data[3];
-    int azlsb = (int) s->raw_data[4];
-    int azmsb = (int) s->raw_data[5];
+    power_ctl &= 0xF7 ;
+    ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    return 1;
+}
 
-    s->ax = (axmsb << 8) + axlsb;
-    s->ay = (aymsb << 8) + aylsb;
-    s->az = (azmsb << 8) + azlsb;
+// enable accelerometer for measurements
+int accelerometer_measure()
+{
+    char power_ctl;
+    int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }
+    power_ctl |= 0x8 ;
+    ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }    
     return 1;
 }
 
-void read_gyro(struct gyro* s){}
+int gyro_turnon()
+{
+    char power_ctl;
+    int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }
+    power_ctl |= 0x8 ;
+    ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
+        return 0;
+    }    
+    return 1;
+}
+
+int gyro_turnoff()
+{
+    char power_ctl;
+    int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    power_ctl &= 0xF7 ;
+    ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
+        return 0;
+    }
+    return 1;
+}
+
+int read_gyro(struct sensor* s)
+{
+    int ret = read(gyro_w, GYRO_X, s->raw_data, 6);
+    if (ret == 0)
+    {
+        pc.printf("Error, could not read (read_gyro)\r\n");
+        return 0;
+    }
+    int16_t gxlsb = (int16_t) s->raw_data[0];
+    int16_t gxmsb = (int16_t) s->raw_data[1];
+    int16_t gylsb = (int16_t) s->raw_data[2];
+    int16_t gymsb = (int16_t) s->raw_data[3];
+    int16_t gzlsb = (int16_t) s->raw_data[4];
+    int16_t gzmsb = (int16_t) s->raw_data[5];
+
+    s->gx = ((gxmsb << 8) + gxlsb);
+    s->gy = ((gymsb << 8) + gylsb);
+    s->gz = ((gzmsb << 8) + gzlsb);
+    return 1;
+}
 int read_compass(void){return 0;}
 int read_barometer(void){return 0;}
 
-void config_accelerometer(void){}
-void config_gyro(void){}
+int config_accelerometer(void)
+{
+    // take accelerometer out of standby mode
+    int ret = accelerometer_measure();
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error starting up accelerometer\r\n");
+        return 1;
+    }
+    return ret;
+}
+int config_gyro(void)
+{
+    // turn on the gyro via i2c
+    int ret = gyro_turnon();
+    if (ret == 0)
+    {
+        if (DEBUG)
+            pc.printf("Error starting up gyro\r\n");
+        return 1;
+    }
+    return ret;
+}
 void config_compass(void){}
 void config_barometer(void){}
 
-void config_gy80(struct config *c)
+int config_gy80(struct config *c)
 {
     i2c.frequency(c->frequency);
-    config_accelerometer();
-    config_gyro();
-    config_compass();
+    return config_accelerometer();
+    /*config_gyro();
+    config_compass()*/;
 }
\ No newline at end of file