bez sterowania
Dependencies: FastPWM mbed-src
Fork of 2015_01_29_quadro2 by
Offsets.cpp
- Committer:
- Michu90
- Date:
- 2015-02-10
- Revision:
- 10:605b0bfadc2e
- Parent:
- 9:4e94a16ca90c
File content as of revision 10:605b0bfadc2e:
/* 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,float *katmagneto, Serial &serial, IMU &imu) { float offsetGyr[3]; char znak; char buff[120]; int i; float d[9]; float yaw; 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 a ...\n\r"); serial.printf(buff); wait(3); }while(znak!='a'); 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]; yaw = yaw + atan2(d[7],d[6])+4.98333*M_PI/180; } offsetGyr[0]/=1000; offsetGyr[1]/=1000; offsetGyr[2]/=1000; yaw = yaw/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]; *(katmagneto+0) = yaw; sprintf(buff, "Gyroscope offsets in DPS:\n\r"); serial.printf(buff); sprintf(buff, "offsetGyro[0]: %f,offsetGyro[1]: %f,offsetGyro[2]: %f,offsetMagneto: %f\n\r",(offsetGyr[0]*180/M_PI),(offsetGyr[1]*180/M_PI),(offsetGyr[2]*180/M_PI),yaw*180/M_PI); serial.printf(buff); sprintf(buff, "Press b to accept\n\r"); serial.printf(buff); do{ if(serial.readable()){ znak=serial.getc(); } sprintf(buff, "Press b ...\n\r"); serial.printf(buff); wait(3); }while(znak!='b'); 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]; } void Offsets::offsetMagneto(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]; }