Examples with TimestampedData com.qualcomm.robotcore.hardware.TimestampedData used on opensource projects

Example 1 with TimestampedData

use of com.qualcomm.robotcore.hardware.TimestampedData in project robotcode by OutoftheBoxFTC.

the class ModernRoboticsI2cGyro method getAngularVelocity.

@Override
public AngularVelocity getAngularVelocity(AngleUnit unit) {
    TimestampedData data = this.deviceClient.readTimeStamped(Register.RAW_X_VAL.bVal, 3 * 2);
    int rawX = TypeConversion.byteArrayToShort(data.data, 0, ByteOrder.LITTLE_ENDIAN);
    int rawY = TypeConversion.byteArrayToShort(data.data, 2, ByteOrder.LITTLE_ENDIAN);
    int rawZ = TypeConversion.byteArrayToShort(data.data, 4, ByteOrder.LITTLE_ENDIAN);
    float degPerSecondX = rawX * degreesPerSecondPerDigit;
    float degPerSecondY = rawY * degreesPerSecondPerDigit;
    float degPerSecondZ = rawZ * degreesPerSecondPerDigit;
    return new AngularVelocity(AngleUnit.DEGREES, degPerSecondX, degPerSecondY, degPerSecondZ, data.nanoTime).toAngleUnit(unit);
}

Example 2 with TimestampedData

use of com.qualcomm.robotcore.hardware.TimestampedData in project robotcode by OutoftheBoxFTC.

the class ModernRoboticsI2cGyro method getAngularOrientation.

@Override
public Orientation getAngularOrientation(AxesReference reference, AxesOrder order, org.firstinspires.ftc.robotcore.external.navigation.AngleUnit angleUnit) {
    TimestampedData data = this.deviceClient.readTimeStamped(Register.INTEGRATED_Z_VALUE.bVal, 2);
    int integratedZ = TypeConversion.byteArrayToShort(data.data, ByteOrder.LITTLE_ENDIAN);
    float cartesian = AngleUnit.normalizeDegrees(degreesZFromIntegratedZ(integratedZ));
    return new Orientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES, cartesian, 0, 0, data.nanoTime).toAxesReference(reference).toAxesOrder(order).toAngleUnit(angleUnit);
}

Example 3 with TimestampedData

use of com.qualcomm.robotcore.hardware.TimestampedData in project robotcode by OutoftheBoxFTC.

the class NavxMicroNavigationSensor method getAngularVelocity.

@Override
public AngularVelocity getAngularVelocity(AngleUnit unit) {
    TimestampedData data = this.deviceClient.readTimeStamped(Register.GYRO_X_L.bVal, 3 * 2);
    float xDegPerSec = TypeConversion.byteArrayToShort(data.data, 0, ByteOrder.LITTLE_ENDIAN) * gyroScaleFactor;
    float yDegPerSec = TypeConversion.byteArrayToShort(data.data, 2, ByteOrder.LITTLE_ENDIAN) * gyroScaleFactor;
    float zDegPerSec = TypeConversion.byteArrayToShort(data.data, 4, ByteOrder.LITTLE_ENDIAN) * gyroScaleFactor;
    return new AngularVelocity(AngleUnit.DEGREES, xDegPerSec, yDegPerSec, zDegPerSec, data.nanoTime).toAngleUnit(unit);
}

Example 4 with TimestampedData

use of com.qualcomm.robotcore.hardware.TimestampedData in project robotcode by OutoftheBoxFTC.

the class LynxI2cDeviceSynchV2 method readTimeStamped.

/*
     * Supporting firmware version XX.XX.XX
     */
@Override
public synchronized TimestampedData readTimeStamped(final int ireg, final int creg) {
    LynxI2cDeviceSynchV2 deviceHavingProblems = null;
    boolean reportUnhealthy;
    try {
        return acquireI2cLockWhile(new Supplier<TimestampedData>() {

            @Override
            public TimestampedData get() throws InterruptedException, RobotCoreException, LynxNackException {
                LynxCommand<?> tx = new LynxI2cWriteReadMultipleBytesCommand(getModule(), bus, i2cAddr, ireg, creg);
                tx.send();
                readTimeStampedPlaceholder.reset();
                return pollForReadResult(i2cAddr, ireg, creg);
            }
        });
    } catch (InterruptedException | RobotCoreException | RuntimeException e) {
        handleException(e);
    } catch (LynxNackException e) {
        /*
             * This is a possible device problem, go ahead and tell makeFakeData to warn.
             */
        deviceHavingProblems = this;
        handleException(e);
    }
    return readTimeStampedPlaceholder.log(TimestampedI2cData.makeFakeData(deviceHavingProblems, getI2cAddress(), ireg, creg));
}

Example 5 with TimestampedData

use of com.qualcomm.robotcore.hardware.TimestampedData in project robotcode by OutoftheBoxFTC.

the class BNO055IMUImpl method getQuaternionOrientation.

public synchronized Quaternion getQuaternionOrientation() {
    // Ensure we can see the registers we need
    deviceClient.ensureReadWindow(new I2cDeviceSynch.ReadWindow(Register.QUA_DATA_W_LSB.bVal, 8, readMode), upperWindow);
    // Section 3.6.5.5 of BNO055 specification
    TimestampedData ts = deviceClient.readTimeStamped(Register.QUA_DATA_W_LSB.bVal, 8);
    VectorData vector = new VectorData(ts, (1 << 14));
    return new Quaternion(vector.next(), vector.next(), vector.next(), vector.next(), vector.data.nanoTime);
}