Diff: sensor.cpp

Revision:

0:0c627ff4c5ed

Child:

2:452dd766d212

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sensor.cpp	Wed Oct 30 02:31:43 2013 +0000
@@ -0,0 +1,82 @@
+#include "sensor.h"
+
+I2C i2c(PTE1, PTE0);
+
+char set_i2c_pointer(char addr, char reg)
+{
+    if (i2c.write(addr) == 0)
+    {
+        return 0;
+    }
+    if (i2c.write(reg) == 0)
+    {
+        return 0;
+    }
+    return 1;
+}   
+
+int read(char addr, char reg, char *buf, int n)
+{
+    i2c.start();
+    if (set_i2c_pointer(addr, reg) == 0)
+        return 0;
+    if (i2c.read(addr|1, buf, n, true) != 0)
+        return 0;
+    i2c.stop();
+    return n;
+}
+
+int write(char addr, char reg, char *buf, int n)
+{
+    i2c.start();
+    if (set_i2c_pointer(addr, reg) == 0)
+        return 0;
+    for (int i=0; i<n; i++)
+    {
+        if (i2c.write(buf[i]) == 0)
+        {   
+            i2c.stop();
+            return i;
+        }
+    }
+    i2c.stop();
+    return n;
+    
+}
+
+int read_accelerometer(struct accel* s)
+{
+    int ret = read(accel_w, ACCEL_X, s->raw_data, 6);
+    if (ret == 0)
+    {
+        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];
+
+    s->ax = (axmsb << 8) + axlsb;
+    s->ay = (aymsb << 8) + aylsb;
+    s->az = (azmsb << 8) + azlsb;
+    return 1;
+}
+
+void read_gyro(struct gyro* s){}
+int read_compass(void){return 0;}
+int read_barometer(void){return 0;}
+
+void config_accelerometer(void){}
+void config_gyro(void){}
+void config_compass(void){}
+void config_barometer(void){}
+
+void config_gy80(struct config *c)
+{
+    i2c.frequency(c->frequency);
+    config_accelerometer();
+    config_gyro();
+    config_compass();
+}
\ No newline at end of file