Oskar Weigl / Mbed 2 deprecated Quadcopter_copy

Very early flyable code.

Dependencies:   mbed RF12B

Sensors.h@0:9fcb3bf5c231, 2011-10-01 (annotated)

Committer:
madcowswe
Date:
Sat Oct 01 12:57:23 2011 +0000
Revision:
0:9fcb3bf5c231
This edit is for testing: not flyable

Who changed what in which revision?

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