ardupilot/Tools/ArdupilotMegaPlanner/RAW_Sensor.cs

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using ZedGraph;
using AGaugeApp;
using System.IO.Ports;
using System.Threading;

namespace ArdupilotMega
{
    public partial class RAW_Sensor : Form
    {
        // for graph
        RollingPointPairList list1 = new RollingPointPairList(10 * 50);
        RollingPointPairList list2 = new RollingPointPairList(10 * 50);
        RollingPointPairList list3 = new RollingPointPairList(10 * 50);
        RollingPointPairList list4 = new RollingPointPairList(10 * 50);
        RollingPointPairList list5 = new RollingPointPairList(10 * 50);
        RollingPointPairList list6 = new RollingPointPairList(10 * 50);
        object thisLock = new object();

        int tickStart = 0;

        MAVLink comPort = new MAVLink();

        public RAW_Sensor()
        {
            InitializeComponent();

            CreateChart(zg1, "Raw Sensors", "Time", "Raw Data");
        }

        public struct plot
        {
            public string Name;
            public RollingPointPairList PointList;
            public Color color;
        }

        public void CreateChart(ZedGraphControl zgc,string Title, string XAxis, string YAxis)
        {
            GraphPane myPane = zgc.GraphPane;

            // Set the titles and axis labels
            myPane.Title.Text = Title;
            myPane.XAxis.Title.Text = XAxis;
            myPane.YAxis.Title.Text = YAxis;

            LineItem myCurve;

            myCurve = myPane.AddCurve("Accel X", list1, Color.Red, SymbolType.None);
            myCurve = myPane.AddCurve("Accel Y", list2, Color.Green, SymbolType.None);
            myCurve = myPane.AddCurve("Accel Z", list3, Color.SandyBrown, SymbolType.None);
            myCurve = myPane.AddCurve("Gyro X", list4, Color.Blue, SymbolType.None);
            myCurve = myPane.AddCurve("Gyro Y", list5, Color.Black, SymbolType.None);
            myCurve = myPane.AddCurve("Gyro Z", list6, Color.Violet, SymbolType.None);


            // Show the x axis grid
            myPane.XAxis.MajorGrid.IsVisible = true;

            myPane.XAxis.Scale.Min = 0;
            myPane.XAxis.Scale.Max = 5;

            // Make the Y axis scale red
            myPane.YAxis.Scale.FontSpec.FontColor = Color.Red;
            myPane.YAxis.Title.FontSpec.FontColor = Color.Red;
            // turn off the opposite tics so the Y tics don't show up on the Y2 axis
            myPane.YAxis.MajorTic.IsOpposite = false;
            myPane.YAxis.MinorTic.IsOpposite = false;
            // Don't display the Y zero line
            myPane.YAxis.MajorGrid.IsZeroLine = true;
            // Align the Y axis labels so they are flush to the axis
            myPane.YAxis.Scale.Align = AlignP.Inside;
            // Manually set the axis range
            //myPane.YAxis.Scale.Min = -1;
            //myPane.YAxis.Scale.Max = 1;

            // Fill the axis background with a gradient
            myPane.Chart.Fill = new Fill(Color.White, Color.LightGray, 45.0f);

            // Sample at 20ms intervals
            timer1.Interval = 100;
            timer1.Enabled = true;
            timer1.Start();


            // Calculate the Axis Scale Ranges
            zgc.AxisChange();

            tickStart = Environment.TickCount;


        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            double time = (Environment.TickCount - tickStart) / 1000.0;

            // Make sure that the curvelist has at least one curve
            if (zg1.GraphPane == null || zg1.GraphPane.CurveList.Count <= 0)
                return;

            // Get the first CurveItem in the graph
            LineItem curve = zg1.GraphPane.CurveList[0] as LineItem;
            if (curve == null)
                return;

            // Get the PointPairList
            IPointListEdit list = curve.Points as IPointListEdit;
            // If this is null, it means the reference at curve.Points does not
            // support IPointListEdit, so we won't be able to modify it
            if (list == null)
                return;

            // Time is measured in seconds
            //double time = (Environment.TickCount - tickStart) / 1000.0;

            // Keep the X scale at a rolling 30 second interval, with one
            // major step between the max X value and the end of the axis
            Scale xScale = zg1.GraphPane.XAxis.Scale;
            if (time > xScale.Max - xScale.MajorStep)
            {
                xScale.Max = time + xScale.MajorStep;
                xScale.Min = xScale.Max - 10.0;
            }

            // Make sure the Y axis is rescaled to accommodate actual data
            try
            {
                zg1.AxisChange();
            }
            catch { }
            // Force a redraw
            zg1.Invalidate();
        }

