Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
0
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
}
Ejemplo n.º 3
0
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;
}
Ejemplo n.º 4
0
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);
}
Ejemplo n.º 5
0
/* 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;
    }
}
Ejemplo n.º 6
0
/* 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; 
	}
}