Ejemplo n.º 1
0
        public void Activate()
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                Enabled = false;
                return;
            }
            Enabled = true;

            startup = true;

            if (MainV2.comPort.MAV.cs.version > Version.Parse("3.2.1") &&
                MainV2.comPort.MAV.cs.firmware == MainV2.Firmwares.ArduCopter2)
            {
                QuickAPM25.Visible         = false;
                buttonAPMExternal.Visible  = false;
                buttonQuickPixhawk.Visible = false;
                label1.Visible             = false;
            }

            if (MainV2.comPort.MAV.cs.version >= Version.Parse("3.7.1") &&
                MainV2.comPort.MAV.cs.firmware == MainV2.Firmwares.ArduPlane ||
                Control.ModifierKeys == Keys.Control)
            {
                groupBoxonboardcalib.Visible = true;
                label4.Visible          = true;
                groupBoxmpcalib.Visible = true;
            }
            else if ((MainV2.comPort.MAV.cs.capabilities & (uint)MAVLink.MAV_PROTOCOL_CAPABILITY.COMPASS_CALIBRATION) == 0)
            {
                groupBoxonboardcalib.Visible = false;
                label4.Visible          = false;
                groupBoxmpcalib.Visible = true;
            }
            else
            {
                groupBoxonboardcalib.Visible = true;
                label4.Visible          = false;
                groupBoxmpcalib.Visible = false;
            }

            // General Compass Settings
            CHK_enablecompass.setup(1, 0, "MAG_ENABLE", MainV2.comPort.MAV.param);
            CHK_compass_learn.setup(1, 0, "COMPASS_LEARN", MainV2.comPort.MAV.param);
            if (MainV2.comPort.MAV.param["COMPASS_DEC"] != null)
            {
                var dec = MainV2.comPort.MAV.param["COMPASS_DEC"].Value * MathHelper.rad2deg;

                var min = (dec - (int)dec) * 60;

                TXT_declination_deg.Text = ((int)dec).ToString("0");
                TXT_declination_min.Text = min.ToString("0");
            }

            if (MainV2.comPort.MAV.param["COMPASS_AUTODEC"] != null)
            {
                CHK_autodec.Checked = MainV2.comPort.MAV.param["COMPASS_AUTODEC"].ToString() == "1" ? true : false;
            }


            // Compass 1 settings
            CHK_compass1_use.setup(1, 0, "COMPASS_USE", MainV2.comPort.MAV.param);
            CHK_compass1_external.setup(1, 0, "COMPASS_EXTERNAL", MainV2.comPort.MAV.param);
            CMB_compass1_orient.setup(ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_ORIENT",
                                                                                         MainV2.comPort.MAV.cs.firmware.ToString()), "COMPASS_ORIENT", MainV2.comPort.MAV.param);

            if (!MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS_X"))
            {
                Enabled = false;
                return;
            }

            int offset1_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS_X"];
            int offset1_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS_Y"];
            int offset1_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS_Z"];

            // Turn offsets red if any offset exceeds a threshold, or all values are 0 (not yet calibrated)
            if (absmax(offset1_x, offset1_y, offset1_z) > THRESHOLD_OFS_RED)
            {
                LBL_compass1_offset.ForeColor = Color.Red;
            }
            else if (absmax(offset1_x, offset1_y, offset1_z) > THRESHOLD_OFS_YELLOW)
            {
                LBL_compass1_offset.ForeColor = Color.Yellow;
            }
            else if (offset1_x == 0 && offset1_y == 0 & offset1_z == 0)
            {
                LBL_compass1_offset.ForeColor = Color.Red;
            }
            else
            {
                LBL_compass1_offset.ForeColor = Color.Green;
            }


            LBL_compass1_offset.Text = "OFFSETS  X: " +
                                       offset1_x.ToString() +
                                       ",   Y: " + offset1_y.ToString() +
                                       ",   Z: " + offset1_z.ToString();
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_MOT_X"))
            {
                LBL_compass1_mot.Text = "MOT          X: " +
                                        ((int)MainV2.comPort.MAV.param["COMPASS_MOT_X"]).ToString() +
                                        ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT_Y"]).ToString() +
                                        ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT_Z"]).ToString();
            }

            if (!MainV2.DisplayConfiguration.displayCompassConfiguration)
            {
                CHK_compass1_use.Enabled      = false;
                CHK_compass1_external.Enabled = false;
                CMB_compass1_orient.Enabled   = false;
            }

            // Compass 2 settings
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_EXTERN2"))
            {
                CHK_compass2_use.setup(1, 0, "COMPASS_USE2", MainV2.comPort.MAV.param);
                CHK_compass2_external.setup(1, 0, "COMPASS_EXTERN2", MainV2.comPort.MAV.param);
                CMB_compass2_orient.setup(ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_ORIENT2",
                                                                                             MainV2.comPort.MAV.cs.firmware.ToString()), "COMPASS_ORIENT2", MainV2.comPort.MAV.param);

                CMB_primary_compass.setup(typeof(CompassNumber), "COMPASS_PRIMARY", MainV2.comPort.MAV.param);

                int offset2_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_X"];
                int offset2_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_Y"];
                int offset2_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_Z"];

                if (absmax(offset2_x, offset2_y, offset2_z) > THRESHOLD_OFS_RED)
                {
                    LBL_compass2_offset.ForeColor = Color.Red;
                }
                else if (absmax(offset2_x, offset2_y, offset2_z) > THRESHOLD_OFS_YELLOW)
                {
                    LBL_compass2_offset.ForeColor = Color.Yellow;
                }
                else if (offset2_x == 0 && offset2_y == 0 & offset2_z == 0)
                {
                    LBL_compass2_offset.ForeColor = Color.Red;
                }
                else
                {
                    LBL_compass2_offset.ForeColor = Color.Green;
                }


                LBL_compass2_offset.Text = "OFFSETS  X: " +
                                           offset2_x.ToString() +
                                           ",   Y: " + offset2_y.ToString() +
                                           ",   Z: " + offset2_z.ToString();
                if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_MOT2_X"))
                {
                    LBL_compass2_mot.Text = "MOT          X: " +
                                            ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_X"]).ToString() +
                                            ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_Y"]).ToString() +
                                            ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_Z"]).ToString();
                }

                if (!MainV2.DisplayConfiguration.displayCompassConfiguration)
                {
                    CHK_compass2_use.Enabled      = false;
                    CHK_compass2_external.Enabled = false;
                    CMB_compass2_orient.Enabled   = false;
                }
            }
            else
            {
                groupBoxCompass2.Hide();
            }

            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_EXTERN3"))
            {
                CHK_compass3_external.setup(1, 0, "COMPASS_EXTERN3", MainV2.comPort.MAV.param);
                CHK_compass3_use.setup(1, 0, "COMPASS_USE3", MainV2.comPort.MAV.param);
                CMB_compass3_orient.setup(ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_ORIENT3",
                                                                                             MainV2.comPort.MAV.cs.firmware.ToString()), "COMPASS_ORIENT3", MainV2.comPort.MAV.param);

                int offset3_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_X"];
                int offset3_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_Y"];
                int offset3_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_Z"];

                if (absmax(offset3_x, offset3_y, offset3_z) > THRESHOLD_OFS_RED)
                {
                    LBL_compass3_offset.ForeColor = Color.Red;
                }
                else if (absmax(offset3_x, offset3_y, offset3_z) > THRESHOLD_OFS_YELLOW)
                {
                    LBL_compass3_offset.ForeColor = Color.Yellow;
                }
                else if (offset3_x == 0 && offset3_y == 0 & offset3_z == 0)
                {
                    LBL_compass3_offset.ForeColor = Color.Red;
                }
                else
                {
                    LBL_compass3_offset.ForeColor = Color.Green;
                }


                LBL_compass3_offset.Text = "OFFSETS  X: " +
                                           offset3_x.ToString() +
                                           ",   Y: " + offset3_y.ToString() +
                                           ",   Z: " + offset3_z.ToString();
                if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_MOT3_X"))
                {
                    LBL_compass3_mot.Text = "MOT          X: " +
                                            ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_X"]).ToString() +
                                            ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_Y"]).ToString() +
                                            ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_Z"]).ToString();
                }

                if (!MainV2.DisplayConfiguration.displayCompassConfiguration)
                {
                    CHK_compass3_use.Enabled      = false;
                    CHK_compass3_external.Enabled = false;
                    CMB_compass3_orient.Enabled   = false;
                }
            }
            else
            {
                groupBoxCompass3.Hide();
            }

            mavlinkComboBoxfitness.setup(ParameterMetaDataRepository.GetParameterOptionsInt("COMPASS_CAL_FIT",
                                                                                            MainV2.comPort.MAV.cs.firmware.ToString()), "COMPASS_CAL_FIT", MainV2.comPort.MAV.param);

            ShowRelevantFields();

            if (!MainV2.DisplayConfiguration.displayCompassConfiguration)
            {
                CHK_enablecompass.Enabled   = false;
                CHK_compass_learn.Enabled   = false;
                CHK_autodec.Enabled         = false;
                CMB_primary_compass.Enabled = false;
            }

            startup = false;
        }
