ardupilot/Tools/ArdupilotMegaPlanner/MagCalib.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO;
using System.Collections;
using netDxf;
using netDxf.Entities;
using netDxf.Tables;
using netDxf.Header;
using System.Reflection;
using log4net;

namespace ArdupilotMega
{
    public class MagCalib
    {
        private static readonly ILog log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);

        /// <summary>
        /// Self contained process tlog and save/display offsets
        /// </summary>
        public static void ProcessLog(int throttleThreshold = 0)
        {
            OpenFileDialog openFileDialog1 = new OpenFileDialog();
            openFileDialog1.Filter = "*.tlog|*.tlog";
            openFileDialog1.FilterIndex = 2;
            openFileDialog1.RestoreDirectory = true;
            openFileDialog1.Multiselect = true;
            try
            {
                openFileDialog1.InitialDirectory = Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar;
            }
            catch { } // incase dir doesnt exist

            openFileDialog1.FileName = @"C:\Users\hog\Downloads\2012-02-05.log";

            if (openFileDialog1.ShowDialog() == DialogResult.OK)
            {
                try
                {
                    double[] ans = getOffsets(openFileDialog1.FileName, throttleThreshold);

                    if (ans.Length != 1)
                        SaveOffsets(ans);
                }
                catch (Exception ex) { log.Debug(ex.ToString()); }
            }
        }

