Oskar Weigl
/
Quadcopter_copy
Very early flyable code.
Diff: Sensors.h
- Revision:
- 0:9fcb3bf5c231
diff -r 000000000000 -r 9fcb3bf5c231 Sensors.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensors.h Sat Oct 01 12:57:23 2011 +0000 @@ -0,0 +1,98 @@ + +//In here be functions related to Sensors + +// +void setupaccel() { + i2c.start(); + i2c.write(ACCADDRESS << 1 | 0); + i2c.write(7); + i2c.write(1); + i2c.stop(); +} + +// +void getaccel(signed char returnbuff[3]) { + i2c.start(); + i2c.write(ACCADDRESS << 1 | 0); + i2c.write(0); + i2c.stop(); + + //Read the axis data + i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3); + + for (int i = 0; i < 3; i++) { + returnbuff[i] = returnbuff[i] << 2; + returnbuff[i] /= 4; + } + +} + +void calibgyro() { + + //i2c.frequency(9600); + + //activating the wiimotion + + i2c.start(); + //device address | write + i2c.write(0x53 << 1 | 0); + //register address + i2c.write(0xfe); + //data + i2c.write(0x04); + i2c.stop(); + + for (int i = 0; i < 10; i++) { + //sending a 0x00 to flag that we want data + i2c.start(); + //device address | write (note new address) + i2c.write(0x52 << 1 | 0); + //send 0x00 + i2c.write(0x00); + i2c.stop(); + + //reading the data + char wmpdata[6]; + i2c.read(0x52 << 1 | 1, wmpdata, 6); + pc.printf("%x %x %x %x %x %x\r\n", wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]); + + gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 10; + gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 10; + gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 10; + + wait(0.05); + + } + +} + +void getgyros(float gyrodata[3]) { + //sending a 0x00 to flag that we want data + i2c.start(); + //device address | write (note new address) + i2c.write(0x52 << 1 | 0); + //send 0x00 + i2c.write(0x00); + i2c.stop(); + + //reading the data + char wmpdata[6]; + i2c.read(0x52 << 1 | 1, wmpdata, 6); + + gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]); + gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]); + gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]); + + //detect if we ever went into fast mode + if (!(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[5] & 0x02)) myled = 1; + + //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]); + + //wait(0.05); + +} + +#ifdef ALTSENSOR +float getaltsensor() { + return altsensor * +} +#endif \ No newline at end of file