public ProgressReporterDialogue()
 {
     InitializeComponent();
     doWorkArgs            = new ProgressWorkerEventArgs();
     this.AutoScaleMode    = System.Windows.Forms.AutoScaleMode.None;
     this.btnClose.Visible = false;
 }
Example #2
0
        public ProgressReporterDialogue()
        {
            InitializeComponent();
            doWorkArgs = new ProgressWorkerEventArgs();

            this.btnClose.Visible = false;
        }
 public ProgressReporterDialogue()
 {
     InitializeComponent();
     doWorkArgs = new ProgressWorkerEventArgs();
     this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.None;
     this.btnClose.Visible = false;
 }
Example #4
0
        private void pdr_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            var fw = new Firmware();
            fw.Progress -= fw_Progress1;
            fw.Progress += fw_Progress;
            softwares = fw.getFWList(firmwareurl);

            foreach (var soft in softwares)
            {
                updateDisplayNameInvoke(soft);
            }
        }
        void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // list of x,y,z 's
            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            // backup current rate and set to 10 hz
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
            MainV2.comPort.MAV.cs.ratesensors = 10;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz

            DateTime deadline = DateTime.Now.AddSeconds(60);

            float oldmx = 0;
            float oldmy = 0;
            float oldmz = 0;

            while (deadline > DateTime.Now)
            {
                double timeremaining = (deadline - DateTime.Now).TotalSeconds;
                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples");

                if (e.CancelRequested)
                {
                    // restore old sensor rate
                    MainV2.comPort.MAV.cs.ratesensors = backupratesens;
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

                    e.CancelAcknowledged = true;
                    return;
                }

                if (oldmx != MainV2.comPort.MAV.cs.mx &&
                    oldmy != MainV2.comPort.MAV.cs.my &&
                    oldmz != MainV2.comPort.MAV.cs.mz)
                {
                    data.Add(new Tuple<float, float, float>(
                        MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x,
                        MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y,
                        MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z));

                    oldmx = MainV2.comPort.MAV.cs.mx;
                    oldmy = MainV2.comPort.MAV.cs.my;
                    oldmz = MainV2.comPort.MAV.cs.mz;
                }
            }

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            if (data.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                return;
            }

            ans = MagCalib.LeastSq(data);
        }
Example #6
0
        static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // old method
            float minx = 0;
            float maxx = 0;
            float miny = 0;
            float maxy = 0;
            float minz = 0;
            float maxz = 0;

            // backup current rate and set to 10 hz
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
            MainV2.comPort.MAV.cs.ratesensors = 10;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz

            var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);

            string extramsg = "";

            ((ProgressReporterSphere)sender).sphere1.Clear();

            int lastcount = 0;

            while (true)
            {
                // slow down execution
                System.Threading.Thread.Sleep(20);

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);

                if (e.CancelRequested)
                {
                    // restore old sensor rate
                    MainV2.comPort.MAV.cs.ratesensors = backupratesens;
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

                    e.CancelAcknowledged = false;
                    e.CancelRequested = false;
                    break;
                }

                    if (data.Count == 0)
                        continue;

                // dont use dup data
                if (lastcount == data.Count)
                    continue;

                lastcount = data.Count;

                float rawmx = data[data.Count - 1].Item1;
                float rawmy = data[data.Count - 1].Item2;
                float rawmz = data[data.Count - 1].Item3;

                // for old method
                setMinorMax(rawmx, ref minx, ref maxx);
                setMinorMax(rawmy, ref miny, ref maxy);
                setMinorMax(rawmz, ref minz, ref maxz);

                // get the current estimated centerpoint
                HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
                HIL.Vector3 point;

                // add to sphere with center correction
                point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
                ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z));

                //find the mean radius
                float radius = 0;
                for (int i = 0; i < data.Count; i++)
                {
                    point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
                    radius += (float)(point + centre).length();
                }
                radius /= data.Count;

                //test that we can find one point near a set of points all around the sphere surface
                int factor = 4; // 4 point check 16 points
                float max_distance = radius / 3; //pretty generouse
                for (int j = 0; j < factor; j++)
                {
                    double theta = (Math.PI * (j + 0.5)) / factor;

                    for (int i = 0; i < factor; i++)
                    {
                        double phi = (2 * Math.PI * i) / factor;

                        HIL.Vector3 point_sphere = new HIL.Vector3(
                            (float)(Math.Sin(theta) * Math.Cos(phi) * radius),
                            (float)(Math.Sin(theta) * Math.Sin(phi) * radius),
                            (float)(Math.Cos(theta) * radius)) - centre;

                        //log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);

                        bool found = false;
                        for (int k = 0; k < data.Count; k++)
                        {
                            point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
                            double d = (point_sphere - point).length();
                            if (d < max_distance)
                            {
                                found = true;
                                break;
                            }
                        }
                        if (!found)
                        {
                            extramsg = "more data needed";
                            //e.ErrorMessage = "Data missing for some directions";
                            //ans = null;
                            //return;
                            j = factor;
                            break;
                        }
                        else
                        {
                            extramsg = "";
                        }
                    }
                }
            }

            MainV2.comPort.UnSubscribeToPacketType(sub);

            if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
            {
                e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
                ans = null;
                return;
            }

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            if (extramsg != "")
            {
                if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
                {
                    e.CancelAcknowledged = true;
                    e.CancelRequested = true;
                    ans = null;
                    return;
                }
            }

            // remove outlyers
            RemoveOutliers(ref data);

            if (data.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                return;
            }

            bool ellipsoid = false;

            if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
            {
                ellipsoid = true;
            }

            ans = MagCalib.LeastSq(data, ellipsoid);
        }
        private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
        {
            frmProgressReporter.UpdateProgressAndStatus(-1, Strings.MavlinkConnecting);

            giveComport = true;

            if (BaseStream is SerialPort)
            {
                // allow settings to settle - previous dtr 
                System.Threading.Thread.Sleep(1000);
            }

            Terrain = new TerrainFollow(this);

            bool hbseen = false;

            try
            {
                BaseStream.ReadBufferSize = 16*1024;

                lock (objlock) // so we dont have random traffic
                {
                    log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);

                    BaseStream.Open();

                    BaseStream.DiscardInBuffer();

                    // other boards seem to have issues if there is no delay? posible bootloader timeout issue
                    Thread.Sleep(1000);
                }

                MAVLinkMessage buffer = new MAVLinkMessage();
                MAVLinkMessage buffer1 = new MAVLinkMessage();

                DateTime start = DateTime.Now;
                DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);

                var countDown = new System.Timers.Timer {Interval = 1000, AutoReset = false};
                countDown.Elapsed += (sender, e) =>
                {
                    int secondsRemaining = (deadline - e.SignalTime).Seconds;
                    frmProgressReporter.UpdateProgressAndStatus(-1, string.Format(Strings.Trying, secondsRemaining));
                    if (secondsRemaining > 0) countDown.Start();
                };
                countDown.Start();

                // px4 native
                BaseStream.WriteLine("sh /etc/init.d/rc.usb");

                int count = 0;

                while (true)
                {
                    if (progressWorkerEventArgs.CancelRequested)
                    {
                        progressWorkerEventArgs.CancelAcknowledged = true;
                        countDown.Stop();
                        if (BaseStream.IsOpen)
                            BaseStream.Close();
                        giveComport = false;
                        return;
                    }

                    log.Info(DateTime.Now.Millisecond + " Start connect loop ");

                    if (DateTime.Now > deadline)
                    {
                        //if (Progress != null)
                        //    Progress(-1, "No Heartbeat Packets");
                        countDown.Stop();
                        this.Close();

                        if (hbseen)
                        {
                            progressWorkerEventArgs.ErrorMessage = Strings.Only1Hb;
                            throw new Exception(Strings.Only1HbD);
                        }
                        else
                        {
                            progressWorkerEventArgs.ErrorMessage = "No Heartbeat Packets Received";
                            throw new Exception(@"Can not establish a connection\n
Please check the following
1. You have firmware loaded
2. You have the correct serial port selected
3. PX4 - You have the microsd card installed
4. Try a diffrent usb port\n\n" +
                                                "No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
                        }
                    }

                    System.Threading.Thread.Sleep(1);

                    // can see 2 heartbeat packets at any time, and will connect - was one after the other

                    if (buffer.Length == 0)
                        buffer = getHeartBeat();

                    System.Threading.Thread.Sleep(1);

                    if (buffer1.Length == 0)
                        buffer1 = getHeartBeat();


                    if (buffer.Length > 0 || buffer1.Length > 0)
                        hbseen = true;

                    count++;

                    // 2 hbs that match
                    if (buffer.Length > 5 && buffer1.Length > 5 && buffer.sysid == buffer1.sysid && buffer.compid == buffer1.compid)
                    {
                        mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();

                        if (hb.type != (byte) MAVLink.MAV_TYPE.GCS)
                        {
                            SetupMavConnect(buffer, hb);
                            break;
                        }
                    }

                    // 2 hb's that dont match. more than one sysid here
                    if (buffer.Length > 5 && buffer1.Length > 5 && (buffer.sysid == buffer1.sysid || buffer.compid == buffer1.compid))
                    {
                        mavlink_heartbeat_t hb = buffer.ToStructure<mavlink_heartbeat_t>();

                        if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
                        {
                            SetupMavConnect(buffer, hb);
                            break;
                        }

                        hb = buffer1.ToStructure<mavlink_heartbeat_t>();

                        if (hb.type != (byte) MAVLink.MAV_TYPE.ANTENNA_TRACKER && hb.type != (byte) MAVLink.MAV_TYPE.GCS)
                        {
                            SetupMavConnect(buffer1, hb);
                            break;
                        }
                    }
                }

                countDown.Stop();

                getVersion();

                frmProgressReporter.UpdateProgressAndStatus(0,
                    "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");

                if (getparams)
                {
                    getParamListBG();
                }

                byte[] temp = ASCIIEncoding.ASCII.GetBytes("Mission Planner " + Application.ProductVersion + "\0");
                Array.Resize(ref temp, 50);
                generatePacket((byte) MAVLINK_MSG_ID.STATUSTEXT,
                    new mavlink_statustext_t() {severity = (byte) MAV_SEVERITY.INFO, text = temp});

                if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
                {
                    giveComport = false;
                    if (BaseStream.IsOpen)
                        BaseStream.Close();
                    return;
                }
            }
            catch (Exception e)
            {
                try
                {
                    BaseStream.Close();
                }
                catch
                {
                }
                giveComport = false;
                if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
                    progressWorkerEventArgs.ErrorMessage = Strings.ConnectFailed;
                log.Error(e);
                throw;
            }
            //frmProgressReporter.Close();
            giveComport = false;
            frmProgressReporter.UpdateProgressAndStatus(100, Strings.Done);
            log.Info("Done open " + MAV.sysid + " " + MAV.compid);
            MAV.packetslost = 0;
            MAV.synclost = 0;
        }
 void FrmProgressReporterGetParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
 {
     getParamListBG();
 }
