imu rev1
Fork of AIviate by
Diff: sensor.cpp
- Revision:
- 0:0c627ff4c5ed
- Child:
- 2:452dd766d212
diff -r 000000000000 -r 0c627ff4c5ed sensor.cpp --- /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