bez sterowania
Dependencies: FastPWM mbed-src
Fork of 2015_01_29_quadro2 by
Offsets.cpp@5:c3caf8b83e6b, 2014-12-19 (annotated)
- Committer:
- Michu90
- Date:
- Fri Dec 19 13:33:21 2014 +0000
- Revision:
- 5:c3caf8b83e6b
- Child:
- 9:4e94a16ca90c
a
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Michu90 | 5:c3caf8b83e6b | 1 | /* |
Michu90 | 5:c3caf8b83e6b | 2 | 2014_12_15 |
Michu90 | 5:c3caf8b83e6b | 3 | Author: Michał Szewc |
Michu90 | 5:c3caf8b83e6b | 4 | Free to use |
Michu90 | 5:c3caf8b83e6b | 5 | */ |
Michu90 | 5:c3caf8b83e6b | 6 | |
Michu90 | 5:c3caf8b83e6b | 7 | #include "Offsets.h" |
Michu90 | 5:c3caf8b83e6b | 8 | |
Michu90 | 5:c3caf8b83e6b | 9 | |
Michu90 | 5:c3caf8b83e6b | 10 | Offsets::Offsets() |
Michu90 | 5:c3caf8b83e6b | 11 | { |
Michu90 | 5:c3caf8b83e6b | 12 | reset(); |
Michu90 | 5:c3caf8b83e6b | 13 | } |
Michu90 | 5:c3caf8b83e6b | 14 | |
Michu90 | 5:c3caf8b83e6b | 15 | Offsets::~Offsets(void) |
Michu90 | 5:c3caf8b83e6b | 16 | { |
Michu90 | 5:c3caf8b83e6b | 17 | } |
Michu90 | 5:c3caf8b83e6b | 18 | |
Michu90 | 5:c3caf8b83e6b | 19 | void Offsets::reset() |
Michu90 | 5:c3caf8b83e6b | 20 | { |
Michu90 | 5:c3caf8b83e6b | 21 | i=0; |
Michu90 | 5:c3caf8b83e6b | 22 | } |
Michu90 | 5:c3caf8b83e6b | 23 | |
Michu90 | 5:c3caf8b83e6b | 24 | |
Michu90 | 5:c3caf8b83e6b | 25 | void Offsets::setOffsets(float *o, Serial &serial, IMU &imu) |
Michu90 | 5:c3caf8b83e6b | 26 | { |
Michu90 | 5:c3caf8b83e6b | 27 | float offsetGyr[3]; |
Michu90 | 5:c3caf8b83e6b | 28 | char znak; |
Michu90 | 5:c3caf8b83e6b | 29 | char buff[120]; |
Michu90 | 5:c3caf8b83e6b | 30 | int i; |
Michu90 | 5:c3caf8b83e6b | 31 | float d[9]; |
Michu90 | 5:c3caf8b83e6b | 32 | |
Michu90 | 5:c3caf8b83e6b | 33 | sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press o\n\r"); |
Michu90 | 5:c3caf8b83e6b | 34 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 35 | do{ |
Michu90 | 5:c3caf8b83e6b | 36 | if(serial.readable()){ |
Michu90 | 5:c3caf8b83e6b | 37 | znak=serial.getc(); |
Michu90 | 5:c3caf8b83e6b | 38 | } |
Michu90 | 5:c3caf8b83e6b | 39 | sprintf(buff, "Still waiting for o ...\n\r"); |
Michu90 | 5:c3caf8b83e6b | 40 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 41 | wait(3); |
Michu90 | 5:c3caf8b83e6b | 42 | }while(znak!='o'); |
Michu90 | 5:c3caf8b83e6b | 43 | znak=0; |
Michu90 | 5:c3caf8b83e6b | 44 | sprintf(buff, "Thanks! Gyroscope offsets are being set\n\r"); |
Michu90 | 5:c3caf8b83e6b | 45 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 46 | for (i=0;i<1000;i++){ |
Michu90 | 5:c3caf8b83e6b | 47 | imu.readData(d); |
Michu90 | 5:c3caf8b83e6b | 48 | offsetGyr[0]+=d[0]; |
Michu90 | 5:c3caf8b83e6b | 49 | offsetGyr[1]+=d[1]; |
Michu90 | 5:c3caf8b83e6b | 50 | offsetGyr[2]+=d[2]; |
Michu90 | 5:c3caf8b83e6b | 51 | } |
Michu90 | 5:c3caf8b83e6b | 52 | offsetGyr[0]/=1000; |
Michu90 | 5:c3caf8b83e6b | 53 | offsetGyr[1]/=1000; |
Michu90 | 5:c3caf8b83e6b | 54 | offsetGyr[2]/=1000; |
Michu90 | 5:c3caf8b83e6b | 55 | |
Michu90 | 5:c3caf8b83e6b | 56 | //o[0] = offsetGyr[0]; |
Michu90 | 5:c3caf8b83e6b | 57 | //o[1] = offsetGyr[1]; |
Michu90 | 5:c3caf8b83e6b | 58 | //o[2] = offsetGyr[2]; |
Michu90 | 5:c3caf8b83e6b | 59 | // lub tak: |
Michu90 | 5:c3caf8b83e6b | 60 | *(o+0) = offsetGyr[0]; |
Michu90 | 5:c3caf8b83e6b | 61 | *(o+1) = offsetGyr[1]; |
Michu90 | 5:c3caf8b83e6b | 62 | *(o+2) = offsetGyr[2]; |
Michu90 | 5:c3caf8b83e6b | 63 | |
Michu90 | 5:c3caf8b83e6b | 64 | sprintf(buff, "Gyroscope offsets in DPS:\n\r"); |
Michu90 | 5:c3caf8b83e6b | 65 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 66 | sprintf(buff, "offsetGyro[0]: %f,offsetGyro[1]: %f,offsetGyro[2]: %f,\n\r",(offsetGyr[0]*180/M_PI),(offsetGyr[1]*180/M_PI),(offsetGyr[2]*180/M_PI)); |
Michu90 | 5:c3caf8b83e6b | 67 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 68 | sprintf(buff, "Press e to accept\n\r"); |
Michu90 | 5:c3caf8b83e6b | 69 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 70 | do{ |
Michu90 | 5:c3caf8b83e6b | 71 | if(serial.readable()){ |
Michu90 | 5:c3caf8b83e6b | 72 | znak=serial.getc(); |
Michu90 | 5:c3caf8b83e6b | 73 | } |
Michu90 | 5:c3caf8b83e6b | 74 | sprintf(buff, "Press e ...\n\r"); |
Michu90 | 5:c3caf8b83e6b | 75 | serial.printf(buff); |
Michu90 | 5:c3caf8b83e6b | 76 | wait(3); |
Michu90 | 5:c3caf8b83e6b | 77 | }while(znak!='e'); |
Michu90 | 5:c3caf8b83e6b | 78 | znak=0; |
Michu90 | 5:c3caf8b83e6b | 79 | return; |
Michu90 | 5:c3caf8b83e6b | 80 | |
Michu90 | 5:c3caf8b83e6b | 81 | |
Michu90 | 5:c3caf8b83e6b | 82 | } |
Michu90 | 5:c3caf8b83e6b | 83 | |
Michu90 | 5:c3caf8b83e6b | 84 | |
Michu90 | 5:c3caf8b83e6b | 85 | void Offsets::offsetData(float *d, float *D, float *O) |
Michu90 | 5:c3caf8b83e6b | 86 | { |
Michu90 | 5:c3caf8b83e6b | 87 | /*for (int i=0; i<3; i++) { |
Michu90 | 5:c3caf8b83e6b | 88 | O[i] = d[i]-D[i]; |
Michu90 | 5:c3caf8b83e6b | 89 | }*/ |
Michu90 | 5:c3caf8b83e6b | 90 | O[0] = d[0]-D[0]; |
Michu90 | 5:c3caf8b83e6b | 91 | O[1] = d[1]-D[1]; |
Michu90 | 5:c3caf8b83e6b | 92 | O[2] = d[2]-D[2]; |
Michu90 | 5:c3caf8b83e6b | 93 | } |
Michu90 | 5:c3caf8b83e6b | 94 | |
Michu90 | 5:c3caf8b83e6b | 95 | void Offsets::offsetData2(double *d, float *D, float *O) |
Michu90 | 5:c3caf8b83e6b | 96 | { |
Michu90 | 5:c3caf8b83e6b | 97 | /*for (int i=0; i<3; i++) { |
Michu90 | 5:c3caf8b83e6b | 98 | O[i] = d[i]-D[i]; |
Michu90 | 5:c3caf8b83e6b | 99 | }*/ |
Michu90 | 5:c3caf8b83e6b | 100 | O[0] = d[0]-D[0]; |
Michu90 | 5:c3caf8b83e6b | 101 | O[1] = d[1]-D[1]; |
Michu90 | 5:c3caf8b83e6b | 102 | O[2] = d[2]-D[2]; |
Michu90 | 5:c3caf8b83e6b | 103 | } |