Example #9
0
File: GCS.cs Project: kkouer/PcGcs
        void saveWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            try
            {
                MAVLinkInterface port = comPort;

                if (!port.BaseStream.IsOpen)
                {
                    //throw new Exception("Please connect first!");
                    throw new Exception("请先连接无人机!");
                }

                comPort.giveComport = true;
                int a = 0;

                // define the home point
                Locationwp home = new Locationwp();
                try
                {
                    home.id = (byte)MAVLink.MAV_CMD.WAYPOINT;
                    home.lat = (double.Parse(TXT_homelat.Text));
                    home.lng = (double.Parse(TXT_homelng.Text));
                    home.alt = (float.Parse(TXT_homealt.Text) / CurrentState.multiplierdist); // use saved home
                }
                catch { throw new Exception("Your home location is invalid"); }

                // log
                //log.Info("wps values " + comPort.MAV.wps.Values.Count);
                //log.Info("cmd rows " + (Commands.Rows.Count + 1)); // + home

                // check for changes / future mod to send just changed wp's
                if (comPort.MAV.wps.Values.Count == (Commands.Rows.Count + 1))
                {
                    Hashtable wpstoupload = new Hashtable();

                    a = -1;
                    foreach (var item in comPort.MAV.wps.Values)
                    {
                        // skip home
                        if (a == -1)
                        {
                            a++;
                            continue;
                        }

                        MAVLink.mavlink_mission_item_t temp = DataViewtoLocationwp(a);

                        if (temp.command == item.command &&
                            temp.x == item.x &&
                            temp.y == item.y &&
                            temp.z == item.z &&
                            temp.param1 == item.param1 &&
                            temp.param2 == item.param2 &&
                            temp.param3 == item.param3 &&
                            temp.param4 == item.param4
                            )
                        {
                            //log.Info("wp match " + (a + 1));
                        }
                        else
                        {
                            //log.Info("wp no match" + (a + 1));
                            wpstoupload[a] = "";
                        }

                        a++;
                    }
                }

                // set wp total
                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传航点数量");

                ushort totalwpcountforupload = (ushort)(Commands.Rows.Count + 1);

                port.setWPTotal(totalwpcountforupload); // + home

                // set home location - overwritten/ignored depending on firmware.
                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "上传回家点");

                var homeans = port.setWP(home, 0, MAVLink.MAV_FRAME.GLOBAL, 0);

                if (homeans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                {
                    CustomMessageBox.Show(Strings.ErrorRejectedByMAV, Strings.ERROR);
                    return;
                }

                // define the default frame.
                MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;

                // get the command list from the datagrid
                var commandlist = GetCommandList();

                // upload from wp1, as home is alreadey sent
                a = 1;
                // process commandlist to the mav
                foreach (var temp in commandlist)
                {
                    // this code below fails
                    //a = commandlist.IndexOf(temp) + 1;

                    ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / Commands.Rows.Count, "上传航点 " + a);

                    // make sure we are using the correct frame for these commands
                    if (temp.id < (byte)MAVLink.MAV_CMD.LAST || temp.id == (byte)MAVLink.MAV_CMD.DO_SET_HOME)
                    {
                        var mode = currentaltmode;

                        if (mode == altmode.Terrain)
                        {
                            frame = MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT;
                        }
                        else if (mode == altmode.Absolute)
                        {
                            frame = MAVLink.MAV_FRAME.GLOBAL;
                        }
                        else
                        {
                            frame = MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;
                        }
                    }

                    // try send the wp
                    MAVLink.MAV_MISSION_RESULT ans = port.setWP(temp, (ushort)(a), frame, 0);

                    // we timed out while uploading wps/ command wasnt replaced/ command wasnt added
                    if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ERROR)
                    {
                        // resend for partial upload
                        port.setWPPartialUpdate((ushort)(a), totalwpcountforupload);
                        // reupload this point.
                        ans = port.setWP(temp, (ushort)(a), frame, 0);
                    }

                    if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_NO_SPACE)
                    {
                        //e.ErrorMessage = "Upload failed, please reduce the number of wp's";
                        e.ErrorMessage = "上传航点失败,航点数量太多,请减少航点数量.";
                        return;
                    }
                    if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID)
                    {
                        e.ErrorMessage = "Upload failed, mission was rejected byt the Mav,\n item had a bad option wp# " + a + " " + ans;
                        return;
                    }
                    if (ans == MAVLink.MAV_MISSION_RESULT.MAV_MISSION_INVALID_SEQUENCE)
                    {
                        // invalid sequence can only occur if we failed to see a response from the apm when we sent the request.
                        // therefore it did see the request and has moved on that step, and so do we.
                        a++;
                        continue;
                    }
                    if (ans != MAVLink.MAV_MISSION_RESULT.MAV_MISSION_ACCEPTED)
                    {
                        //e.ErrorMessage = "Upload wps failed " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
                        e.ErrorMessage = "上传失败 " + Enum.Parse(typeof(MAVLink.MAV_CMD), temp.id.ToString()) + " " + Enum.Parse(typeof(MAVLink.MAV_MISSION_RESULT), ans.ToString());
                        return;
                    }

                    a++;
                }

                port.setWPACK();

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(95, "Setting params");

                // m
                port.setParam("WP_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist);

                // cm's
                port.setParam("WPNAV_RADIUS", (byte)int.Parse(TXT_WPRad.Text) / CurrentState.multiplierdist * 100);

                try
                {
                    port.setParam(new[] { "LOITER_RAD", "WP_LOITER_RAD" }, int.Parse(TXT_loiterrad.Text) / CurrentState.multiplierdist);
                }
                catch
                {

                }

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "完成.");
            }
            catch (Exception ex)
            {
                log.Error(ex); comPort.giveComport = false; throw;
            }

            comPort.giveComport = false;
        }
