private static Task CalibrateAsync(CancellationToken token)
{
var chip = new BNO055Sensor(Board.Peripherals, OperationMode.NDOF);
return(new Calibrator(chip).CalibrateAsync(token));
}
private static async Task RunAsync(AbsArguments args, CancellationToken token)
{
while (true)
{
var chip = new BNO055Sensor(Board.Peripherals, OperationMode.NDOF);
Console.WriteLine(chip.UnitSelection);
Double3 velocity = 0;
var last = DateTimeOffset.UtcNow;
while (true)
{
if (token.IsCancellationRequested)
{
return;
}
if (args.Quaternion)
{
Console.WriteLine($"Quaternion: {chip.ReadQuaternion()}");
}
if (args.RollPitchYaw)
{
var rpy = chip.ReadRollPitchYaw();
Console.WriteLine($"Roll pitch yaw: rad {rpy} deg {rpy * 180 / Math.PI}");
}
if (args.Euler)
{
Console.WriteLine($"Euler: {chip.ReadEulerData().ToString(" 000.00;-000.00")}");
}
if (args.Accel)
{
Console.WriteLine($"Accel: {chip.ReadAccel()}");
}
if (args.Mag)
{
Console.WriteLine($"Mag: {chip.ReadMag()}");
}
if (args.LinearAccel)
{
Console.WriteLine($"Linear Accel: {chip.ReadLinearAccel()}");
}
if (args.Compass)
{
var mag = chip.ReadMag();
var yaw = Math.Atan2(mag.Y, mag.X);
Console.WriteLine($"Compass: Rad:{yaw}, Deg:{yaw * 180 / Math.PI}");
}
if (args.Velocity)
{
var accel = chip.ReadLinearAccel();
var now = DateTimeOffset.UtcNow;
velocity = velocity + accel * (now - last).TotalSeconds;
last = now;
Console.WriteLine($"Velocity: {velocity}");
}
if (args.Gyro)
{
Console.WriteLine($"Gyro: {chip.ReadGyro()}");
}
if (args.Temp)
{
Console.WriteLine($"Temp: {chip.ReadTemp()}");
}
await Task.Delay(100, token);
}
}
}
public Calibrator(BNO055Sensor sensor)
{
_sensor = sensor;
}
