Oskar Weigl
This is for ICRS\' second generation Quadcopter
Sensors.h@0:0bbf2f16da9c, 2011-11-18 (annotated)
- Committer:
- madcowswe
- Date:
- Fri Nov 18 18:23:33 2011 +0000
- Revision:
- 0:0bbf2f16da9c
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| madcowswe | 0:0bbf2f16da9c | 1 | |
| madcowswe | 0:0bbf2f16da9c | 2 | //In here be functions related to Sensors |
| madcowswe | 0:0bbf2f16da9c | 3 | |
| madcowswe | 0:0bbf2f16da9c | 4 | // |
| madcowswe | 0:0bbf2f16da9c | 5 | void setupaccel() { |
| madcowswe | 0:0bbf2f16da9c | 6 | i2c.start(); |
| madcowswe | 0:0bbf2f16da9c | 7 | i2c.write(ACCADDRESS << 1 | 0); |
| madcowswe | 0:0bbf2f16da9c | 8 | i2c.write(7); |
| madcowswe | 0:0bbf2f16da9c | 9 | i2c.write(1); |
| madcowswe | 0:0bbf2f16da9c | 10 | i2c.stop(); |
| madcowswe | 0:0bbf2f16da9c | 11 | } |
| madcowswe | 0:0bbf2f16da9c | 12 | |
| madcowswe | 0:0bbf2f16da9c | 13 | // |
| madcowswe | 0:0bbf2f16da9c | 14 | void getaccel(signed char returnbuff[3]) { |
| madcowswe | 0:0bbf2f16da9c | 15 | i2c.start(); |
| madcowswe | 0:0bbf2f16da9c | 16 | i2c.write(ACCADDRESS << 1 | 0); |
| madcowswe | 0:0bbf2f16da9c | 17 | i2c.write(0); |
| madcowswe | 0:0bbf2f16da9c | 18 | i2c.stop(); |
| madcowswe | 0:0bbf2f16da9c | 19 | |
| madcowswe | 0:0bbf2f16da9c | 20 | //Read the axis data |
| madcowswe | 0:0bbf2f16da9c | 21 | i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3); |
| madcowswe | 0:0bbf2f16da9c | 22 | |
| madcowswe | 0:0bbf2f16da9c | 23 | for (int i = 0; i < 3; i++) { |
| madcowswe | 0:0bbf2f16da9c | 24 | returnbuff[i] = returnbuff[i] << 2; |
| madcowswe | 0:0bbf2f16da9c | 25 | returnbuff[i] /= 4; |
| madcowswe | 0:0bbf2f16da9c | 26 | } |
| madcowswe | 0:0bbf2f16da9c | 27 | |
| madcowswe | 0:0bbf2f16da9c | 28 | } |
| madcowswe | 0:0bbf2f16da9c | 29 | |
| madcowswe | 0:0bbf2f16da9c | 30 | void calibgyro() { |
| madcowswe | 0:0bbf2f16da9c | 31 | |
| madcowswe | 0:0bbf2f16da9c | 32 | //activating the wiimotion + |
| madcowswe | 0:0bbf2f16da9c | 33 | i2c.start(); |
| madcowswe | 0:0bbf2f16da9c | 34 | //device address | write |
| madcowswe | 0:0bbf2f16da9c | 35 | i2c.write(0x53 << 1 | 0); |
| madcowswe | 0:0bbf2f16da9c | 36 | //register address |
| madcowswe | 0:0bbf2f16da9c | 37 | i2c.write(0xfe); |
| madcowswe | 0:0bbf2f16da9c | 38 | //data |
| madcowswe | 0:0bbf2f16da9c | 39 | i2c.write(0x04); |
| madcowswe | 0:0bbf2f16da9c | 40 | i2c.stop(); |
| madcowswe | 0:0bbf2f16da9c | 41 | |
| madcowswe | 0:0bbf2f16da9c | 42 | for (int i = 0; i < 100; i++) { |
| madcowswe | 0:0bbf2f16da9c | 43 | //sending a 0x00 to flag that we want data |
| madcowswe | 0:0bbf2f16da9c | 44 | i2c.start(); |
| madcowswe | 0:0bbf2f16da9c | 45 | //device address | write (note new address) |
| madcowswe | 0:0bbf2f16da9c | 46 | i2c.write(0x52 << 1 | 0); |
| madcowswe | 0:0bbf2f16da9c | 47 | //send 0x00 |
| madcowswe | 0:0bbf2f16da9c | 48 | i2c.write(0x00); |
| madcowswe | 0:0bbf2f16da9c | 49 | i2c.stop(); |
| madcowswe | 0:0bbf2f16da9c | 50 | |
| madcowswe | 0:0bbf2f16da9c | 51 | //reading the data |
| madcowswe | 0:0bbf2f16da9c | 52 | char wmpdata[6]; |
| madcowswe | 0:0bbf2f16da9c | 53 | i2c.read(0x52 << 1 | 1, wmpdata, 6); |
| madcowswe | 0:0bbf2f16da9c | 54 | //pc.printf("%x %x %x %x %x %x\r\n", wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]); |
| madcowswe | 0:0bbf2f16da9c | 55 | |
| madcowswe | 0:0bbf2f16da9c | 56 | gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 100; |
| madcowswe | 0:0bbf2f16da9c | 57 | gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 100; |
| madcowswe | 0:0bbf2f16da9c | 58 | gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 100; |
| madcowswe | 0:0bbf2f16da9c | 59 | |
| madcowswe | 0:0bbf2f16da9c | 60 | wait(0.05); |
| madcowswe | 0:0bbf2f16da9c | 61 | |
| madcowswe | 0:0bbf2f16da9c | 62 | } |
| madcowswe | 0:0bbf2f16da9c | 63 | |
| madcowswe | 0:0bbf2f16da9c | 64 | } |
| madcowswe | 0:0bbf2f16da9c | 65 | |
| madcowswe | 0:0bbf2f16da9c | 66 | void getgyros(float gyrodata[3]) { |
| madcowswe | 0:0bbf2f16da9c | 67 | //sending a 0x00 to flag that we want data |
| madcowswe | 0:0bbf2f16da9c | 68 | i2c.start(); |
| madcowswe | 0:0bbf2f16da9c | 69 | //device address | write (note new address) |
| madcowswe | 0:0bbf2f16da9c | 70 | i2c.write(0x52 << 1 | 0); |
| madcowswe | 0:0bbf2f16da9c | 71 | //send 0x00 |
| madcowswe | 0:0bbf2f16da9c | 72 | i2c.write(0x00); |
| madcowswe | 0:0bbf2f16da9c | 73 | i2c.stop(); |
| madcowswe | 0:0bbf2f16da9c | 74 | |
| madcowswe | 0:0bbf2f16da9c | 75 | //reading the data |
| madcowswe | 0:0bbf2f16da9c | 76 | char wmpdata[6]; |
| madcowswe | 0:0bbf2f16da9c | 77 | i2c.read(0x52 << 1 | 1, wmpdata, 6); |
| madcowswe | 0:0bbf2f16da9c | 78 | |
| madcowswe | 0:0bbf2f16da9c | 79 | //detect if we ever went into fast mode |
| madcowswe | 0:0bbf2f16da9c | 80 | bool fastdiscard = !(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[3] & 0x01); |
| madcowswe | 0:0bbf2f16da9c | 81 | |
| madcowswe | 0:0bbf2f16da9c | 82 | if (fastdiscard) |
| madcowswe | 0:0bbf2f16da9c | 83 | myled = 1; |
| madcowswe | 0:0bbf2f16da9c | 84 | else { |
| madcowswe | 0:0bbf2f16da9c | 85 | gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]); |
| madcowswe | 0:0bbf2f16da9c | 86 | gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]); |
| madcowswe | 0:0bbf2f16da9c | 87 | gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]); |
| madcowswe | 0:0bbf2f16da9c | 88 | } |
| madcowswe | 0:0bbf2f16da9c | 89 | |
| madcowswe | 0:0bbf2f16da9c | 90 | //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]); |
| madcowswe | 0:0bbf2f16da9c | 91 | |
| madcowswe | 0:0bbf2f16da9c | 92 | //wait(0.05); |
| madcowswe | 0:0bbf2f16da9c | 93 | |
| madcowswe | 0:0bbf2f16da9c | 94 | } |
| madcowswe | 0:0bbf2f16da9c | 95 | |
| madcowswe | 0:0bbf2f16da9c | 96 | #ifdef ALTSENSOR |
| madcowswe | 0:0bbf2f16da9c | 97 | float getaltsensor() { |
| madcowswe | 0:0bbf2f16da9c | 98 | return altsensor * |
| madcowswe | 0:0bbf2f16da9c | 99 | } |
| madcowswe | 0:0bbf2f16da9c | 100 | #endif |