        /// <summary>
        /// Processes a tlog to get the offsets - creates dxf of data
        /// </summary>
        /// <param name="fn">Filename</param>
        /// <returns>Offsets</returns>
        public static double[] getOffsets(string fn, int throttleThreshold = 0)
        {
            // based off tridge's work
            string logfile = fn;

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

            // this is for a dxf
            Polyline3dVertex vertex;
            List<Polyline3dVertex> vertexes = new List<Polyline3dVertex>();

            // data storage
            Tuple<float, float, float> offset = new Tuple<float, float, float>(0, 0, 0);
            List<Tuple<float, float, float>> data = new List<Tuple<float, float, float>>();

            Hashtable filter = new Hashtable();

            // track data to use
            bool useData = false;

            log.Info("Start log: " + DateTime.Now);

                MAVLink mine = new MAVLink();
                try
                {
                    mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read));
                }
                catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return new double[] {0}; }

                mine.logreadmode = true;

                mine.MAV.packets.Initialize(); // clear

                // gather data
                while (mine.logplaybackfile.BaseStream.Position < mine.logplaybackfile.BaseStream.Length)
                {
                    byte[] packetraw = mine.readPacket();

                    var packet = mine.DebugPacket(packetraw, false);

                    // this is for packets we dont know about
                    if (packet == null)
                        continue;

                    if (packet.GetType() == typeof(MAVLink.mavlink_vfr_hud_t))
                    {
                        if (((MAVLink.mavlink_vfr_hud_t)packet).throttle >= throttleThreshold)
                        {
                            useData = true;
                        }
                        else
                        {
                            useData = false;
                        }

                    }

                    if (packet.GetType() == typeof(MAVLink.mavlink_sensor_offsets_t))
                    {
                        offset = new Tuple<float, float, float>(
                            ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_x,
                            ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_y,
                            ((MAVLink.mavlink_sensor_offsets_t)packet).mag_ofs_z);
                    }
                    else if (packet.GetType() == typeof(MAVLink.mavlink_raw_imu_t) && useData)
                    {
                        int div = 20;

                        // fox dxf
                        vertex = new Polyline3dVertex(new Vector3f(
                            ((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
                            ((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
                            ((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3)
                            );
                        vertexes.Add(vertex);


                        // for old method
                        setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1, ref minx, ref maxx);
                        setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2, ref miny, ref maxy);
                        setMinorMax(((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3, ref minz, ref maxz);

                        // for new lease sq
                        string item = (int)(((MAVLink.mavlink_raw_imu_t)packet).xmag / div) + "," +
                            (int)(((MAVLink.mavlink_raw_imu_t)packet).ymag / div) + "," +
                            (int)(((MAVLink.mavlink_raw_imu_t)packet).zmag / div);

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

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


                        data.Add(new Tuple<float, float, float>(
                            ((MAVLink.mavlink_raw_imu_t)packet).xmag - offset.Item1,
                            ((MAVLink.mavlink_raw_imu_t)packet).ymag - offset.Item2,
                            ((MAVLink.mavlink_raw_imu_t)packet).zmag - offset.Item3));

                    }

                }

                log.Info("Log Processed " + DateTime.Now);

                Console.WriteLine("Extracted " + data.Count + " data points");
                Console.WriteLine("Current offset: " + offset);

                mine.logreadmode = false;
                mine.logplaybackfile.Close();
                mine.logplaybackfile = null;

                if (data.Count < 10)
                {
                    CustomMessageBox.Show("Log does not contain enough data");
                    throw new Exception("Not Enough Data");
                }

                double[] x = LeastSq(data);

                System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2);

                log.Info("Least Sq Done " + DateTime.Now);

                // create a dxf for those who want to "see" the calibration
                DxfDocument dxf = new DxfDocument();

                Polyline3d polyline = new Polyline3d(vertexes, true);
                polyline.Layer = new Layer("polyline3d");
                polyline.Layer.Color.Index = 24;
                dxf.AddEntity(polyline);

                Point pnt = new Point(new Vector3f(-offset.Item1, -offset.Item2, -offset.Item3));
                pnt.Layer = new Layer("old offset");
                pnt.Layer.Color.Index = 22;
                dxf.AddEntity(pnt);

                pnt = new Point(new Vector3f(-(float)x[0], -(float)x[1], -(float)x[2]));
                pnt.Layer = new Layer("new offset");
                pnt.Layer.Color.Index = 21;
                dxf.AddEntity(pnt);

                dxf.Save("magoffset.dxf", DxfVersion.AutoCad2000);

                log.Info("dxf Done " + DateTime.Now);

                Array.Resize<double>(ref x, 3);

                return x;
        }

        /// <summary>
        /// Does the least sq adjustment to find the center of the sphere
        /// </summary>
        /// <param name="data">list of x,y,z data</param>
        /// <returns>offsets</returns>
        public static double[] LeastSq(List<Tuple<float, float, float>> data)
        {
            double[] x = new double[] { 0, 0, 0, 0 };
            double epsg = 0.0000000001;
            double epsf = 0;
            double epsx = 0;
            int maxits = 0;
            alglib.minlmstate state;
            alglib.minlmreport rep;

            alglib.minlmcreatev(data.Count, x, 100, out state);
            alglib.minlmsetcond(state, epsg, epsf, epsx, maxits);
            alglib.minlmoptimize(state, sphere_error, null, data);
            alglib.minlmresults(state, out x, out rep);

            log.InfoFormat("{0}", rep.terminationtype);
            log.InfoFormat("{0}", alglib.ap.format(x, 2));

            return x;
        }

        /// <summary>
        /// saves the offests to eeprom, os displays if cant
        /// </summary>
        /// <param name="ofs">offsets</param>
        public static void SaveOffsets(double[] ofs)
        {
            if (MainV2.comPort.MAV.param.ContainsKey("COMPASS_OFS_X") && MainV2.comPort.BaseStream.IsOpen)
            {
                try
                {
                    // disable learning
                    MainV2.comPort.setParam("COMPASS_LEARN", 0);
                    // set values
                    MainV2.comPort.setParam("COMPASS_OFS_X", (float)ofs[0]);
                    MainV2.comPort.setParam("COMPASS_OFS_Y", (float)ofs[1]);
                    MainV2.comPort.setParam("COMPASS_OFS_Z", (float)ofs[2]);
                }
                catch { 
                    CustomMessageBox.Show("Set Compass offset failed"); 
                    return; 
                }

                CustomMessageBox.Show("New offsets are " + ofs[0].ToString("0") + " " + ofs[1].ToString("0") + " " + ofs[2].ToString("0") +"\nThese have been saved for you.", "New Mag Offsets");
            }
            else
            {
                CustomMessageBox.Show("New offsets are " + ofs[0].ToString("0") + " " + ofs[1].ToString("0") + " " + ofs[2].ToString("0") + "\n\nPlease write these down for manual entry", "New Mag Offsets");
            }
        }
        /// <summary>
        /// Min or max finder
        /// </summary>
        /// <param name="value">value to process</param>
        /// <param name="min">current min</param>
        /// <param name="max">current max</param>
        private static void setMinorMax(float value, ref float min, ref float max) 
        {
            if (value > max)         
                max = value;
            if (value < min)
                min = value;
        }


        static void sphere_error(double[] xi, double[] fi, object obj)
        {
            double xofs = xi[0];
            double yofs = xi[1];
            double zofs = xi[2];
            double r = xi[3];
            int a = 0;
            foreach (var d in (List<Tuple<float, float, float>>)obj)
            {
                double x = d.Item1;
                double y = d.Item2;
                double z = d.Item3;
                double err = r - Math.Sqrt(Math.Pow((x + xofs), 2) + Math.Pow((y + yofs), 2) + Math.Pow((z + zofs), 2));
                fi[a] = err;
                a++;
            }
        }
    }
}