Example #10
0
 void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
 {
     this.ConvertBini(inputfn, outputfn, true);
 }
Example #11
0
        void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // list of x,y,z 's
            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            // backup current rate and set to 10 hz
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
            MainV2.comPort.MAV.cs.ratesensors = 10;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz

            DateTime deadline = DateTime.Now.AddSeconds(60);

            float oldmx = 0;
            float oldmy = 0;
            float oldmz = 0;

            ((ProgressReporterSphere)sender).sphere1.Clear();

            while (deadline > DateTime.Now)
            {
                double timeremaining = (deadline - DateTime.Now).TotalSeconds;
                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus((int)(((60 - timeremaining) / 60) * 100), timeremaining.ToString("0") + " Seconds - got " + data.Count + " Samples");

                if (e.CancelRequested)
                {
                    // restore old sensor rate
                    MainV2.comPort.MAV.cs.ratesensors = backupratesens;
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

                    e.CancelAcknowledged = false;
                    e.CancelRequested = false;
                    break;
                }

                if (oldmx != MainV2.comPort.MAV.cs.mx &&
                    oldmy != MainV2.comPort.MAV.cs.my &&
                    oldmz != MainV2.comPort.MAV.cs.mz)
                {
                    data.Add(new Tuple<float, float, float>(
                        MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x,
                        MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y,
                        MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z));

                    oldmx = MainV2.comPort.MAV.cs.mx;
                    oldmy = MainV2.comPort.MAV.cs.my;
                    oldmz = MainV2.comPort.MAV.cs.mz;

                    ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(oldmx,oldmy,oldmz));
                }
            }

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            if (data.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                return;
            }

            bool ellipsoid = false;

            if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
            {
                ellipsoid = true;
            }

            ans = MagCalib.LeastSq(data, ellipsoid);

            //find the mean radius
            HIL.Vector3 centre = new HIL.Vector3((float)-ans[0], (float)-ans[1], (float)-ans[2]);
            HIL.Vector3 point;
            float radius = 0;
            for (int i = 0; i < data.Count; i++)
            {
                point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
                radius += (float)(point - centre).length();
            }
            radius /= data.Count;

            //test that we can find one point near a set of points all around the sphere surface
            int factor = 2; // 4 point check 2x2
            float max_distance = radius / 3; //pretty generouse
            for (int j = 0; j < factor; j++)
            {
                double theta = (Math.PI * (j + 0.5)) / factor;

                for (int i = 0; i < factor; i++)
                {
                    double phi = (2 * Math.PI * i) / factor;

                    HIL.Vector3 point_sphere = new HIL.Vector3(
                        (float)(Math.Sin(theta) * Math.Cos(phi) * radius),
                        (float)(Math.Sin(theta) * Math.Sin(phi) * radius),
                        (float)(Math.Cos(theta) * radius)) + centre;

                    Console.WriteLine("{0} {1}", theta * rad2deg, phi * rad2deg);

                    bool found = false;
                    for (int k = 0; k < data.Count; k++)
                    {
                        point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
                        double d = (point_sphere - point).length();
                        if (d < max_distance)
                        {
                            found = true;
                            break;
                        }
                    }
                    if (!found)
                    {
                        e.ErrorMessage = "Data missing for some directions";
                        ans = null;
                        return;
                    }
                }
            }
        }
Example #12
0
        static void DoUpdateWorker_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // TODO: Is this the right place?

            #region Fetch Parameter Meta Data

            var progressReporterDialogue = ((ProgressReporterDialogue) sender);
            progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters");

            try
            {
                ParameterMetaDataParser.GetParameterInformation();
            }
            catch (Exception ex)
            {
                log.Error(ex.ToString());
                CustomMessageBox.Show("Error getting Parameter Information");
            }

            #endregion Fetch Parameter Meta Data

            progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL");

            try
            {
                File.WriteAllText(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "writetest.txt", "this is a test");
            }
            catch(Exception ex)
            {
                log.Info("Write test failed");
                throw new Exception("Unable to write to the install directory", ex);
            }
            finally
            {
                try
                {
                    File.Delete(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + "writetest.txt");
                }
                catch
                {
                    log.Info("Write test cleanup failed");
                }
            }

            // check for updates
            //  if (Debugger.IsAttached)
            {
                //      log.Info("Skipping update test as it appears we are debugging");
            }
            //  else
            {
                updateCheckMain(progressReporterDialogue);
            }
        }
