Quadrocopter / 2015_04_17_quadro_bez_sterowania

bez sterowania

Dependencies:   FastPWM mbed-src

Fork of 2015_01_29_quadro2 by Quadrocopter

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?

UserRevisionLine numberNew 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 }