Ejemplo n.º 2
0
        public void Activate()
        {
            if (!MainV2.comPort.BaseStream.IsOpen)
            {
                Enabled = false;
                return;
            }
            Enabled = true;

            startup = true;

            // General Compass Settings
            CHK_enablecompass.setup(1, 0, "MAG_ENABLE", MainV2.comPort.MAV.param);
            CHK_compass_learn.setup(1, 0, "COMPASS_LEARN", MainV2.comPort.MAV.param);
            if (MainV2.comPort.MAV.param["COMPASS_DEC"] != null)
            {
                var dec = MainV2.comPort.MAV.param["COMPASS_DEC"].Value * rad2deg;

                var min = (dec - (int)dec) * 60;

                TXT_declination_deg.Text = ((int)dec).ToString("0");
                TXT_declination_min.Text = min.ToString("0");
            }

            if (MainV2.comPort.MAV.param["COMPASS_AUTODEC"] != null)
            {
                CHK_autodec.Checked = MainV2.comPort.MAV.param["COMPASS_AUTODEC"].ToString() == "1" ? true : false;
            }


            // Compass 1 settings
            CHK_compass1_use.setup(1, 0, "COMPASS_USE", MainV2.comPort.MAV.param);
            CHK_compass1_external.setup(1, 0, "COMPASS_EXTERNAL", MainV2.comPort.MAV.param);
            CMB_compass1_orient.setup(typeof(Common.Rotation), "COMPASS_ORIENT", MainV2.comPort.MAV.param);

            int offset1_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS_X"];
            int offset1_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS_Y"];
            int offset1_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS_Z"];

            // Turn offsets red if any offset exceeds a threshold, or all values are 0 (not yet calibrated)
            if (absmax(offset1_x, offset1_y, offset1_z) > THRESHOLD_OFS_RED)
            {
                LBL_compass1_offset.ForeColor = Color.Red;
            }
            else if (absmax(offset1_x, offset1_y, offset1_z) > THRESHOLD_OFS_YELLOW)
            {
                LBL_compass1_offset.ForeColor = Color.Yellow;
            }
            else if (offset1_x == 0 && offset1_y == 0 & offset1_z == 0)
            {
                LBL_compass1_offset.ForeColor = Color.Red;
            }
            else
            {
                LBL_compass1_offset.ForeColor = Color.Green;
            }


            LBL_compass1_offset.Text = "OFFSETS  X: " +
                                       offset1_x.ToString() +
                                       ",   Y: " + offset1_y.ToString() +
                                       ",   Z: " + offset1_z.ToString();
            LBL_compass1_mot.Text = "MOT          X: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT_X"]).ToString() +
                                    ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT_Y"]).ToString() +
                                    ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT_Z"]).ToString();


            // Compass 2 settings
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_EXTERN2"))
            {
                CHK_compass2_use.setup(1, 0, "COMPASS_USE2", MainV2.comPort.MAV.param);
                CHK_compass2_external.setup(1, 0, "COMPASS_EXTERN2", MainV2.comPort.MAV.param);
                CMB_compass2_orient.setup(typeof(Common.Rotation), "COMPASS_ORIENT2", MainV2.comPort.MAV.param);

                CMB_primary_compass.setup(typeof(CompassNumber), "COMPASS_PRIMARY", MainV2.comPort.MAV.param);

                int offset2_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_X"];
                int offset2_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_Y"];
                int offset2_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS2_Z"];

                if (absmax(offset2_x, offset2_y, offset2_z) > THRESHOLD_OFS_RED)
                {
                    LBL_compass2_offset.ForeColor = Color.Red;
                }
                else if (absmax(offset2_x, offset2_y, offset2_z) > THRESHOLD_OFS_YELLOW)
                {
                    LBL_compass2_offset.ForeColor = Color.Yellow;
                }
                else if (offset2_x == 0 && offset2_y == 0 & offset2_z == 0)
                {
                    LBL_compass2_offset.ForeColor = Color.Red;
                }
                else
                {
                    LBL_compass2_offset.ForeColor = Color.Green;
                }


                LBL_compass2_offset.Text = "OFFSETS  X: " +
                                           offset2_x.ToString() +
                                           ",   Y: " + offset2_y.ToString() +
                                           ",   Z: " + offset2_z.ToString();
                LBL_compass2_mot.Text = "MOT          X: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_X"]).ToString() +
                                        ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_Y"]).ToString() +
                                        ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT2_Z"]).ToString();
            }
            else
            {
                groupBoxCompass2.Hide();
            }

            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_EXTERN3"))
            {
                CHK_compass3_external.setup(1, 0, "COMPASS_EXTERN3", MainV2.comPort.MAV.param);
                CHK_compass3_use.setup(1, 0, "COMPASS_USE3", MainV2.comPort.MAV.param);
                CMB_compass3_orient.setup(typeof(Common.Rotation), "COMPASS_ORIENT3", MainV2.comPort.MAV.param);

                int offset3_x = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_X"];
                int offset3_y = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_Y"];
                int offset3_z = (int)MainV2.comPort.MAV.param["COMPASS_OFS3_Z"];

                if (absmax(offset3_x, offset3_y, offset3_z) > THRESHOLD_OFS_RED)
                {
                    LBL_compass3_offset.ForeColor = Color.Red;
                }
                else if (absmax(offset3_x, offset3_y, offset3_z) > THRESHOLD_OFS_YELLOW)
                {
                    LBL_compass3_offset.ForeColor = Color.Yellow;
                }
                else if (offset3_x == 0 && offset3_y == 0 & offset3_z == 0)
                {
                    LBL_compass3_offset.ForeColor = Color.Red;
                }
                else
                {
                    LBL_compass3_offset.ForeColor = Color.Green;
                }


                LBL_compass3_offset.Text = "OFFSETS  X: " +
                                           offset3_x.ToString() +
                                           ",   Y: " + offset3_y.ToString() +
                                           ",   Z: " + offset3_z.ToString();
                LBL_compass3_mot.Text = "MOT          X: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_X"]).ToString() +
                                        ",   Y: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_Y"]).ToString() +
                                        ",   Z: " + ((int)MainV2.comPort.MAV.param["COMPASS_MOT3_Z"]).ToString();
            }
            else
            {
                groupBoxCompass3.Hide();
            }

            ShowRelevantFields();

            startup = false;
        }