I think sensor interface is quite straightforward already, but I don’t understand these functions, I’ve just copied them form the examples:
initRelativeZero()
initAbsoluteZero()
needsAbsoluteZeroSearch()
I’ve written init(), getAngle and getVelocity functions:
float MpuSensor::getAngle(){
// get the position value directly from the sensor
// for example
accelgyro1.getMotion6(&AccelX, &AccelY, &AccelZ, &GyroX, &GyroY, &GyroZ);
uint32_t Now = micros();
float deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
velocity = double(GyroZ) / 32768.0 * 2000.0 / 180.0 * 3.141592;
current_angle += velocity * deltat;
return current_angle;
getVelocity is simlar.
Also I don’t understand in which order getVelocity or getAngle is called. My sensor returns velocity and then I calculate angle. Can I get sensor measurements only in getVelocity and calculate the angle in getAngle? But if getVelocity is called after getAngle (or never called at all), getAngle won’t have actual measurments and will return a calculation based on the previous one.