imu rev1
Fork of AIviate by
sensor.cpp@0:0c627ff4c5ed, 2013-10-30 (annotated)
- Committer:
- teamgoat
- Date:
- Wed Oct 30 02:31:43 2013 +0000
- Revision:
- 0:0c627ff4c5ed
- Child:
- 2:452dd766d212
First compiled version (reads sensors, I think)
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 | 0:0c627ff4c5ed | 3 | I2C i2c(PTE1, PTE0); |
teamgoat | 0:0c627ff4c5ed | 4 | |
teamgoat | 0:0c627ff4c5ed | 5 | char set_i2c_pointer(char addr, char reg) |
teamgoat | 0:0c627ff4c5ed | 6 | { |
teamgoat | 0:0c627ff4c5ed | 7 | if (i2c.write(addr) == 0) |
teamgoat | 0:0c627ff4c5ed | 8 | { |
teamgoat | 0:0c627ff4c5ed | 9 | return 0; |
teamgoat | 0:0c627ff4c5ed | 10 | } |
teamgoat | 0:0c627ff4c5ed | 11 | if (i2c.write(reg) == 0) |
teamgoat | 0:0c627ff4c5ed | 12 | { |
teamgoat | 0:0c627ff4c5ed | 13 | return 0; |
teamgoat | 0:0c627ff4c5ed | 14 | } |
teamgoat | 0:0c627ff4c5ed | 15 | return 1; |
teamgoat | 0:0c627ff4c5ed | 16 | } |
teamgoat | 0:0c627ff4c5ed | 17 | |
teamgoat | 0:0c627ff4c5ed | 18 | int read(char addr, char reg, char *buf, int n) |
teamgoat | 0:0c627ff4c5ed | 19 | { |
teamgoat | 0:0c627ff4c5ed | 20 | i2c.start(); |
teamgoat | 0:0c627ff4c5ed | 21 | if (set_i2c_pointer(addr, reg) == 0) |
teamgoat | 0:0c627ff4c5ed | 22 | return 0; |
teamgoat | 0:0c627ff4c5ed | 23 | if (i2c.read(addr|1, buf, n, true) != 0) |
teamgoat | 0:0c627ff4c5ed | 24 | return 0; |
teamgoat | 0:0c627ff4c5ed | 25 | i2c.stop(); |
teamgoat | 0:0c627ff4c5ed | 26 | return n; |
teamgoat | 0:0c627ff4c5ed | 27 | } |
teamgoat | 0:0c627ff4c5ed | 28 | |
teamgoat | 0:0c627ff4c5ed | 29 | int write(char addr, char reg, char *buf, int n) |
teamgoat | 0:0c627ff4c5ed | 30 | { |
teamgoat | 0:0c627ff4c5ed | 31 | i2c.start(); |
teamgoat | 0:0c627ff4c5ed | 32 | if (set_i2c_pointer(addr, reg) == 0) |
teamgoat | 0:0c627ff4c5ed | 33 | return 0; |
teamgoat | 0:0c627ff4c5ed | 34 | for (int i=0; i<n; i++) |
teamgoat | 0:0c627ff4c5ed | 35 | { |
teamgoat | 0:0c627ff4c5ed | 36 | if (i2c.write(buf[i]) == 0) |
teamgoat | 0:0c627ff4c5ed | 37 | { |
teamgoat | 0:0c627ff4c5ed | 38 | i2c.stop(); |
teamgoat | 0:0c627ff4c5ed | 39 | return i; |
teamgoat | 0:0c627ff4c5ed | 40 | } |
teamgoat | 0:0c627ff4c5ed | 41 | } |
teamgoat | 0:0c627ff4c5ed | 42 | i2c.stop(); |
teamgoat | 0:0c627ff4c5ed | 43 | return n; |
teamgoat | 0:0c627ff4c5ed | 44 | |
teamgoat | 0:0c627ff4c5ed | 45 | } |
teamgoat | 0:0c627ff4c5ed | 46 | |
teamgoat | 0:0c627ff4c5ed | 47 | int read_accelerometer(struct accel* s) |
teamgoat | 0:0c627ff4c5ed | 48 | { |
teamgoat | 0:0c627ff4c5ed | 49 | int ret = read(accel_w, ACCEL_X, s->raw_data, 6); |
teamgoat | 0:0c627ff4c5ed | 50 | if (ret == 0) |
teamgoat | 0:0c627ff4c5ed | 51 | { |
teamgoat | 0:0c627ff4c5ed | 52 | return 0; |
teamgoat | 0:0c627ff4c5ed | 53 | } |
teamgoat | 0:0c627ff4c5ed | 54 | int axlsb = (int) s->raw_data[0]; |
teamgoat | 0:0c627ff4c5ed | 55 | int axmsb = (int) s->raw_data[1]; |
teamgoat | 0:0c627ff4c5ed | 56 | int aylsb = (int) s->raw_data[2]; |
teamgoat | 0:0c627ff4c5ed | 57 | int aymsb = (int) s->raw_data[3]; |
teamgoat | 0:0c627ff4c5ed | 58 | int azlsb = (int) s->raw_data[4]; |
teamgoat | 0:0c627ff4c5ed | 59 | int azmsb = (int) s->raw_data[5]; |
teamgoat | 0:0c627ff4c5ed | 60 | |
teamgoat | 0:0c627ff4c5ed | 61 | s->ax = (axmsb << 8) + axlsb; |
teamgoat | 0:0c627ff4c5ed | 62 | s->ay = (aymsb << 8) + aylsb; |
teamgoat | 0:0c627ff4c5ed | 63 | s->az = (azmsb << 8) + azlsb; |
teamgoat | 0:0c627ff4c5ed | 64 | return 1; |
teamgoat | 0:0c627ff4c5ed | 65 | } |
teamgoat | 0:0c627ff4c5ed | 66 | |
teamgoat | 0:0c627ff4c5ed | 67 | void read_gyro(struct gyro* s){} |
teamgoat | 0:0c627ff4c5ed | 68 | int read_compass(void){return 0;} |
teamgoat | 0:0c627ff4c5ed | 69 | int read_barometer(void){return 0;} |
teamgoat | 0:0c627ff4c5ed | 70 | |
teamgoat | 0:0c627ff4c5ed | 71 | void config_accelerometer(void){} |
teamgoat | 0:0c627ff4c5ed | 72 | void config_gyro(void){} |
teamgoat | 0:0c627ff4c5ed | 73 | void config_compass(void){} |
teamgoat | 0:0c627ff4c5ed | 74 | void config_barometer(void){} |
teamgoat | 0:0c627ff4c5ed | 75 | |
teamgoat | 0:0c627ff4c5ed | 76 | void config_gy80(struct config *c) |
teamgoat | 0:0c627ff4c5ed | 77 | { |
teamgoat | 0:0c627ff4c5ed | 78 | i2c.frequency(c->frequency); |
teamgoat | 0:0c627ff4c5ed | 79 | config_accelerometer(); |
teamgoat | 0:0c627ff4c5ed | 80 | config_gyro(); |
teamgoat | 0:0c627ff4c5ed | 81 | config_compass(); |
teamgoat | 0:0c627ff4c5ed | 82 | } |