예제 #1
0
        //Runs the calibration algorithm
        void calibrateThreadFunction()
        {
            addMessage("Starting Calibration with " + compassSamples.Count.ToString() + " samples.");
            cal = new Calibration(this);
            bool calSuccess = false;
            if(threeAxisButton.Checked)
                calSuccess = cal.calibrate3(compassSamples);
            else
                calSuccess = cal.calibrate2(compassSamples);
            if (calSuccess)
            {
                //Calibration was successful - redraw all the samples in green.
                addMessage("Calibration successful.");
                if (localFieldTextBox.Text == "")
                    setLocalField(cal.estimatedLocalField.ToString("F5"));
                for (int i = 0; i < compassSamples.Count; i++)
                {
                    //apply calibration
                    Vector3 rawData = new Vector3(compassSamples[i].data[0], compassSamples[i].data[1], compassSamples[i].data[2]);
                    Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform);

                    //apply local mag field - calibrate returns a unit (mag 1) field
                    calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData);

                    lock (p)
                    {
                        if (twoAxisButton.Checked)
                        {
                            p.addPoint(new Vector3(
                                calibratedData.X,
                                calibratedData.Y,
                                -calibratedData.Z), Brushes.Green);
                        }
                        else
                        {
                            p.addPoint(new Vector3(
                                calibratedData.X,
                                calibratedData.Z,
                                -calibratedData.Y), Brushes.Green);
                        }
                    }
                }
                calibrated = true;
            }
            else
            {
                //failed
                addMessage("Calibration failed, try again.");
            }

            setButtonEnabled(sampleDoneButton, true, "Start");
        }