//static AHRS.MahonyAHRS AHRS = new AHRS.MahonyAHRS(1f / 256f, 5f); /// <summary> /// Main method. /// </summary> /// <param name="args"> /// Unused. /// </param> static void Main(string[] args) { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString()); try { // Connect to x-IMU Console.WriteLine("Searching for x-IMU..."); x_IMU_API.PortAssignment[] portAssignment = (new x_IMU_API.PortScanner(true, true)).Scan(); x_IMU_API.xIMUserial xIMUserial = new x_IMU_API.xIMUserial(portAssignment[0].PortName); xIMUserial.Open(); Console.WriteLine("Connected to x-IMU " + portAssignment[0].DeviceID + " on " + portAssignment[0].PortName + "."); // Show 3D cuboid forms Console.WriteLine("Showing 3D Cuboid forms..."); Form_3Dcuboid form_3DcuboidA = new Form_3Dcuboid(); Form_3Dcuboid form_3DcuboidB = new Form_3Dcuboid(new string[] { "Form_3Dcuboid/RightInv.png", "Form_3Dcuboid/LeftInv.png", "Form_3Dcuboid/BackInv.png", "Form_3Dcuboid/FrontInv.png", "Form_3Dcuboid/TopInv.png", "Form_3Dcuboid/BottomInv.png" }); form_3DcuboidA.Text += " A"; form_3DcuboidB.Text += " B"; BackgroundWorker backgroundWorkerA = new BackgroundWorker(); BackgroundWorker backgroundWorkerB = new BackgroundWorker(); backgroundWorkerA.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidA.ShowDialog(); }); backgroundWorkerB.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidB.ShowDialog(); }); backgroundWorkerA.RunWorkerAsync(); backgroundWorkerB.RunWorkerAsync(); // x-IMU data received events to update cuboid form xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(delegate(object s, x_IMU_API.QuaternionData e) { form_3DcuboidA.RotationMatrix = e.ConvertToRotationMatrix(); }); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(delegate(object s, x_IMU_API.CalInertialAndMagneticData e) { form_3DcuboidB.RotationMatrix = (new x_IMU_API.QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix(); }); // Algorithm uses IMU update method. Console.WriteLine("Algorithm running in IMU mode."); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); Console.WriteLine("Press any key to continue..."); Console.ReadKey(); // Algorithm uses AHRS update method. Console.WriteLine("Algorithm running in AHRS mode."); xIMUserial.CalInertialAndMagneticDataReceived -= new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS); } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }
/// <summary> /// Algorithm object. /// </summary> //static AHRS.MadgwickAHRS AHRS = new AHRS.MadgwickAHRS(1f / 256f, 0.1f); //static AHRS.MahonyAHRS AHRS = new AHRS.MahonyAHRS(1f / 256f, 5f); /// <summary> /// Main method. /// </summary> /// <param name="args"> /// Unused. /// </param> static void Main(string[] args) { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major.ToString() + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor.ToString()); try { // Connect to x-IMU Console.WriteLine("Searching for x-IMU..."); //x_IMU_API.PortAssignment[] portAssignment = (new x_IMU_API.PortScanner(true, true)).Scan(); //x_IMU_API.xIMUserial xIMUserial = new x_IMU_API.xIMUserial(portAssignment[0].PortName); //xIMUserial.Open(); //Console.WriteLine("Connected to x-IMU " + portAssignment[0].DeviceID + " on " + portAssignment[0].PortName + "."); // Show 3D cuboid forms Console.WriteLine("Showing 3D Cuboid forms..."); Form_3Dcuboid form_3DcuboidA = new Form_3Dcuboid(); //Form_3Dcuboid form_3DcuboidB = new Form_3Dcuboid(new string[] { "Form_3Dcuboid/RightInv.png", "Form_3Dcuboid/LeftInv.png", "Form_3Dcuboid/BackInv.png", "Form_3Dcuboid/FrontInv.png", "Form_3Dcuboid/TopInv.png", "Form_3Dcuboid/BottomInv.png" }); form_3DcuboidA.Text += " A"; //form_3DcuboidB.Text += " B"; BackgroundWorker backgroundWorkerA = new BackgroundWorker(); //BackgroundWorker backgroundWorkerB = new BackgroundWorker(); backgroundWorkerA.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidA.ShowDialog(); }); //backgroundWorkerB.DoWork += new DoWorkEventHandler(delegate { form_3DcuboidB.ShowDialog(); }); backgroundWorkerA.RunWorkerAsync(); //backgroundWorkerB.RunWorkerAsync(); // x-IMU data received events to update cuboid form //xIMUserial.QuaternionDataReceived += new x_IMU_API.xIMUserial.onQuaternionDataReceived(delegate(object s, x_IMU_API.QuaternionData e) { form_3DcuboidA.RotationMatrix = e.ConvertToRotationMatrix(); }); //xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(delegate(object s, x_IMU_API.CalInertialAndMagneticData e) { form_3DcuboidB.RotationMatrix = (new x_IMU_API.QuaternionData(AHRS.Quaternion)).ConvertToConjugate().ConvertToRotationMatrix(); }); // Algorithm uses IMU update method. Console.WriteLine("Algorithm running in IMU mode."); //xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); Console.WriteLine("Press any key to continue..."); Console.ReadKey(); // Algorithm uses AHRS update method. Console.WriteLine("Algorithm running in AHRS mode."); //xIMUserial.CalInertialAndMagneticDataReceived -= new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateIMU); //xIMUserial.CalInertialAndMagneticDataReceived += new x_IMU_API.xIMUserial.onCalInertialAndMagneticDataReceived(xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS); } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }
/// <summary> /// Main method. /// </summary> private static void Main() { Console.WriteLine(Assembly.GetExecutingAssembly().GetName().Name + " " + Assembly.GetExecutingAssembly().GetName().Version.Major + "." + Assembly.GetExecutingAssembly().GetName().Version.Minor); try { var xImUserial = new QuadRotorImu(_stopWatch); xImUserial.Open(); // Show 3D cuboid forms Console.WriteLine("Showing 3D Cuboid forms..."); _form3DcuboidB = new Form_3Dcuboid(new[] { "Form_3Dcuboid/RightInv.png", "Form_3Dcuboid/LeftInv.png", "Form_3Dcuboid/BackInv.png", "Form_3Dcuboid/FrontInv.png", "Form_3Dcuboid/TopInv.png", "Form_3Dcuboid/BottomInv.png" }); _form3DcuboidB.Text += " B"; _backgroundWorkerB.DoWork += delegate { _form3DcuboidB.ShowDialog(); }; _backgroundWorkerB.RunWorkerAsync(); // Algorithm uses AHRS update method. Console.WriteLine("Algorithm running in AHRS mode."); xImUserial.CalInertialAndMagneticDataReceived += xIMUserial_CalInertialAndMagneticDataReceived_updateAHRS; } catch (Exception ex) { Console.WriteLine("Error: " + ex.Message); } Console.WriteLine("Press any key to exit..."); Console.ReadKey(); }