Example #13
0
        static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // turn learning off
            MainV2.comPort.setParam("COMPASS_LEARN", 0);

            bool havecompass2 = false;

            //compass2 get mag2 offsets
            float com2ofsx = 0;
            float com2ofsy = 0;
            float com2ofsz = 0;
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
            {
                com2ofsx = MainV2.comPort.GetParam("COMPASS_OFS2_X");
                com2ofsy = MainV2.comPort.GetParam("COMPASS_OFS2_Y");
                com2ofsz = MainV2.comPort.GetParam("COMPASS_OFS2_Z");

                havecompass2 = true;
            }
            
            // old method
            float minx = 0;
            float maxx = 0;
            float miny = 0;
            float maxy = 0;
            float minz = 0;
            float maxz = 0;

            // backup current rate and set to 10 hz
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
            MainV2.comPort.MAV.cs.ratesensors = 10;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz

            var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);

            var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);

            string extramsg = "";

            ((ProgressReporterSphere)sender).sphere1.Clear();
            ((ProgressReporterSphere)sender).sphere2.Clear();

            int lastcount = 0;
            DateTime lastlsq = DateTime.MinValue;
            DateTime lastlsq2 = DateTime.MinValue;

            HIL.Vector3 centre = new HIL.Vector3();

            while (true)
            {
                // slow down execution
                System.Threading.Thread.Sleep(20);

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + datacompass1.Count + " Samples " + extramsg);

                if (e.CancelRequested)
                {
                    // restore old sensor rate
                    MainV2.comPort.MAV.cs.ratesensors = backupratesens;
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

                    e.CancelAcknowledged = false;
                    e.CancelRequested = false;
                    break;
                }

                if (datacompass1.Count == 0)
                    continue;
                
                // dont use dup data
                if (lastcount == datacompass1.Count)
                    continue;

                lastcount = datacompass1.Count;

                float rawmx = datacompass1[datacompass1.Count - 1].Item1;
                float rawmy = datacompass1[datacompass1.Count - 1].Item2;
                float rawmz = datacompass1[datacompass1.Count - 1].Item3;

                // for old method
                setMinorMax(rawmx, ref minx, ref maxx);
                setMinorMax(rawmy, ref miny, ref maxy);
                setMinorMax(rawmz, ref minz, ref maxz);

                // get the current estimated centerpoint
                //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));

                // run lsq every seconds when more than 100 datapoints
                if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
                {
                    lastlsq = DateTime.Now;
                    lock (datacompass1)
                    {
                        var lsq = MagCalib.LeastSq(datacompass1, false);
                        // simple validation
                        if (Math.Abs(lsq[0]) < 999)
                        {
                            centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
                            log.Info("new centre " + centre.ToString());

                            ((ProgressReporterSphere)sender).sphere1.CenterPoint = new OpenTK.Vector3((float)centre.x, (float)centre.y, (float)centre.z);
                        }
                    }
                }

                // run lsq every seconds when more than 100 datapoints
                if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
                {
                    lastlsq2 = DateTime.Now;
                    lock (datacompass2)
                    {
                        var lsq = MagCalib.LeastSq(datacompass2, false);
                        // simple validation
                        if (Math.Abs(lsq[0]) < 999)
                        {
                            HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
                            log.Info("new centre2 " + centre2.ToString());

                            ((ProgressReporterSphere)sender).sphere2.CenterPoint = new OpenTK.Vector3((float)centre2.x, (float)centre2.y, (float)centre2.z);
                        }
                    }
                }

                HIL.Vector3 point;

                // add to sphere with center correction
                point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;
                ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
                ((ProgressReporterSphere)sender).sphere1.AimClear();

                if (havecompass2 && datacompass2.Count > 0)
                {
                    float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
                    float raw2my = datacompass2[datacompass2.Count - 1].Item2;
                    float raw2mz = datacompass2[datacompass2.Count - 1].Item3;

                    ((ProgressReporterSphere)sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
                    ((ProgressReporterSphere)sender).sphere2.AimClear();
                }

                //find the mean radius                    
                float radius = 0;
                for (int i = 0; i < datacompass1.Count; i++)
                {
                    point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
                    radius += (float)(point + centre).length();
                }
                radius /= datacompass1.Count;

                //test that we can find one point near a set of points all around the sphere surface
                string displayresult = "";
                int factor = 4; // 4 point check 16 points
                float max_distance = radius / 3; //pretty generouse
                for (int j = 0; j < factor; j++)
                {
                    double theta = (Math.PI * (j + 0.5)) / factor;

                    for (int i = 0; i < factor; i++)
                    {
                        double phi = (2 * Math.PI * i) / factor;

                        HIL.Vector3 point_sphere = new HIL.Vector3(
                            (float)(Math.Sin(theta) * Math.Cos(phi) * radius),
                            (float)(Math.Sin(theta) * Math.Sin(phi) * radius),
                            (float)(Math.Cos(theta) * radius)) - centre;

                        //log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);

                        bool found = false;
                        for (int k = 0; k < datacompass1.Count; k++)
                        {
                            point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
                            double d = (point_sphere - point).length();
                            if (d < max_distance)
                            {
                                found = true;
                                break;
                            }
                        }
                        if (!found)
                        {
                            displayresult = "more data needed " + (theta * rad2deg).ToString("0") + " " + (phi * rad2deg).ToString("0");
                            ((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
                            //j = factor;
                            break;
                        }
                    }
                }
                extramsg = displayresult;
            }

            MainV2.comPort.UnSubscribeToPacketType(sub);
            MainV2.comPort.UnSubscribeToPacketType(sub2);

            if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 || minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
            {
                e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
                ans = null;
                ans2 = null;
                return;
            }

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            if (extramsg != "")
            {
                if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
                {
                    e.CancelAcknowledged = true;
                    e.CancelRequested = true;
                    ans = null;
                    ans2 = null;
                    return;
                }
            }

            // remove outlyers
            RemoveOutliers(ref datacompass1);
            if (havecompass2 && datacompass2.Count > 0)
            {
                RemoveOutliers(ref datacompass2);
            }

            if (datacompass1.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                ans2 = null;
                return;
            }

            bool ellipsoid = false;

            if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
            {
                ellipsoid = true;
            }

            log.Info("Compass 1");
            ans = MagCalib.LeastSq(datacompass1, ellipsoid);

            if (havecompass2 && datacompass2.Count > 0)
            {
                log.Info("Compass 2");
                ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
            }
        }
Example #14
0
        void DoCalibration(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            var prd = ((ProgressReporterDialogue)sender);

            prd.UpdateProgressAndStatus(-1, "Starting Compass Mot");

            int8_t comp_type;                 // throttle or current based compensation
            Vector3f compass_base = new Vector3f();              // compass vector when throttle is zero
            Vector3f motor_impact = new Vector3f();              // impact of motors on compass vector
            Vector3f motor_impact_scaled;       // impact of motors on compass vector scaled with throttle
            Vector3f motor_compensation;        // final compensation to be stored to eeprom
            float throttle_pct;              // throttle as a percentage 0.0 ~ 1.0
            float throttle_pct_max = 0.0f;   // maximum throttle reached (as a percentage 0~1.0)
            float current_amps_max = 0.0f;   // maximum current reached
            float interference_pct;          // interference as a percentage of total mag field (for reporting purposes only)
            //uint32_t last_run_time;
            uint8_t print_counter = 49;
            bool updated = false;           // have we updated the compensation vector at least once

            if ((float)MainV2.comPort.MAV.param["BATT_MONITOR"] == 4f) // volt and current
            {
                comp_type = (sbyte)comptype.Current;
                prd.UpdateProgressAndStatus(-1, "Compass Mot using current");
            }
            else
            {
                comp_type = (sbyte)comptype.Throttle;
                prd.UpdateProgressAndStatus(-1, "Compass Mot using throttle");
            }

            if ((float)MainV2.comPort.MAV.param["COMPASS_USE"] != 1)
            {
                e.ErrorMessage = "Compass is disabled";
                return;
            }

            // request streams
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 10); // rc out
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50); // mag out

            // reset compass mot
            MainV2.comPort.setParam("COMPASS_MOTCT ", 0.0f);

            MainV2.comPort.setParam("COMPASS_MOT_X", 0.000000f);
            MainV2.comPort.setParam("COMPASS_MOT_Y", 0.000000f);
            MainV2.comPort.setParam("COMPASS_MOT_Z", 0.000000f);

            // store initial x,y,z compass values
            compass_base.x = MainV2.comPort.MAV.cs.mx;
            compass_base.y = MainV2.comPort.MAV.cs.my;
            compass_base.z = MainV2.comPort.MAV.cs.mz;

            // initialise motor compensation
            motor_compensation = new Vector3f(0, 0, 0);

            int magseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.RAW_IMU];
            int rcseen = MainV2.comPort.MAV.packetseencount[(byte)MAVLink.MAVLINK_MSG_ID.HIL_RC_INPUTS_RAW];
            DateTime deadline = DateTime.Now.AddSeconds(10);

            prd.UpdateProgressAndStatus(-1, "Waiting for Mag and RC data");

            while (true)
            {
                if (magseen > (magseen + 100) && rcseen > (rcseen + 20))
                {
                    break;
                }

                if (e.CancelRequested)
                {
                    e.CancelAcknowledged = true;
                    return;
                }

                if (DateTime.Now > deadline)
                {
                    e.ErrorMessage = "Not enough packets where received\n" + magseen + " mag " + rcseen + " rc";
                    return;
                }
            }

            while (true)
            {
                if (prd.doWorkArgs.CancelRequested)
                {
                    prd.doWorkArgs.CancelAcknowledged = true;
                    break;
                }

                // radio

                // passthorugh - cant do.

                // compass read

                // battery read

                // calculate scaling for throttle
                int checkme;
                throttle_pct = (float)MainV2.comPort.MAV.cs.ch3percent / 100.0f;
                throttle_pct = constrain_float(throttle_pct, 0.0f, 1.0f);

                // if throttle is zero, update base x,y,z values
                if (throttle_pct == 0.0f)
                {
                    compass_base.x = compass_base.x * 0.99f + (float)MainV2.comPort.MAV.cs.mx * 0.01f;
                    compass_base.y = compass_base.y * 0.99f + (float)MainV2.comPort.MAV.cs.my * 0.01f;
                    compass_base.z = compass_base.z * 0.99f + (float)MainV2.comPort.MAV.cs.mz * 0.01f;

                    // causing printing to happen as soon as throttle is lifted
                    print_counter = 49;
                }
                else
                {

                    // calculate diff from compass base and scale with throttle
                    motor_impact.x = MainV2.comPort.MAV.cs.mx - compass_base.x;
                    motor_impact.y = MainV2.comPort.MAV.cs.my - compass_base.y;
                    motor_impact.z = MainV2.comPort.MAV.cs.mz - compass_base.z;

                    // throttle based compensation
                    if (comp_type == (byte)comptype.Throttle)
                    {
                        // scale by throttle
                        motor_impact_scaled = motor_impact / throttle_pct;

                        // adjust the motor compensation to negate the impact
                        motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
                        updated = true;
                    }
                    else
                    {
                        // current based compensation if more than 3amps being drawn
                        motor_impact_scaled = motor_impact / MainV2.comPort.MAV.cs.current;

                        // adjust the motor compensation to negate the impact if drawing over 3amps
                        if (MainV2.comPort.MAV.cs.current >= 3.0f)
                        {
                            motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
                            updated = true;
                        }
                    }

                    // record maximum throttle and current
                    throttle_pct_max = max(throttle_pct_max, throttle_pct);
                    current_amps_max = max(current_amps_max, MainV2.comPort.MAV.cs.current);

                    // display output at 1hz if throttle is above zero
                    print_counter++;
                    if (print_counter >= 50)
                    {
                        print_counter = 0;
                        var line = String.Format("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f  comp x:%4.2f y:%4.2f z:%4.2f\n", (int)MainV2.comPort.MAV.cs.ch3percent, (float)MainV2.comPort.MAV.cs.current, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
                        Console.Write(line);
                        prd.UpdateProgressAndStatus(-1, line);
                    }
                }
            }

            MainV2.comPort.doARM(false);

            // set and save motor compensation
            if (updated)
            {
                MainV2.comPort.setParam("COMPASS_MOTCT ", comp_type);

                MainV2.comPort.setParam("COMPASS_MOT_X", (float)motor_compensation.x);
                MainV2.comPort.setParam("COMPASS_MOT_Y", (float)motor_compensation.y);
                MainV2.comPort.setParam("COMPASS_MOT_Z", (float)motor_compensation.z);

                // calculate and display interference compensation at full throttle as % of total mag field
                if (comp_type == (byte)comptype.Throttle)
                {
                    // interference is impact@fullthrottle / mag field * 100
                    interference_pct = (float)motor_compensation.length() / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
                }
                else
                {
                    // interference is impact/amp * (max current seen / max throttle seen) / mag field * 100
                    interference_pct = (float)motor_compensation.length() * (current_amps_max / throttle_pct_max) / (float)COMPASS_MAGFIELD_EXPECTED * 100.0f;
                }
                string line = String.Format("\nInterference at full throttle is {0}% of mag field\n\n", (int)interference_pct);
                Console.Write(line);
                prd.UpdateProgressAndStatus(100, line);
            }
            else
            {
                prd.UpdateProgressAndStatus(100, "Failed");
            }

            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 2);
        }
