void calibrateGyro(float *xCalib, float *yCalib, float *zCalib, int scale) { int xGyro = 0; int yGyro = 0; int zGyro = 0; *xCalib = 0; *yCalib = 0; *zCalib = 0; uint8_t loopCount = 20; for (int i=0; i<loopCount; i++){ getGyroValues(&xGyro, &yGyro, &zGyro); *xCalib += xGyro; *yCalib += yGyro; *zCalib += zGyro; _delay_ms(5); } *xCalib = toDegSec(*xCalib/loopCount, scale); *yCalib = toDegSec(*yCalib/loopCount, scale); *zCalib = toDegSec(*zCalib/loopCount, scale); }
int Gyroscope::calibrate(int measurements){ //use this function in order to determine the offset and change GYRO_OFFSET accordingly if (!measurements) return -10000; //if a 0 was an argument, return a very high value, to avoid a division with 0 and indicate error long sum = 0; for (int i = 0; i < measurements; i++){ sum += getGyroValues(); delay(10); } return sum/measurements; //return the average }
void Gyroscope::updateDisplacement(){ //TO-DO: optimize volatile float gyroRate = 0; volatile int gyroValue = getGyroValues(); volatile short drift = GYRO_OFFSET - gyroValue; if (abs(drift) > GYRO_THRESHOLD){ gyroRate = (gyroValue - GYRO_OFFSET) * GYRO_SENSITIVITY; } volatile unsigned long now = millis(); _angularDisplacement += gyroRate / (1000 / (now - _prevSample)); _prevSample = now; }
void getGyroValues(float *x, float *y, float *z) { int xi = 0; int yi = 0; int zi = 0; getGyroValues( &xi, &yi, &zi); *x = toDegSec((float)xi, 500); *y = toDegSec((float)yi, 500); *z = toDegSec((float)zi, 500); }
/* based on http://www.pieter-jan.com/node/7 integration algorithm */ void Gyroscope::update(){ if (millis()- _prevSample > _samplingRate){ float gyroRate = 0; int gyroValue = getGyroValues(); short drift = _gyroOffset - gyroValue; if (abs(drift) > GYRO_THRESHOLD){ //if there has been a big enough drift (trying to contemplate for the noise) gyroRate = drift * GYRO_SENSITIVITY; } unsigned long now = millis(); _angularDisplacement += gyroRate / (1000.0 / (now - _prevSample)); _prevSample = now; } }
/* based on http://www.pieter-jan.com/node/7 integration algorithm */ void Gyroscope::update(){ if (millis()- _prevSample > _samplingRate){ float gyroRate = 0; int gyroValue = getGyroValues(); short drift = GYRO_OFFSET - gyroValue; if (abs(drift) > GYRO_THRESHOLD){ gyroRate = (gyroValue - GYRO_OFFSET) * GYRO_SENSITIVITY; } unsigned long now = millis(); _angularDisplacement += gyroRate / (1000 / (now - _prevSample)); _prevSample = now; } }