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