Example #15
0
 public ProgressReporterDialogue()
 {
     doWorkArgs         = new ProgressWorkerEventArgs();
     this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.None;
 }
        static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // turn learning off
            MainV2.comPort.setParam("COMPASS_LEARN", 0);

            bool havecompass2 = false;
            bool havecompass3 = false;

            //compass2 get mag2 offsets
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
            {
                MainV2.comPort.setParam("COMPASS_OFS2_X", 0, true);
                MainV2.comPort.setParam("COMPASS_OFS2_Y", 0, true);
                MainV2.comPort.setParam("COMPASS_OFS2_Z", 0, true);

                havecompass2 = true;
            }

            //compass3
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS3_X"))
            {
                MainV2.comPort.setParam("COMPASS_OFS3_X", 0, true);
                MainV2.comPort.setParam("COMPASS_OFS3_Y", 0, true);
                MainV2.comPort.setParam("COMPASS_OFS3_Z", 0, true);

                havecompass3 = true;
            }

            int hittarget = 14; // int.Parse(File.ReadAllText("magtarget.txt"));

            // old method
            float minx = 0;
            float maxx = 0;
            float miny = 0;
            float maxy = 0;
            float minz = 0;
            float maxz = 0;

            // backup current rate and set
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;

            byte backuprateatt = MainV2.comPort.MAV.cs.rateattitude;
            byte backupratepos = MainV2.comPort.MAV.cs.rateposition;

            MainV2.comPort.MAV.cs.ratesensors = 2;
            MainV2.comPort.MAV.cs.rateattitude = 0;
            MainV2.comPort.MAV.cs.rateposition = 0;

            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);

            // subscribe to data packets
            var sub = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.RAW_IMU, ReceviedPacket);

            var sub2 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU2, ReceviedPacket);

            var sub3 = MainV2.comPort.SubscribeToPacketType(MAVLink.MAVLINK_MSG_ID.SCALED_IMU3, ReceviedPacket);

            string extramsg = "";

            // clear any old data
            ((ProgressReporterSphere) sender).sphere1.Clear();
            ((ProgressReporterSphere) sender).sphere2.Clear();
            ((ProgressReporterSphere) sender).sphere3.Clear();

            // keep track of data count and last lsq run
            int lastcount = 0;
            DateTime lastlsq = DateTime.MinValue;
            DateTime lastlsq2 = DateTime.MinValue;
            DateTime lastlsq3 = DateTime.MinValue;

            HIL.Vector3 centre = new HIL.Vector3();

            while (true)
            {
                // slow down execution
                System.Threading.Thread.Sleep(10);

                string str = "Got + " + datacompass1.Count + " samples\n" +
                             "Compass 1 error: " + error;
                if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS2_X"))
                    str += "\nCompass 2 error: " + error2;
                if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS3_X"))
                    str += "\nCompass 3 error: " + error3;
                str += "\n" + extramsg;

                ((ProgressReporterDialogue) sender).UpdateProgressAndStatus(-1, str);

                if (e.CancelRequested)
                {
                    e.CancelAcknowledged = false;
                    e.CancelRequested = false;
                    break;
                }

                if (datacompass1.Count == 0)
                    continue;

                float rawmx = datacompass1[datacompass1.Count - 1].Item1;
                float rawmy = datacompass1[datacompass1.Count - 1].Item2;
                float rawmz = datacompass1[datacompass1.Count - 1].Item3;

                // for old method
                setMinorMax(rawmx, ref minx, ref maxx);
                setMinorMax(rawmy, ref miny, ref maxy);
                setMinorMax(rawmz, ref minz, ref maxz);

                // get the current estimated centerpoint
                //new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));

                //Console.WriteLine("1 " + DateTime.Now.Millisecond);

                // run lsq every second when more than 100 datapoints
                if (datacompass1.Count > 100 && lastlsq.Second != DateTime.Now.Second)
                {
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.ALL, 0);
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, 50);

                    lastlsq = DateTime.Now;
                    lock (datacompass1)
                    {
                        var lsq = MagCalib.LeastSq(datacompass1, false);
                        // simple validation
                        if (Math.Abs(lsq[0]) < 999)
                        {
                            centre = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
                            log.Info("new centre " + centre.ToString());

                            ((ProgressReporterSphere) sender).sphere1.CenterPoint = new OpenTK.Vector3(
                                (float) centre.x, (float) centre.y, (float) centre.z);
                        }
                    }
                }

                // run lsq every second when more than 100 datapoints
                if (datacompass2.Count > 100 && lastlsq2.Second != DateTime.Now.Second)
                {
                    lastlsq2 = DateTime.Now;
                    lock (datacompass2)
                    {
                        var lsq = MagCalib.LeastSq(datacompass2, false);
                        // simple validation
                        if (Math.Abs(lsq[0]) < 999)
                        {
                            HIL.Vector3 centre2 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
                            log.Info("new centre2 " + centre2.ToString());

                            ((ProgressReporterSphere) sender).sphere2.CenterPoint = new OpenTK.Vector3(
                                (float) centre2.x, (float) centre2.y, (float) centre2.z);
                        }
                    }
                }

                // run lsq every second when more than 100 datapoints
                if (datacompass3.Count > 100 && lastlsq3.Second != DateTime.Now.Second)
                {
                    lastlsq3 = DateTime.Now;
                    lock (datacompass3)
                    {
                        var lsq = MagCalib.LeastSq(datacompass3, false);
                        // simple validation
                        if (Math.Abs(lsq[0]) < 999)
                        {
                            HIL.Vector3 centre3 = new HIL.Vector3(lsq[0], lsq[1], lsq[2]);
                            log.Info("new centre2 " + centre3.ToString());

                            ((ProgressReporterSphere) sender).sphere3.CenterPoint = new OpenTK.Vector3(
                                (float) centre3.x, (float) centre3.y, (float) centre3.z);
                        }
                    }
                }

                //Console.WriteLine("1a " + DateTime.Now.Millisecond);

                // dont use dup data
                if (lastcount == datacompass1.Count)
                    continue;

                lastcount = datacompass1.Count;

                // add to sphere with center correction
                ((ProgressReporterSphere) sender).sphere1.AddPoint(new OpenTK.Vector3(rawmx, rawmy, rawmz));
                ((ProgressReporterSphere) sender).sphere1.AimClear();

                if (datacompass2.Count > 30)
                {
                    float raw2mx = datacompass2[datacompass2.Count - 1].Item1;
                    float raw2my = datacompass2[datacompass2.Count - 1].Item2;
                    float raw2mz = datacompass2[datacompass2.Count - 1].Item3;

                    ((ProgressReporterSphere) sender).sphere2.AddPoint(new OpenTK.Vector3(raw2mx, raw2my, raw2mz));
                    ((ProgressReporterSphere) sender).sphere2.AimClear();
                }

                if (datacompass3.Count > 30)
                {
                    float raw3mx = datacompass3[datacompass3.Count - 1].Item1;
                    float raw3my = datacompass3[datacompass3.Count - 1].Item2;
                    float raw3mz = datacompass3[datacompass3.Count - 1].Item3;

                    ((ProgressReporterSphere) sender).sphere3.AddPoint(new OpenTK.Vector3(raw3mx, raw3my, raw3mz));
                    ((ProgressReporterSphere) sender).sphere3.AimClear();
                }

                //Console.WriteLine("2 " + DateTime.Now.Millisecond);

                HIL.Vector3 point;

                point = new HIL.Vector3(rawmx, rawmy, rawmz) + centre;

                //find the mean radius                    
                float radius = 0;
                for (int i = 0; i < datacompass1.Count; i++)
                {
                    point = new HIL.Vector3(datacompass1[i].Item1, datacompass1[i].Item2, datacompass1[i].Item3);
                    radius += (float) (point + centre).length();
                }
                radius /= datacompass1.Count;

                //test that we can find one point near a set of points all around the sphere surface
                int pointshit = 0;
                string displayresult = "";
                int factor = 3; // pitch
                int factor2 = 4; // yaw
                float max_distance = radius/3; //pretty generouse
                for (int j = 0; j <= factor; j++)
                {
                    double theta = (Math.PI*(j + 0.5))/factor;

                    for (int i = 0; i <= factor2; i++)
                    {
                        double phi = (2*Math.PI*i)/factor2;

                        HIL.Vector3 point_sphere = new HIL.Vector3(
                            (float) (Math.Sin(theta)*Math.Cos(phi)*radius),
                            (float) (Math.Sin(theta)*Math.Sin(phi)*radius),
                            (float) (Math.Cos(theta)*radius)) - centre;

                        //log.InfoFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);

                        bool found = false;
                        for (int k = 0; k < datacompass1.Count; k++)
                        {
                            point = new HIL.Vector3(datacompass1[k].Item1, datacompass1[k].Item2, datacompass1[k].Item3);
                            double d = (point_sphere - point).length();
                            if (d < max_distance)
                            {
                                pointshit++;
                                found = true;
                                break;
                            }
                        }
                        // draw them all
                        //((ProgressReporterSphere)sender).sphere1.AimFor(new OpenTK.Vector3((float)point_sphere.x, (float)point_sphere.y, (float)point_sphere.z));
                        if (!found)
                        {
                            displayresult = "more data needed Aim For " +
                                            GetColour((int) (theta*rad2deg), (int) (phi*rad2deg));
                            ((ProgressReporterSphere) sender).sphere1.AimFor(new OpenTK.Vector3((float) point_sphere.x,
                                (float) point_sphere.y, (float) point_sphere.z));
                            //j = factor;
                            //break;
                        }
                    }
                }
                extramsg = displayresult;

                //Console.WriteLine("3 "+ DateTime.Now.Millisecond);

                // check primary compass error
                if (error < 0.2 && pointshit > hittarget && ((ProgressReporterSphere) sender).autoaccept)
                {
                    extramsg = "";
                    break;
                }
            }

            MainV2.comPort.UnSubscribeToPacketType(sub);
            MainV2.comPort.UnSubscribeToPacketType(sub2);
            MainV2.comPort.UnSubscribeToPacketType(sub3);

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.MAV.cs.rateattitude = backuprateatt;
            MainV2.comPort.MAV.cs.rateposition = backupratepos;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.POSITION, MainV2.comPort.MAV.cs.rateposition);
                // request gps
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA1, MainV2.comPort.MAV.cs.rateattitude);
                // request attitude
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA2, MainV2.comPort.MAV.cs.rateattitude);
                // request vfr
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.EXTRA3, MainV2.comPort.MAV.cs.ratesensors);
                // request extra stuff - tridge
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);
                // request raw sensor
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RC_CHANNELS, MainV2.comPort.MAV.cs.raterc);
                // request rc info

            if (MainV2.speechEnable)
            {
                MainV2.speechEngine.SpeakAsync("Compass Calibration Complete");
            }
            else
            {
                Console.Beep();
            }

            if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 || miny > 0 && maxy > 0 || miny < 0 && maxy < 0 ||
                minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
            {
                e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
                ans = null;
                ans2 = null;
                return;
            }

            if (extramsg != "")
            {
                if (CustomMessageBox.Show(Strings.MissingDataPoints, Strings.RunAnyway, MessageBoxButtons.YesNo) ==
                    DialogResult.No)
                {
                    e.CancelAcknowledged = true;
                    e.CancelRequested = true;
                    ans = null;
                    ans2 = null;
                    ans3 = null;
                    return;
                }
            }

            // remove outlyers
            RemoveOutliers(ref datacompass1);
            if (havecompass2 && datacompass2.Count > 0)
            {
                RemoveOutliers(ref datacompass2);
            }
            if (havecompass3 && datacompass3.Count > 0)
            {
                RemoveOutliers(ref datacompass3);
            }

            if (datacompass1.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                ans2 = null;
                return;
            }

            bool ellipsoid = false;

            if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
            {
                ellipsoid = true;
            }

            log.Info("Compass 1");
            ans = MagCalib.LeastSq(datacompass1, ellipsoid);

            if (havecompass2 && datacompass2.Count > 0)
            {
                log.Info("Compass 2");
                ans2 = MagCalib.LeastSq(datacompass2, ellipsoid);
            }

            if (havecompass3 && datacompass3.Count > 0)
            {
                log.Info("Compass 3");
                ans3 = MagCalib.LeastSq(datacompass3, ellipsoid);
            }
        }