        private void timer2serial_Tick(object sender, EventArgs e)
        {
            if (!comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
                return;

            //Console.WriteLine(DateTime.Now.Millisecond + " timer2 serial");
            try
            {
                MainV2.cs.UpdateCurrentSettings(currentStateBindingSource);
            }
            catch { }

            if (sw != null && sw.BaseStream.CanWrite)
            {
                sw.WriteLine(string.Format("{0},{1},{2},{3},{4},{5},{6}",DateTime.Now.ToString(), MainV2.cs.ax, MainV2.cs.ay, MainV2.cs.az, MainV2.cs.gx,MainV2.cs.gy, MainV2.cs.gz));
            }

            double time = (Environment.TickCount - tickStart) / 1000.0;

            if (chkax.Checked)
            {
                list1.Add(time, ArdupilotMega.MainV2.cs.ax);
            }
            else { list1.Clear(); }
            if (chkay.Checked)
            {
                list2.Add(time, ArdupilotMega.MainV2.cs.ay);
            }
            else { list2.Clear(); }
            if (chkaz.Checked)
            {
                list3.Add(time, ArdupilotMega.MainV2.cs.az);
            }
            else { list3.Clear(); }
            if (chkgx.Checked)
            {
                list4.Add(time, ArdupilotMega.MainV2.cs.gx);
            }
            else { list4.Clear(); }
            if (chkgy.Checked)
            {
                list5.Add(time, ArdupilotMega.MainV2.cs.gy);
            }
            else { list5.Clear(); }
            if (chkgz.Checked)
            {
                list6.Add(time, ArdupilotMega.MainV2.cs.gz);
            }
            else { list6.Clear(); }
        }

        private void ACM_Setup_Load(object sender, EventArgs e)
        {

            timer2serial.Interval = 10;
            timer2serial.Enabled = true;
            timer2serial.Start();

            tabControl.SelectedTab = tabRadio;

            tabControl.SelectedTab = tabRawSensor;

            //tabControl1_SelectedIndexChanged(sender, e);


        }

        private void ACM_Setup_FormClosed(object sender, FormClosedEventArgs e)
        {
            if (comPort != null && comPort.BaseStream.IsOpen)
            {
                try
                {
                    if (sw != null)
                        sw.Close();

                    MainV2.cs.ratesensors = 3; // hardcode 3 hz

                    comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors); // request raw sensor
                }
                catch { }
            }
            timer1.Stop();
            timer2serial.Stop();
        }

        private void tabControl1_SelectedIndexChanged(object sender, EventArgs e)
        {
                    try
                    {
                        comPort = MainV2.comPort;

                        if (!comPort.BaseStream.IsOpen && !MainV2.comPort.logreadmode)
                        {
                            CustomMessageBox.Show("Please connect first");
                            this.Close();
                        }

                        //comPort.DtrEnable = true;
                        //comPort.Open();
                        //comPort.stopall(true); // ensure off

                        Console.WriteLine("Req streams {0} {1}", comPort.bps, DateTime.Now);
                        //comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTENDED_STATUS, 0); // mode gps raw
                        //comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.POSITION, 3); // request location
                        //comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA1, 3); // request attitude
                        //comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.EXTRA2, 3); // request vfr
                        comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RAW_SENSORS, MainV2.cs.ratesensors); // request raw sensor
                        //comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RC_CHANNELS, 3); // request rc info
                    }
                    catch
                    {
                        CustomMessageBox.Show("Comport open failed");
                        return;
                    }
                timer1.Start();
        }

        private void CMB_rawupdaterate_SelectedIndexChanged(object sender, EventArgs e)
        {
            MainV2.cs.ratesensors = (byte)int.Parse(CMB_rawupdaterate.Text);
            comPort.requestDatastream((byte)ArdupilotMega.MAVLink.MAV_DATA_STREAM.RAW_SENSORS, (byte)int.Parse(CMB_rawupdaterate.Text)); // request raw sensor
        }

        System.IO.StreamWriter sw = null;

        private void BUT_savecsv_Click(object sender, EventArgs e)
        {
            SaveFileDialog ofd = new SaveFileDialog();
            ofd.AddExtension = true;
            ofd.DefaultExt = ".csv";
            ofd.ShowDialog();
            if (ofd.FileName != "")
            {
                sw = new System.IO.StreamWriter(ofd.OpenFile());
            }
        }

    }


}