team10 team10 / AIviate1

imu rev1

Dependencies:   IMUfilter mbed

Fork of AIviate by UCLA IEEE

sensor.cpp@5:d85bac38cdff, 2013-11-26 (annotated)

Committer:
team10
Date:
Tue Nov 26 20:29:54 2013 +0000
Revision:
5:d85bac38cdff
Parent:
4:44a5b1e8fd27 
imu rev1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamgoat 0:0c627ff4c5ed 1 #include "sensor.h"
teamgoat 0:0c627ff4c5ed 2
teamgoat 2:452dd766d212 3
teamgoat 2:452dd766d212 4 extern Serial pc;
team10 5:d85bac38cdff 5 I2C i2c(p28, p27);
teamgoat 0:0c627ff4c5ed 6
teamgoat 0:0c627ff4c5ed 7 char set_i2c_pointer(char addr, char reg)
teamgoat 0:0c627ff4c5ed 8 {
teamgoat 0:0c627ff4c5ed 9 if (i2c.write(addr) == 0)
teamgoat 0:0c627ff4c5ed 10 {
teamgoat 2:452dd766d212 11 if (DEBUG)
teamgoat 2:452dd766d212 12 pc.printf("Could not write device address (set_i2c_pointer)\r\n");
teamgoat 0:0c627ff4c5ed 13 return 0;
teamgoat 0:0c627ff4c5ed 14 }
teamgoat 0:0c627ff4c5ed 15 if (i2c.write(reg) == 0)
teamgoat 0:0c627ff4c5ed 16 {
teamgoat 2:452dd766d212 17 if (DEBUG)
teamgoat 2:452dd766d212 18 pc.printf("Could not write reg address (set_i2c_pointer)\r\n");
teamgoat 0:0c627ff4c5ed 19 return 0;
teamgoat 0:0c627ff4c5ed 20 }
teamgoat 0:0c627ff4c5ed 21 return 1;
teamgoat 0:0c627ff4c5ed 22 }
teamgoat 0:0c627ff4c5ed 23
teamgoat 0:0c627ff4c5ed 24 int read(char addr, char reg, char *buf, int n)
teamgoat 0:0c627ff4c5ed 25 {
teamgoat 0:0c627ff4c5ed 26 i2c.start();
teamgoat 0:0c627ff4c5ed 27 if (set_i2c_pointer(addr, reg) == 0)
teamgoat 2:452dd766d212 28 {
teamgoat 2:452dd766d212 29 if (DEBUG)
teamgoat 2:452dd766d212 30 pc.printf("Could not set i2c pointer (read)\r\n");
teamgoat 0:0c627ff4c5ed 31 return 0;
teamgoat 2:452dd766d212 32 }
teamgoat 0:0c627ff4c5ed 33 if (i2c.read(addr|1, buf, n, true) != 0)
teamgoat 2:452dd766d212 34 {
teamgoat 2:452dd766d212 35 if (DEBUG)
teamgoat 2:452dd766d212 36 pc.printf("Could not execute read sequence (read)\r\n");
teamgoat 0:0c627ff4c5ed 37 return 0;
teamgoat 2:452dd766d212 38 }
teamgoat 0:0c627ff4c5ed 39 i2c.stop();
teamgoat 0:0c627ff4c5ed 40 return n;
teamgoat 0:0c627ff4c5ed 41 }
teamgoat 0:0c627ff4c5ed 42
teamgoat 0:0c627ff4c5ed 43 int write(char addr, char reg, char *buf, int n)
teamgoat 0:0c627ff4c5ed 44 {
teamgoat 0:0c627ff4c5ed 45 i2c.start();
teamgoat 0:0c627ff4c5ed 46 if (set_i2c_pointer(addr, reg) == 0)
teamgoat 2:452dd766d212 47 {
teamgoat 2:452dd766d212 48 if (DEBUG)
teamgoat 2:452dd766d212 49 pc.printf("Could not set i2c pointer (write)\r\n");
teamgoat 0:0c627ff4c5ed 50 return 0;
teamgoat 2:452dd766d212 51 }
teamgoat 0:0c627ff4c5ed 52 for (int i=0; i<n; i++)
teamgoat 0:0c627ff4c5ed 53 {
teamgoat 0:0c627ff4c5ed 54 if (i2c.write(buf[i]) == 0)
teamgoat 0:0c627ff4c5ed 55 {
teamgoat 0:0c627ff4c5ed 56 i2c.stop();
teamgoat 2:452dd766d212 57 if (DEBUG)
teamgoat 2:452dd766d212 58 pc.printf("Only sent %i/%i bytes (write)\r\n", i, n);
teamgoat 0:0c627ff4c5ed 59 return i;
teamgoat 0:0c627ff4c5ed 60 }
teamgoat 0:0c627ff4c5ed 61 }
teamgoat 0:0c627ff4c5ed 62 i2c.stop();
teamgoat 0:0c627ff4c5ed 63 return n;
teamgoat 0:0c627ff4c5ed 64
teamgoat 0:0c627ff4c5ed 65 }
teamgoat 0:0c627ff4c5ed 66
teamgoat 2:452dd766d212 67 int read_accelerometer(struct sensor* s)
teamgoat 0:0c627ff4c5ed 68 {
teamgoat 0:0c627ff4c5ed 69 int ret = read(accel_w, ACCEL_X, s->raw_data, 6);
teamgoat 0:0c627ff4c5ed 70 if (ret == 0)
teamgoat 0:0c627ff4c5ed 71 {
teamgoat 2:452dd766d212 72 pc.printf("Error, could not read (read_accelerometer)\r\n");
teamgoat 2:452dd766d212 73 return 0;
teamgoat 2:452dd766d212 74 }
teamgoat 2:452dd766d212 75 int16_t axlsb = (int16_t) s->raw_data[0];
teamgoat 2:452dd766d212 76 int16_t axmsb = (int16_t) s->raw_data[1];
teamgoat 2:452dd766d212 77 int16_t aylsb = (int16_t) s->raw_data[2];
teamgoat 2:452dd766d212 78 int16_t aymsb = (int16_t) s->raw_data[3];
teamgoat 2:452dd766d212 79 int16_t azlsb = (int16_t) s->raw_data[4];
teamgoat 2:452dd766d212 80 int16_t azmsb = (int16_t) s->raw_data[5];
teamgoat 2:452dd766d212 81
teamgoat 2:452dd766d212 82 s->ax = ((axmsb << 8) + axlsb);
teamgoat 2:452dd766d212 83 s->ay = ((aymsb << 8) + aylsb);
teamgoat 2:452dd766d212 84 s->az = ((azmsb << 8) + azlsb);
teamgoat 2:452dd766d212 85 return 1;
teamgoat 2:452dd766d212 86 }
teamgoat 2:452dd766d212 87
teamgoat 2:452dd766d212 88 // disable accelerometer to save power
teamgoat 2:452dd766d212 89 int accelerometer_standby()
teamgoat 2:452dd766d212 90 {
teamgoat 2:452dd766d212 91 char power_ctl;
teamgoat 2:452dd766d212 92 int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
teamgoat 2:452dd766d212 93 if (ret == 0)
teamgoat 2:452dd766d212 94 {
teamgoat 2:452dd766d212 95 if (DEBUG)
teamgoat 2:452dd766d212 96 pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
teamgoat 0:0c627ff4c5ed 97 return 0;
teamgoat 0:0c627ff4c5ed 98 }
teamgoat 2:452dd766d212 99 power_ctl &= 0xF7 ;
teamgoat 2:452dd766d212 100 ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
teamgoat 2:452dd766d212 101 if (ret == 0)
teamgoat 2:452dd766d212 102 {
teamgoat 2:452dd766d212 103 if (DEBUG)
teamgoat 2:452dd766d212 104 pc.printf("Error putting accelerometer in standby (accelerometer_standby)\r\n");
teamgoat 2:452dd766d212 105 return 0;
teamgoat 2:452dd766d212 106 }
teamgoat 2:452dd766d212 107 return 1;
teamgoat 2:452dd766d212 108 }
teamgoat 0:0c627ff4c5ed 109
teamgoat 2:452dd766d212 110 // enable accelerometer for measurements
teamgoat 2:452dd766d212 111 int accelerometer_measure()
teamgoat 2:452dd766d212 112 {
teamgoat 2:452dd766d212 113 char power_ctl;
teamgoat 2:452dd766d212 114 int ret = read(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
teamgoat 2:452dd766d212 115 if (ret == 0)
teamgoat 2:452dd766d212 116 {
teamgoat 2:452dd766d212 117 if (DEBUG)
teamgoat 2:452dd766d212 118 pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
teamgoat 2:452dd766d212 119 return 0;
teamgoat 2:452dd766d212 120 }
teamgoat 2:452dd766d212 121 power_ctl |= 0x8 ;
teamgoat 2:452dd766d212 122 ret = write(accel_w, ACCEL_POWER_CTL, &power_ctl, 1);
teamgoat 2:452dd766d212 123 if (ret == 0)
teamgoat 2:452dd766d212 124 {
teamgoat 2:452dd766d212 125 if (DEBUG)
teamgoat 2:452dd766d212 126 pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\r\n");
teamgoat 2:452dd766d212 127 return 0;
teamgoat 2:452dd766d212 128 }
teamgoat 0:0c627ff4c5ed 129 return 1;
teamgoat 0:0c627ff4c5ed 130 }
teamgoat 0:0c627ff4c5ed 131
teamgoat 2:452dd766d212 132 int gyro_turnon()
teamgoat 2:452dd766d212 133 {
teamgoat 2:452dd766d212 134 char power_ctl;
teamgoat 2:452dd766d212 135 int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
teamgoat 2:452dd766d212 136 if (ret == 0)
teamgoat 2:452dd766d212 137 {
teamgoat 2:452dd766d212 138 if (DEBUG)
teamgoat 3:f9e18a9cd9af 139 pc.printf("Error turning on gyro (gyro_turnon)\r\n");
teamgoat 2:452dd766d212 140 return 0;
teamgoat 2:452dd766d212 141 }
teamgoat 2:452dd766d212 142 power_ctl |= 0x8 ;
teamgoat 2:452dd766d212 143 ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
teamgoat 2:452dd766d212 144 if (ret == 0)
teamgoat 2:452dd766d212 145 {
teamgoat 2:452dd766d212 146 if (DEBUG)
teamgoat 3:f9e18a9cd9af 147 pc.printf("Error turning on gyro (gyro_turnon)\r\n");
teamgoat 2:452dd766d212 148 return 0;
teamgoat 2:452dd766d212 149 }
teamgoat 2:452dd766d212 150 return 1;
teamgoat 2:452dd766d212 151 }
teamgoat 2:452dd766d212 152
teamgoat 2:452dd766d212 153 int gyro_turnoff()
teamgoat 2:452dd766d212 154 {
teamgoat 2:452dd766d212 155 char power_ctl;
teamgoat 2:452dd766d212 156 int ret = read(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
teamgoat 2:452dd766d212 157 if (ret == 0)
teamgoat 2:452dd766d212 158 {
teamgoat 2:452dd766d212 159 if (DEBUG)
teamgoat 3:f9e18a9cd9af 160 pc.printf("Error turning off gyro (gyro_turnoff)\r\n");
teamgoat 2:452dd766d212 161 return 0;
teamgoat 2:452dd766d212 162 }
teamgoat 2:452dd766d212 163 power_ctl &= 0xF7 ;
teamgoat 2:452dd766d212 164 ret = write(gyro_w, GYRO_CTRL_REG1, &power_ctl, 1);
teamgoat 2:452dd766d212 165 if (ret == 0)
teamgoat 2:452dd766d212 166 {
teamgoat 2:452dd766d212 167 if (DEBUG)
teamgoat 3:f9e18a9cd9af 168 pc.printf("Error turning off gyro (gyro_turnoff)\r\n");
teamgoat 2:452dd766d212 169 return 0;
teamgoat 2:452dd766d212 170 }
teamgoat 2:452dd766d212 171 return 1;
teamgoat 2:452dd766d212 172 }
teamgoat 2:452dd766d212 173
teamgoat 2:452dd766d212 174 int read_gyro(struct sensor* s)
teamgoat 2:452dd766d212 175 {
teamgoat 2:452dd766d212 176 int ret = read(gyro_w, GYRO_X, s->raw_data, 6);
teamgoat 2:452dd766d212 177 if (ret == 0)
teamgoat 2:452dd766d212 178 {
teamgoat 2:452dd766d212 179 pc.printf("Error, could not read (read_gyro)\r\n");
teamgoat 2:452dd766d212 180 return 0;
teamgoat 2:452dd766d212 181 }
teamgoat 2:452dd766d212 182 int16_t gxlsb = (int16_t) s->raw_data[0];
teamgoat 2:452dd766d212 183 int16_t gxmsb = (int16_t) s->raw_data[1];
teamgoat 2:452dd766d212 184 int16_t gylsb = (int16_t) s->raw_data[2];
teamgoat 2:452dd766d212 185 int16_t gymsb = (int16_t) s->raw_data[3];
teamgoat 2:452dd766d212 186 int16_t gzlsb = (int16_t) s->raw_data[4];
teamgoat 2:452dd766d212 187 int16_t gzmsb = (int16_t) s->raw_data[5];
teamgoat 2:452dd766d212 188
teamgoat 2:452dd766d212 189 s->gx = ((gxmsb << 8) + gxlsb);
teamgoat 2:452dd766d212 190 s->gy = ((gymsb << 8) + gylsb);
teamgoat 2:452dd766d212 191 s->gz = ((gzmsb << 8) + gzlsb);
teamgoat 2:452dd766d212 192 return 1;
teamgoat 2:452dd766d212 193 }
teamgoat 0:0c627ff4c5ed 194 int read_compass(void){return 0;}
teamgoat 0:0c627ff4c5ed 195 int read_barometer(void){return 0;}
teamgoat 0:0c627ff4c5ed 196
teamgoat 2:452dd766d212 197 int config_accelerometer(void)
teamgoat 2:452dd766d212 198 {
teamgoat 2:452dd766d212 199 // take accelerometer out of standby mode
teamgoat 2:452dd766d212 200 int ret = accelerometer_measure();
teamgoat 2:452dd766d212 201 if (ret == 0)
teamgoat 2:452dd766d212 202 {
teamgoat 2:452dd766d212 203 if (DEBUG)
teamgoat 2:452dd766d212 204 pc.printf("Error starting up accelerometer\r\n");
teamgoat 3:f9e18a9cd9af 205 return 0;
teamgoat 2:452dd766d212 206 }
teamgoat 3:f9e18a9cd9af 207 return 8;
teamgoat 2:452dd766d212 208 }
teamgoat 4:44a5b1e8fd27 209 int config_gyro()
teamgoat 2:452dd766d212 210 {
teamgoat 2:452dd766d212 211 // turn on the gyro via i2c
teamgoat 2:452dd766d212 212 int ret = gyro_turnon();
teamgoat 2:452dd766d212 213 if (ret == 0)
teamgoat 2:452dd766d212 214 {
teamgoat 2:452dd766d212 215 if (DEBUG)
teamgoat 2:452dd766d212 216 pc.printf("Error starting up gyro\r\n");
teamgoat 3:f9e18a9cd9af 217 return 0;
teamgoat 2:452dd766d212 218 }
teamgoat 3:f9e18a9cd9af 219 return 4;
teamgoat 2:452dd766d212 220 }
teamgoat 3:f9e18a9cd9af 221 int config_compass(void){return 2;}
teamgoat 3:f9e18a9cd9af 222 int config_barometer(void){return 1;}
teamgoat 0:0c627ff4c5ed 223
teamgoat 2:452dd766d212 224 int config_gy80(struct config *c)
teamgoat 0:0c627ff4c5ed 225 {
teamgoat 3:f9e18a9cd9af 226 // return value is a 4-bit number: AGCB, indicating
teamgoat 3:f9e18a9cd9af 227 // the return values of accel, gyro, compass, and barometer
teamgoat 0:0c627ff4c5ed 228 i2c.frequency(c->frequency);
teamgoat 3:f9e18a9cd9af 229 int ret = config_accelerometer();
teamgoat 3:f9e18a9cd9af 230 ret |= config_gyro();
teamgoat 3:f9e18a9cd9af 231 ret |= config_compass();
teamgoat 3:f9e18a9cd9af 232 ret |= config_barometer();
teamgoat 3:f9e18a9cd9af 233 return ret;
teamgoat 0:0c627ff4c5ed 234 }