Example #17
0
        static void DoUpdateWorker_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // TODO: Is this the right place?
            #region Fetch Parameter Meta Data

            var progressReporterDialogue = ((ProgressReporterDialogue)sender);
            progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Updated Parameters");

            try
            {

                ParameterMetaDataParser.GetParameterInformation();
            }
            catch (Exception ex) { log.Error(ex.ToString()); CustomMessageBox.Show("Error getting Parameter Information"); }

            #endregion Fetch Parameter Meta Data

            progressReporterDialogue.UpdateProgressAndStatus(-1, "Getting Base URL");
            // check for updates
            //  if (Debugger.IsAttached)
            {
                //      log.Info("Skipping update test as it appears we are debugging");
            }
            //  else
            {
                updateCheckMain(progressReporterDialogue);
            }
        }
Example #18
0
File: GCS.cs Project: kkouer/PcGcs
        void getWPs(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            List<Locationwp> cmds = new List<Locationwp>();

            try
            {
                MAVLinkInterface port = comPort;

                if (!port.BaseStream.IsOpen)
                {
                    throw new Exception("Please Connect First!");
                }

                comPort.giveComport = true;

                param = port.MAV.param;

                //log.Info("Getting WP #");

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(0, "Getting WP count");

                int cmdcount = port.getWPCount();

                for (ushort a = 0; a < cmdcount; a++)
                {
                    if (((ProgressReporterDialogue)sender).doWorkArgs.CancelRequested)
                    {
                        ((ProgressReporterDialogue)sender).doWorkArgs.CancelAcknowledged = true;
                        throw new Exception("Cancel Requested");
                    }

                    //log.Info("Getting WP" + a);
                    ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(a * 100 / cmdcount, "下载航点 " + a);
                    cmds.Add(port.getWP(a));
                }

                port.setWPACK();

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "完成");

                //log.Info("Done");
            }
            catch { throw; }

            WPtoScreen(cmds);
        }
 void FrmProgressReporterDoWorkNOParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
 {
     OpenBg(sender, false, e);
 }
