bez sterowania
Dependencies: FastPWM mbed-src
Fork of 2015_01_29_quadro2 by
Diff: Offsets.cpp
- Revision:
- 5:c3caf8b83e6b
- Child:
- 9:4e94a16ca90c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Offsets.cpp Fri Dec 19 13:33:21 2014 +0000 @@ -0,0 +1,103 @@ +/* +2014_12_15 +Author: Michał Szewc +Free to use +*/ + +#include "Offsets.h" + + +Offsets::Offsets() +{ + reset(); +} + +Offsets::~Offsets(void) +{ +} + +void Offsets::reset() +{ + i=0; +} + + +void Offsets::setOffsets(float *o, Serial &serial, IMU &imu) +{ + float offsetGyr[3]; + char znak; + char buff[120]; + int i; + float d[9]; + + sprintf(buff, "Hello!\n\rPlace Qudrocopter motionlessly and press o\n\r"); + serial.printf(buff); + do{ + if(serial.readable()){ + znak=serial.getc(); + } + sprintf(buff, "Still waiting for o ...\n\r"); + serial.printf(buff); + wait(3); + }while(znak!='o'); + znak=0; + sprintf(buff, "Thanks! Gyroscope offsets are being set\n\r"); + serial.printf(buff); + for (i=0;i<1000;i++){ + imu.readData(d); + offsetGyr[0]+=d[0]; + offsetGyr[1]+=d[1]; + offsetGyr[2]+=d[2]; + } + offsetGyr[0]/=1000; + offsetGyr[1]/=1000; + offsetGyr[2]/=1000; + + //o[0] = offsetGyr[0]; + //o[1] = offsetGyr[1]; + //o[2] = offsetGyr[2]; + // lub tak: + *(o+0) = offsetGyr[0]; + *(o+1) = offsetGyr[1]; + *(o+2) = offsetGyr[2]; + + sprintf(buff, "Gyroscope offsets in DPS:\n\r"); + serial.printf(buff); + 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)); + serial.printf(buff); + sprintf(buff, "Press e to accept\n\r"); + serial.printf(buff); + do{ + if(serial.readable()){ + znak=serial.getc(); + } + sprintf(buff, "Press e ...\n\r"); + serial.printf(buff); + wait(3); + }while(znak!='e'); + znak=0; + return; + + +} + + +void Offsets::offsetData(float *d, float *D, float *O) +{ + /*for (int i=0; i<3; i++) { + O[i] = d[i]-D[i]; + }*/ + O[0] = d[0]-D[0]; + O[1] = d[1]-D[1]; + O[2] = d[2]-D[2]; +} + +void Offsets::offsetData2(double *d, float *D, float *O) +{ + /*for (int i=0; i<3; i++) { + O[i] = d[i]-D[i]; + }*/ + O[0] = d[0]-D[0]; + O[1] = d[1]-D[1]; + O[2] = d[2]-D[2]; +} \ No newline at end of file