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;
}
}