Example #20
0
        private void OpenBg(object PRsender, bool getparams, ProgressWorkerEventArgs progressWorkerEventArgs)
        {
            frmProgressReporter.UpdateProgressAndStatus(-1, "Mavlink Connecting...");

            giveComport = true;

            // allow settings to settle - previous dtr
            System.Threading.Thread.Sleep(500);

            // reset
            MAV.sysid = 0;
            MAV.compid = 0;
            MAV.param = new Hashtable();
            MAV.packets.Initialize();
            MAV.VersionString = "";
            MAV.SoftwareVersions = "";
            MAV.SerialString = "";

            bool hbseen = false;

            try
            {
                BaseStream.ReadBufferSize = 16 * 1024;

                lock (objlock) // so we dont have random traffic
                {
                    log.Info("Open port with " + BaseStream.PortName + " " + BaseStream.BaudRate);

                    BaseStream.Open();

                    BaseStream.DiscardInBuffer();

                    // other boards seem to have issues if there is no delay? posible bootloader timeout issue
                    Thread.Sleep(1000);
                }

                byte[] buffer = new byte[0];
                byte[] buffer1 = new byte[0];

                DateTime start = DateTime.Now;
                DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS);

                var countDown = new System.Timers.Timer { Interval = 1000, AutoReset = false };
                countDown.Elapsed += (sender, e) =>
                {
                    int secondsRemaining = (deadline - e.SignalTime).Seconds;
                    //if (Progress != null)
                    //    Progress(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
                    frmProgressReporter.UpdateProgressAndStatus(-1, string.Format("Trying to connect.\nTimeout in {0}", secondsRemaining));
                    if (secondsRemaining > 0) countDown.Start();
                };
                countDown.Start();

                int count = 0;

                while (true)
                {
                    if (progressWorkerEventArgs.CancelRequested)
                    {
                        progressWorkerEventArgs.CancelAcknowledged = true;
                        countDown.Stop();
                        if (BaseStream.IsOpen)
                            BaseStream.Close();
                        giveComport = false;
                        return;
                    }

                    // incase we are in setup mode
                    //BaseStream.WriteLine("planner\rgcs\r");

                    log.Info(DateTime.Now.Millisecond + " Start connect loop ");

                    if (DateTime.Now > deadline)
                    {
                        //if (Progress != null)
                        //    Progress(-1, "No Heatbeat Packets");
                        countDown.Stop();
                        this.Close();

                        if (hbseen)
                        {
                            progressWorkerEventArgs.ErrorMessage = "Only 1 Heatbeat Received";
                            throw new Exception("Only 1 Mavlink Heartbeat Packets was read from this port - Verify your hardware is setup correctly\nMission Planner waits for 2 valid heartbeat packets before connecting");
                        }
                        else
                        {
                            progressWorkerEventArgs.ErrorMessage = "No Heatbeat Packets Received";
                            throw new Exception(@"Can not establish a connection\n
            Please check the following
            1. You have firmware loaded
            2. You have the correct serial port selected
            3. PX4 - You have the microsd card installed
            4. Try a diffrent usb port\n\n"+"No Mavlink Heartbeat Packets where read from this port - Verify Baud Rate and setup\nMission Planner waits for 2 valid heartbeat packets before connecting");
                        }
                    }

                    System.Threading.Thread.Sleep(1);

                    // incase we are in setup mode
                    //BaseStream.WriteLine("planner\rgcs\r");

                    // can see 2 heartbeat packets at any time, and will connect - was one after the other

                    if (buffer.Length == 0)
                        buffer = getHeartBeat();

                    // incase we are in setup mode
                    //BaseStream.WriteLine("planner\rgcs\r");

                    System.Threading.Thread.Sleep(1);

                    if (buffer1.Length == 0)
                        buffer1 = getHeartBeat();

                    if (buffer.Length > 0 || buffer1.Length > 0)
                        hbseen = true;

                    count++;

                    if (buffer.Length > 5 && buffer1.Length > 5 && buffer[3] == buffer1[3] && buffer[4] == buffer1[4])
                    {
                        mavlink_heartbeat_t hb = buffer.ByteArrayToStructure<mavlink_heartbeat_t>(6);

                        if (hb.type != (byte)MAVLink.MAV_TYPE.GCS)
                        {

                            mavlinkversion = hb.mavlink_version;
                            MAV.aptype = (MAV_TYPE)hb.type;
                            MAV.apname = (MAV_AUTOPILOT)hb.autopilot;

                            setAPType();

                            MAV.sysid = buffer[3];
                            MAV.compid = buffer[4];
                            MAV.recvpacketcount = buffer[2];
                            log.InfoFormat("ID sys {0} comp {1} ver{2}", MAV.sysid, MAV.compid, mavlinkversion);
                            break;
                        }
                    }

                }

                countDown.Stop();

                frmProgressReporter.UpdateProgressAndStatus(0, "Getting Params.. (sysid " + MAV.sysid + " compid " + MAV.compid + ") ");

                if (getparams)
                {
                    getParamListBG();
                }

                if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true)
                {
                    giveComport = false;
                    if (BaseStream.IsOpen)
                        BaseStream.Close();
                    return;
                }
            }
            catch (Exception e)
            {
                try
                {
                    BaseStream.Close();
                }
                catch { }
                giveComport = false;
                if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage))
                    progressWorkerEventArgs.ErrorMessage = "Connect Failed";
                log.Error(e);
                throw;
            }
            //frmProgressReporter.Close();
            giveComport = false;
            frmProgressReporter.UpdateProgressAndStatus(100, "Done.");
            log.Info("Done open " + MAV.sysid + " " + MAV.compid);
            packetslost = 0;
            synclost = 0;
        }
 void FrmProgressReporterGetParams(object sender, ProgressWorkerEventArgs e, object passdata = null)
 {
     Hashtable old = new Hashtable(MAV.param);
     getParamListBG();
     if (frmProgressReporter.doWorkArgs.CancelRequested)
     {
         MAV.param = old;
     }
 }
        static void prd_DoWork(object sender, ProgressWorkerEventArgs e, object passdata = null)
        {
            // list of x,y,z 's
            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            // old method
            float minx = 0;
            float maxx = 0;
            float miny = 0;
            float maxy = 0;
            float minz = 0;
            float maxz = 0;

            // backup current rate and set to 10 hz
            byte backupratesens = MainV2.comPort.MAV.cs.ratesensors;
            MainV2.comPort.MAV.cs.ratesensors = 10;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors); // mag captures at 10 hz

            float oldmx = 0;
            float oldmy = 0;
            float oldmz = 0;

            // filter data points to only x number per quadrant
            int div = 20;
            Hashtable filter = new Hashtable();

            string extramsg = "";

            ((ProgressReporterSphere)sender).sphere1.Clear();

            while (true)
            {
                // slow down execution
                System.Threading.Thread.Sleep(1);

                ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Got " + data.Count + " Samples " + extramsg);

                if (e.CancelRequested)
                {
                    // restore old sensor rate
                    MainV2.comPort.MAV.cs.ratesensors = backupratesens;
                    MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

                    e.CancelAcknowledged = false;
                    e.CancelRequested = false;
                    break;
                }

                if (oldmx != MainV2.comPort.MAV.cs.mx &&
                    oldmy != MainV2.comPort.MAV.cs.my &&
                    oldmz != MainV2.comPort.MAV.cs.mz)
                {
                    // for new lease sq
                    string item = (int)(MainV2.comPort.MAV.cs.mx / div) + "," +
                        (int)(MainV2.comPort.MAV.cs.my / div) + "," +
                        (int)(MainV2.comPort.MAV.cs.mz / div);

                    if (filter.ContainsKey(item))
                    {
                        filter[item] = (int)filter[item] + 1;

                        if ((int)filter[item] > 3)
                            continue;
                    }
                    else
                    {
                        filter[item] = 1;
                    }

                    // add data
                    data.Add(new Tuple<float, float, float>(
            MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x,
            MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y,
            MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z));

                    oldmx = MainV2.comPort.MAV.cs.mx;
                    oldmy = MainV2.comPort.MAV.cs.my;
                    oldmz = MainV2.comPort.MAV.cs.mz;

                    // for old method
                    setMinorMax(MainV2.comPort.MAV.cs.mx - (float)MainV2.comPort.MAV.cs.mag_ofs_x, ref minx, ref maxx);
                    setMinorMax(MainV2.comPort.MAV.cs.my - (float)MainV2.comPort.MAV.cs.mag_ofs_y, ref miny, ref maxy);
                    setMinorMax(MainV2.comPort.MAV.cs.mz - (float)MainV2.comPort.MAV.cs.mag_ofs_z, ref minz, ref maxz);

                    // get the current estimated centerpoint
                    HIL.Vector3 centre = new HIL.Vector3((float)-((maxx + minx) / 2), (float)-((maxy + miny) / 2), (float)-((maxz + minz) / 2));
                    HIL.Vector3 point;

                    // add to sphere after trnslating the centre point
                    point = new HIL.Vector3(oldmx, oldmy, oldmz) + centre;
                    ((ProgressReporterSphere)sender).sphere1.AddPoint(new OpenTK.Vector3((float)point.x, (float)point.y, (float)point.z));

                    //find the mean radius
                    float radius = 0;
                    for (int i = 0; i < data.Count; i++)
                    {
                        point = new HIL.Vector3(data[i].Item1, data[i].Item2, data[i].Item3);
                        radius += (float)(point - centre).length();
                    }
                    radius /= data.Count;

                    //test that we can find one point near a set of points all around the sphere surface
                    int factor = 4; // 4 point check 16 points
                    float max_distance = radius / 3; //pretty generouse
                    for (int j = 0; j < factor; j++)
                    {
                        double theta = (Math.PI * (j + 0.5)) / factor;

                        for (int i = 0; i < factor; i++)
                        {
                            double phi = (2 * Math.PI * i) / factor;

                            HIL.Vector3 point_sphere = new HIL.Vector3(
                                (float)(Math.Sin(theta) * Math.Cos(phi) * radius),
                                (float)(Math.Sin(theta) * Math.Sin(phi) * radius),
                                (float)(Math.Cos(theta) * radius)) - centre;

                            //log.DebugFormat("magcalib check - {0} {1} dist {2}", theta * rad2deg, phi * rad2deg, max_distance);

                            bool found = false;
                            for (int k = 0; k < data.Count; k++)
                            {
                                point = new HIL.Vector3(data[k].Item1, data[k].Item2, data[k].Item3);
                                double d = (point_sphere - point).length();
                                if (d < max_distance)
                                {
                                    found = true;
                                    break;
                                }
                            }
                            if (!found)
                            {
                                extramsg = "more data needed";
                                //e.ErrorMessage = "Data missing for some directions";
                                //ans = null;
                                //return;
                                j = factor;
                                break;
                            }
                            else
                            {
                                extramsg = "";
                            }
                        }
                    }
                }
            }

            if (minx > 0 && maxx > 0 || minx < 0 && maxx < 0 ||miny > 0 && maxy > 0 || miny < 0 && maxy < 0 ||minz > 0 && maxz > 0 || minz < 0 && maxz < 0)
            {
                e.ErrorMessage = "Bad compass raw values. Check for magnetic interferance.";
                ans = null;
                return;
            }

            // restore old sensor rate
            MainV2.comPort.MAV.cs.ratesensors = backupratesens;
            MainV2.comPort.requestDatastream(MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.comPort.MAV.cs.ratesensors);

            if (extramsg != "")
            {
                if (CustomMessageBox.Show("You are missing data points. do you want to run the calibration anyway?", "run anyway", MessageBoxButtons.YesNo) == DialogResult.No)
                {
                    e.CancelAcknowledged = true;
                    e.CancelRequested = true;
                    ans = null;
                    return;
                }
            }

            if (data.Count < 10)
            {
                e.ErrorMessage = "Log does not contain enough data";
                ans = null;
                return;
            }

            bool ellipsoid = false;

            if (MainV2.comPort.MAV.param.ContainsKey("MAG_DIA"))
            {
                ellipsoid = true;
            }

            ans = MagCalib.LeastSq(data, ellipsoid);
        }