imu rev1
Fork of AIviate by
sensor.cpp@2:452dd766d212, 2013-11-01 (annotated)
- Committer:
- teamgoat
- Date:
- Fri Nov 01 00:48:06 2013 +0000
- Revision:
- 2:452dd766d212
- Parent:
- 0:0c627ff4c5ed
- Child:
- 3:f9e18a9cd9af
broken :(
Who changed what in which revision?
User | Revision | Line number | New 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; |
teamgoat | 2:452dd766d212 | 5 | I2C i2c(p9, p10); |
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 | 2:452dd766d212 | 139 | pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\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 | 2:452dd766d212 | 147 | pc.printf("Error putting accelerometer in measure mode (accelerometer_measure)\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 | 2:452dd766d212 | 160 | pc.printf("Error putting accelerometer in standby (accelerometer_standby)\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 | 2:452dd766d212 | 168 | pc.printf("Error putting accelerometer in standby (accelerometer_standby)\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 | 2:452dd766d212 | 205 | return 1; |
teamgoat | 2:452dd766d212 | 206 | } |
teamgoat | 2:452dd766d212 | 207 | return ret; |
teamgoat | 2:452dd766d212 | 208 | } |
teamgoat | 2:452dd766d212 | 209 | int config_gyro(void) |
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 | 2:452dd766d212 | 217 | return 1; |
teamgoat | 2:452dd766d212 | 218 | } |
teamgoat | 2:452dd766d212 | 219 | return ret; |
teamgoat | 2:452dd766d212 | 220 | } |
teamgoat | 0:0c627ff4c5ed | 221 | void config_compass(void){} |
teamgoat | 0:0c627ff4c5ed | 222 | void config_barometer(void){} |
teamgoat | 0:0c627ff4c5ed | 223 | |
teamgoat | 2:452dd766d212 | 224 | int config_gy80(struct config *c) |
teamgoat | 0:0c627ff4c5ed | 225 | { |
teamgoat | 0:0c627ff4c5ed | 226 | i2c.frequency(c->frequency); |
teamgoat | 2:452dd766d212 | 227 | return config_accelerometer(); |
teamgoat | 2:452dd766d212 | 228 | /*config_gyro(); |
teamgoat | 2:452dd766d212 | 229 | config_compass()*/; |
teamgoat | 0:0c627ff4c5ed | 230 | } |