//event for TinkerForge IMU
public void OrientCB(BrickIMUV2 sender, short heading, short roll, short pitch)
{
if (isHeadingCorrectionFromBrick)
{
mf.ahrs.correctionHeadingX16 = heading;
}
}
private static string UID = "XXYYZZ"; // Change XXYYZZ to the UID of your IMU Brick 2.0
// Callback function for all data callback
static void AllDataCB(BrickIMUV2 sender, short[] acceleration, short[] magneticField,
short[] angularVelocity, short[] eulerAngle, short[] quaternion,
short[] linearAcceleration, short[] gravityVector,
short temperature, byte calibrationStatus)
{
Console.WriteLine("Acceleration [X]: " + acceleration[0] / 100.0 + " m/s²");
Console.WriteLine("Acceleration [Y]: " + acceleration[1] / 100.0 + " m/s²");
Console.WriteLine("Acceleration [Z]: " + acceleration[2] / 100.0 + " m/s²");
Console.WriteLine("Magnetic Field [X]: " + magneticField[0] / 16.0 + " µT");
Console.WriteLine("Magnetic Field [Y]: " + magneticField[1] / 16.0 + " µT");
Console.WriteLine("Magnetic Field [Z]: " + magneticField[2] / 16.0 + " µT");
Console.WriteLine("Angular Velocity [X]: " + angularVelocity[0] / 16.0 + " °/s");
Console.WriteLine("Angular Velocity [Y]: " + angularVelocity[1] / 16.0 + " °/s");
Console.WriteLine("Angular Velocity [Z]: " + angularVelocity[2] / 16.0 + " °/s");
Console.WriteLine("Euler Angle [Heading]: " + eulerAngle[0] / 16.0 + " °");
Console.WriteLine("Euler Angle [Roll]: " + eulerAngle[1] / 16.0 + " °");
Console.WriteLine("Euler Angle [Pitch]: " + eulerAngle[2] / 16.0 + " °");
Console.WriteLine("Quaternion [W]: " + quaternion[0] / 16383.0);
Console.WriteLine("Quaternion [X]: " + quaternion[1] / 16383.0);
Console.WriteLine("Quaternion [Y]: " + quaternion[2] / 16383.0);
Console.WriteLine("Quaternion [Z]: " + quaternion[3] / 16383.0);
Console.WriteLine("Linear Acceleration [X]: " + linearAcceleration[0] / 100.0 + " m/s²");
Console.WriteLine("Linear Acceleration [Y]: " + linearAcceleration[1] / 100.0 + " m/s²");
Console.WriteLine("Linear Acceleration [Z]: " + linearAcceleration[2] / 100.0 + " m/s²");
Console.WriteLine("Gravity Vector [X]: " + gravityVector[0] / 100.0 + " m/s²");
Console.WriteLine("Gravity Vector [Y]: " + gravityVector[1] / 100.0 + " m/s²");
Console.WriteLine("Gravity Vector [Z]: " + gravityVector[2] / 100.0 + " m/s²");
Console.WriteLine("Temperature: " + temperature + " °C");
Console.WriteLine("Calibration Status: " + Convert.ToString(calibrationStatus, 2));
Console.WriteLine("");
}
private static string UID = "XXYYZZ"; // Change XXYYZZ to the UID of your IMU Brick 2.0
// Callback function for quaternion callback
static void QuaternionCB(BrickIMUV2 sender, short w, short x, short y, short z)
{
Console.WriteLine("Quaternion [W]: " + w / 16383.0);
Console.WriteLine("Quaternion [X]: " + x / 16383.0);
Console.WriteLine("Quaternion [Y]: " + y / 16383.0);
Console.WriteLine("Quaternion [Z]: " + z / 16383.0);
Console.WriteLine("");
}
//event for TinkerForge IMU
public void OrientCB(BrickIMUV2 sender, short heading, short roll, short pitch)
{
if (isHeadingBrick)
{
mf.mc.prevGyroHeading = mf.mc.gyroHeading;
mf.mc.gyroHeading = heading;
}
if (isRollBrick)
{
mf.mc.rollRaw = roll;
}
}
//constructor
public CAHRS(FormGPS _f)
{
mf = _f;
//non GPS AHRS sensors
isHeadingCorrectionFromAutoSteer = Properties.Settings.Default.setIMU_isHeadingCorrectionFromAutoSteer;
isHeadingCorrectionFromBrick = Properties.Settings.Default.setIMU_isHeadingCorrectionFromBrick;
//isHeadingCorrectionFromExtUDP = Properties.Settings.Default.setIMU_isHeadingCorrectionFromExtUDP;
isRollFromAutoSteer = Properties.Settings.Default.setIMU_isRollFromAutoSteer;
isRollFromAVR = Properties.Settings.Default.setIMU_isRollFromAVR;
isRollFromOGI = Properties.Settings.Default.setIMU_isRollFromOGI;
rollZeroX16 = Properties.Settings.Default.setIMU_rollZeroX16;
pitchZeroX16 = Properties.Settings.Default.setIMU_pitchZeroX16;
isAutoSteerAuto = Properties.Settings.Default.setAS_isAutoSteerAutoOn;
fusionWeight = Properties.Settings.Default.setIMU_fusionWeight;
//usb IMU Tinker
if (isHeadingCorrectionFromBrick)
{
ipcon = new IPConnection(); // Create IP connection
imu = new BrickIMUV2(CAHRS.UID, ipcon); // Create device object
try
{
ipcon.Connect(HOST, PORT); // Connect to brickd - daemon
// Don't use device before ipcon is connected
// Register Orientation callback in AHRS class
imu.OrientationCallback += OrientCB;
// Set period for Orientation callback to 0.1s (100ms)
imu.SetOrientationPeriod(100);
//set the mode with mag
imu.SetSensorFusionMode(1);
imu.LedsOff();
}
catch
{
}
}
}
static void Main()
{
IPConnection ipcon = new IPConnection(); // Create IP connection
BrickIMUV2 imu = new BrickIMUV2(UID, ipcon); // Create device object
ipcon.Connect(HOST, PORT); // Connect to brickd
// Don't use device before ipcon is connected
// Register quaternion callback to function QuaternionCB
imu.QuaternionCallback += QuaternionCB;
// Set period for quaternion callback to 0.1s (100ms)
imu.SetQuaternionPeriod(100);
Console.WriteLine("Press enter to exit");
Console.ReadLine();
ipcon.Disconnect();
}
//constructor
public CAHRS(FormGPS _f)
{
mf = _f;
//non GPS AHRS sensors
isHeadingBNO = Properties.Settings.Default.setIMU_isHeadingFromBNO;
isHeadingBrick = Properties.Settings.Default.setIMU_isHeadingFromBrick;
isHeadingPAOGI = Properties.Settings.Default.setIMU_isHeadingFromPAOGI;
isRollDogs = Properties.Settings.Default.setIMU_isRollFromDogs;
isRollBrick = Properties.Settings.Default.setIMU_isRollFromBrick;
isRollPAOGI = Properties.Settings.Default.setIMU_isRollFromPAOGI;
rollZero = Properties.Settings.Default.setIMU_rollZero;
pitchZero = Properties.Settings.Default.setIMU_pitchZero;
//usb IMU Tinker
if (isHeadingBrick | isRollBrick)
{
ipcon = new IPConnection(); // Create IP connection
imu = new BrickIMUV2(CAHRS.UID, ipcon); // Create device object
try
{
ipcon.Connect(HOST, PORT); // Connect to brickd - daemon
// Don't use device before ipcon is connected
// Register Orientation callback in AHRS class
imu.OrientationCallback += OrientCB;
// Set period for Orientation callback to 0.1s (100ms)
imu.SetOrientationPeriod(200);
//set the mode without mag
imu.SetSensorFusionMode(1);
imu.LedsOff();
}
catch
{
}
}
}
private static string UID = "XXYYZZ"; // Change XXYYZZ to the UID of your IMU Brick 2.0
static void Main()
{
IPConnection ipcon = new IPConnection(); // Create IP connection
BrickIMUV2 imu = new BrickIMUV2(UID, ipcon); // Create device object
ipcon.Connect(HOST, PORT); // Connect to brickd
// Don't use device before ipcon is connected
// Get current quaternion
short w, x, y, z;
imu.GetQuaternion(out w, out x, out y, out z);
Console.WriteLine("Quaternion [W]: " + w / 16383.0);
Console.WriteLine("Quaternion [X]: " + x / 16383.0);
Console.WriteLine("Quaternion [Y]: " + y / 16383.0);
Console.WriteLine("Quaternion [Z]: " + z / 16383.0);
Console.WriteLine("Press enter to exit");
Console.ReadLine();
ipcon.Disconnect();
}
