ardupilot/Tools/ArdupilotMegaPlanner/Camera.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO;
using System.Xml;
using System.Collections;

namespace ArdupilotMega
{
    public partial class Camera : Form
    {
        const float rad2deg = (float)(180 / Math.PI);
        const float deg2rad = (float)(1.0 / rad2deg);

        Dictionary<string, camerainfo> cameras = new Dictionary<string, camerainfo>();

        public struct camerainfo
        {
            public string name;
            public float focallen;
            public float sensorwidth;
            public float sensorheight;
            public float imagewidth;
            public float imageheight;
        }

        public Camera()
        {
            InitializeComponent();
        }

        void doCalc()
        {
            // entered values
            float focallen = (float)num_focallength.Value;
            float flyalt = (float)num_agl.Value;
            int imagewidth = int.Parse(TXT_imgwidth.Text);
            int imageheight = int.Parse(TXT_imgheight.Text);

            float sensorwidth = float.Parse(TXT_senswidth.Text);
            float sensorheight = float.Parse(TXT_sensheight.Text);

            int overlap = (int)num_overlap.Value;
            int sidelap = (int)num_sidelap.Value;


            // scale
            float flscale = 1000 * flyalt / focallen;

            float viewwidth = (sensorwidth * flscale / 1000);
            float viewheight = (sensorheight * flscale / 1000);

            TXT_fovH.Text = (viewwidth).ToString();
            TXT_fovV.Text = (viewheight).ToString();

            TXT_fovAH.Text = (Math.Atan(sensorwidth / (2 * focallen)) * rad2deg * 2).ToString();
            TXT_fovAV.Text = (Math.Atan(sensorheight / (2 * focallen)) * rad2deg * 2).ToString();

            TXT_cmpixel.Text = ((viewheight / imageheight) * 100).ToString("0.00 cm");

            if (CHK_camdirection.Checked)
            {
                TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewheight).ToString();
                TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewwidth).ToString();
            }
            else
            {
                TXT_distflphotos.Text = ((1 - (overlap / 100.0f)) * viewwidth).ToString();
                TXT_distacflphotos.Text = ((1 - (sidelap / 100.0f)) * viewheight).ToString();
            }
        }

        private void num_agl_ValueChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void num_focallength_ValueChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void num_overlap_ValueChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void num_sidelap_ValueChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void CHK_camdirection_CheckedChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void TXT_imgwidth_TextChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void TXT_imgheight_TextChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void TXT_senswidth_TextChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void TXT_sensheight_TextChanged(object sender, EventArgs e)
        {
            doCalc();
        }

        private void CMB_camera_SelectedIndexChanged(object sender, EventArgs e)
        {
            if (cameras.ContainsKey(CMB_camera.Text))
            {
                camerainfo camera = cameras[CMB_camera.Text];

                num_focallength.Value = (decimal)camera.focallen;
                TXT_imgheight.Text = camera.imageheight.ToString();
                TXT_imgwidth.Text = camera.imagewidth.ToString();
                TXT_sensheight.Text = camera.sensorheight.ToString();
                TXT_senswidth.Text = camera.sensorwidth.ToString();
            }

            doCalc();
        }

        private void xmlcamera(bool write)
        {
            string filename = "cameras.xml";

            if (write || !File.Exists(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + filename))
            {
                try
                {
                    XmlTextWriter xmlwriter = new XmlTextWriter(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + filename, Encoding.ASCII);
                    xmlwriter.Formatting = Formatting.Indented;

                    xmlwriter.WriteStartDocument();

                    xmlwriter.WriteStartElement("Cameras");

                    foreach (string key in cameras.Keys)
                    {
                        try
                        {
                            if (key == "")
                                continue;
                            xmlwriter.WriteStartElement("Camera");
                            xmlwriter.WriteElementString("name", cameras[key].name);
                            xmlwriter.WriteElementString("flen", cameras[key].focallen.ToString(new System.Globalization.CultureInfo("en-US")));
                            xmlwriter.WriteElementString("imgh", cameras[key].imageheight.ToString(new System.Globalization.CultureInfo("en-US")));
                            xmlwriter.WriteElementString("imgw", cameras[key].imagewidth.ToString(new System.Globalization.CultureInfo("en-US")));
                            xmlwriter.WriteElementString("senh", cameras[key].sensorheight.ToString(new System.Globalization.CultureInfo("en-US")));
                            xmlwriter.WriteElementString("senw", cameras[key].sensorwidth.ToString(new System.Globalization.CultureInfo("en-US")));
                            xmlwriter.WriteEndElement();
                        }
                        catch { }
                    }

                    xmlwriter.WriteEndElement();

                    xmlwriter.WriteEndDocument();
                    xmlwriter.Close();

                }
                catch (Exception ex) { MessageBox.Show(ex.ToString()); }
            }
            else
            {
                try
                {
                    using (XmlTextReader xmlreader = new XmlTextReader(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + filename))
                    {
                        while (xmlreader.Read())
                        {
                            xmlreader.MoveToElement();
                            try
                            {
                                switch (xmlreader.Name)
                                {
                                    case "Camera":
                                        {
                                            camerainfo camera = new camerainfo();

                                            while (xmlreader.Read())
                                            {
                                                bool dobreak = false;
                                                xmlreader.MoveToElement();
                                                switch (xmlreader.Name)
                                                {
                                                    case "name":
                                                        camera.name = xmlreader.ReadString();
                                                        break;
                                                    case "imgw":
                                                        camera.imagewidth = float.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
                                                        break;
                                                    case "imgh":
                                                        camera.imageheight = float.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
                                                        break;
                                                    case "senw":
                                                        camera.sensorwidth = float.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
                                                        break;
                                                    case "senh":
                                                        camera.sensorheight = float.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
                                                        break;
                                                    case "flen":
                                                        camera.focallen= float.Parse(xmlreader.ReadString(), new System.Globalization.CultureInfo("en-US"));
                                                        break;
                                                    case "Camera":
                                                        cameras.Add(camera.name,camera);
                                                        CMB_camera.Items.Add(camera.name);
                                                        dobreak = true;
                                                        break;
                                                }
                                                if (dobreak)
                                                    break;
                                            }
                                            string temp = xmlreader.ReadString();
                                        }
                                        break;
                                    case "Config":
                                        break;
                                    case "xml":
                                        break;
                                    default:
                                        if (xmlreader.Name == "") // line feeds
                                            break;
                                        //config[xmlreader.Name] = xmlreader.ReadString();
                                        break;
                                }
                            }
                            catch (Exception ee) { Console.WriteLine(ee.Message); } // silent fail on bad entry
                        }
                    }
                }
                catch (Exception ex) { Console.WriteLine("Bad Camera File: " + ex.ToString()); } // bad config file
            }
        }

        private void BUT_save_Click(object sender, EventArgs e)
        {
            camerainfo camera = new camerainfo();

            // check if camera exists alreay
            if (cameras.ContainsKey(CMB_camera.Text))
            {
                camera = cameras[CMB_camera.Text];
            }
            else
            {
                cameras.Add(CMB_camera.Text, camera);
            }

            try
            {
                camera.name = CMB_camera.Text;
                camera.focallen = (float)num_focallength.Value;
                camera.imageheight = float.Parse(TXT_imgheight.Text);
                camera.imagewidth = float.Parse(TXT_imgwidth.Text);
                camera.sensorheight = float.Parse(TXT_sensheight.Text);
                camera.sensorwidth = float.Parse(TXT_senswidth.Text);
            }
            catch { MessageBox.Show("One of your entries is not a valid number"); return; }

            cameras[CMB_camera.Text] = camera;

            xmlcamera(true);
        }

        private void Camera_Load(object sender, EventArgs e)
        {
            xmlcamera(false);
        }

    }
}