static async Task <int> Main(string[] args)
{
var a = new AbsArguments(args);
if (!a.IsValid())
{
Console.WriteLine($"Usage: {Process.GetCurrentProcess().ProcessName} <args>");
Console.WriteLine($"Possible args: {string.Join(", ", AbsArguments.GetPossibleArgs())}");
return(-1);
}
using (var source = new CancellationTokenSource())
{
Console.CancelKeyPress += (s, e) => source.Cancel();
try
{
Pi.Init <BootstrapWiringPi>();
if (a.Calibrate)
{
await CalibrateAsync(source.Token);
}
else
{
await RunAsync(a, source.Token);
}
return(0);
}
catch (TaskCanceledException)
{
Console.WriteLine("Bye!");
return(0);
}
catch (Exception e)
{
Console.WriteLine(e.GetType().FullName);
Console.WriteLine(e.Message);
Console.WriteLine(e.StackTrace);
return(-1);
}
}
}
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);
}
}
}