From b9fb1750d52f27a0b844e018202a307d875ddfe7 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 25 Aug 2012 21:48:06 +0800 Subject: [PATCH] Mission Planner Cleanup --- .gitignore | 4 + Tools/ArdupilotMegaPlanner/HIL/FlashQuad.cs | 364 ------------------ .../ArdupilotMegaPlanner/Mavlink/IMAVLink.cs | 120 ------ .../SerialInput.Designer.cs | 134 ------- Tools/ArdupilotMegaPlanner/SerialInput.cs | 225 ----------- .../SerialInput.es-ES.resx | 123 ------ .../ArdupilotMegaPlanner/SerialInput.pl.resx | 131 ------- Tools/ArdupilotMegaPlanner/SerialInput.resx | 208 ---------- 8 files changed, 4 insertions(+), 1305 deletions(-) delete mode 100644 Tools/ArdupilotMegaPlanner/HIL/FlashQuad.cs delete mode 100644 Tools/ArdupilotMegaPlanner/Mavlink/IMAVLink.cs delete mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs delete mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.cs delete mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.es-ES.resx delete mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.pl.resx delete mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.resx diff --git a/.gitignore b/.gitignore index da602d3434..5bc2cf79a3 100644 --- a/.gitignore +++ b/.gitignore @@ -31,3 +31,7 @@ status.txt *.dll *.obj *.zip + +/Tools/ArdupilotMegaPlanner/3DRRadio/bin +/Tools/ArdupilotMegaPlanner/3DRRadio/obj +/Tools/ArdupilotMegaPlanner/bin \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/HIL/FlashQuad.cs b/Tools/ArdupilotMegaPlanner/HIL/FlashQuad.cs deleted file mode 100644 index 97c07c965a..0000000000 --- a/Tools/ArdupilotMegaPlanner/HIL/FlashQuad.cs +++ /dev/null @@ -1,364 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using System.Text; -using Sharp3D.Math.Core; - -namespace ArdupilotMega.HIL -{ - class FlashQuad - { - public ahrsc ahrs = new ahrsc() ;// :AHRS; - public Vector3D loc = new Vector3D() ;// :Location; - public param g = new param() ;// :Parameters; - //public var apm_rc :APM_RC; - //public var motor_filter_0 :AverageFilter; - //public var motor_filter_1 :AverageFilter; - //public var motor_filter_2 :AverageFilter; - //public var motor_filter_3 :AverageFilter; - public Vector3D drag = new Vector3D();// :Vector3D; // - public Vector3D airspeed = new Vector3D();// :Vector3D; // - //public var thrust :Vector3D; // - public Vector3D position = new Vector3D();// :Vector3D; // - public Vector3D velocity = new Vector3D();// :Vector3D; - //public var velocity_old :Vector3D; - //public var wind :Point; // - public Vector3D rot_accel = new Vector3D();// :Vector3D; // - public Vector3D angle3D = new Vector3D();// :Vector3D; // - //public var windGenerator :Wind; // - - public double gravity = 980.5; - public double thrust_scale = 0.4; - public double throttle = 500; - public double rotation_bias = 1; - - private double _jump_z = 0; - private Vector3D v3 = new Vector3D(); - - public class ahrsc - { - public Matrix3D dcm = new Matrix3D(Matrix3D.Identity); - public Vector3D gyro = new Vector3D(); - public Vector3D omega = new Vector3D(); - public Vector3D accel = new Vector3D(); - public Vector3D rotation_speed = new Vector3D(); - - public double roll_sensor; - public double pitch_sensor; - public double yaw_sensor; - } - - public class param - { - // --------------------------------------------- - // Sim Details controls - // --------------------------------------------- - public int sim_iterations = 99999; - public int sim_speed = 1; - - public double windSpeedMin = 150; - public double windSpeedMax = 200; - public double windPeriod = 30000; - public double windDir = 45; - - public double airDensity = 1.184; - //public var crossSection :Number = 0.015; - public double crossSection = 0.012; - public double dragCE = 0.20; - public double speed_filter_size = 2; - public double motor_kv = 1000; - public double moment = 3; - public double mass = 500; - public int esc_delay = 12; - - // ----------------------------------------- - // Inertial control - // ----------------------------------------- - public double speed_correction_z = 0.0350; - public double xy_speed_correction = 0.030; - public double xy_offset_correction = 0.00001; - public double xy_pos_correction = 0.08; - - public double z_offset_correction = 0.00004; - public double z_pos_correction = 0.2; - - public double accel_bias_x = .9; - public double accel_bias_z = .9; - public double accel_bias_y = .9; - } - - double scale_rc(int sn, float servo, float min, float max) - { - float min_pwm = 1000; - float max_pwm = 2000; - //'''scale a PWM value''' # default to servo range of 1000 to 2000 - if (MainV2.comPort.param.Count > 0) - { - min_pwm = int.Parse(MainV2.comPort.param["RC3_MIN"].ToString()); - max_pwm = int.Parse(MainV2.comPort.param["RC3_MAX"].ToString()); - } - - float p = (servo - min_pwm) / (max_pwm - min_pwm); - - float v = min + p * (max - min); - - if (v < min) - v = min; - if (v > max) - v = max; - return v * 1000; - } - - public void update(ref double[] motor_output, double dt) - { - var _thrust = 0.0; - rot_accel = new Vector3D(0,0,0); - angle3D.X = angle3D.Y = 0; - angle3D.Z = 1; - - // wind = windGenerator.read(); - - // ESC's moving average filter - // var motor_output:Array = new Array(4); - // motor_output[0] = motor_filter_0.apply(apm_rc.get_motor_output(0)); - // motor_output[1] = motor_filter_1.apply(apm_rc.get_motor_output(1)); - // motor_output[2] = motor_filter_2.apply(apm_rc.get_motor_output(2)); - // motor_output[3] = motor_filter_3.apply(apm_rc.get_motor_output(3)); - - - for (int i = 0; i < motor_output.Length; i++) - { - if (motor_output[i] <= 0.0) - { - motor_output[i] = 0; - } - else - { - motor_output[i] = scale_rc(i, (float)motor_output[i], 0.0f, 1.0f); - //servos[i] = motor_speed[i]; - } - } - -/* - 2 - - 1 0 - - 3 - -*/ - - // setup motor rotations - rot_accel.X -= g.motor_kv * motor_output[0]; // roll - rot_accel.X += g.motor_kv * motor_output[1]; - rot_accel.Y -= g.motor_kv * motor_output[3]; - rot_accel.Y += g.motor_kv * motor_output[2]; - - rot_accel.Z += g.motor_kv * motor_output[0] * .08; // YAW - rot_accel.Z += g.motor_kv * motor_output[1] * .08; - rot_accel.Z -= g.motor_kv * motor_output[2] * .08; - rot_accel.Z -= g.motor_kv * motor_output[3] * .08; - - rot_accel.X /= g.moment; - rot_accel.Y /= g.moment; - rot_accel.Z /= g.moment; - - //# rotational air resistance - - // Gyro is the rotation speed in deg/s - // update rotational rates in body frame - ahrs.gyro.X += rot_accel.X * dt; - ahrs.gyro.Y += rot_accel.Y * dt; - ahrs.gyro.Z += rot_accel.Z * dt; - - //ahrs.gyro.z += 200; - ahrs.gyro.Z *= .995;// some drag - - // ahrs.dcm = Matrix3D.Identity; - - // move earth frame to body frame - Vector3D tmp = Matrix3D.Transform(ahrs.dcm,ahrs.gyro) ;//ahrs.dcm.transformVector(ahrs.gyro); - - // update attitude: - ahrs.dcm += new Matrix3D(new Vector3D((tmp.X/100) * dt,0,0),new Vector3D(0,(tmp.Y/100) * dt,0),new Vector3D(0,0,(tmp.Z/100) * dt)); - - //ahrs.dcm.appendRotation((tmp.X/100) * dt, Vector3D.X_AXIS); // ROLL - //ahrs.dcm.appendRotation((tmp.Y/100) * dt, Vector3D.Y_AXIS); // PITCH - //ahrs.dcm.appendRotation((tmp.Z/100) * dt, Vector3D.Z_AXIS); // YAW - - // ------------------------------------ - // calc thrust - // ------------------------------------ - - //get_motor_output returns 0 : 1000 - _thrust += motor_output[0] * thrust_scale; - _thrust += motor_output[1] * thrust_scale; - _thrust += motor_output[2] * thrust_scale; - _thrust += motor_output[3] * thrust_scale; - - Vector3D accel_body = new Vector3D(0, 0, (_thrust * -.9) / g.mass); - //var accel_body:Vector3D = new Vector3D(0, 0, 0); - Vector3D accel_earth = Matrix3D.Transform(ahrs.dcm,(accel_body)); - angle3D = Matrix3D.Transform(ahrs.dcm,(angle3D)); - - //trace(ahrs.gyro.y, accel_earth.x); - - //trace(ahrs.gyro.x, ahrs.gyro.y, ahrs.gyro.z); - - // ------------------------------------ - // calc copter velocity - // ------------------------------------ - // calc Drag - drag.X = .5 * g.airDensity * airspeed.X * airspeed.X * g.dragCE * g.crossSection; - drag.Y = .5 * g.airDensity * airspeed.Y * airspeed.Y * g.dragCE * g.crossSection; - drag.Z = .5 * g.airDensity * airspeed.Z * airspeed.Z * g.dragCE * g.crossSection; - - ///* - // this calulation includes wind - if(airspeed.X >= 0) - accel_earth.X -= drag.X; - else - accel_earth.X += drag.X; - - // Add in Drag - if(airspeed.Y >= 0) - accel_earth.Y -= drag.Y; - else - accel_earth.Y += drag.Y; - - if(airspeed.Z <= 0) - accel_earth.Z -= drag.Z; - else - accel_earth.Z += drag.Z; - //*/ - - // hacked vert disturbance - accel_earth.Z += _jump_z * dt; - _jump_z *= .999; - - - // Add in Gravity - accel_earth.Z += gravity; - - if(accel_earth.Z < 0) - accel_earth.Z *=.9; - - - if(position.Z <=.11 && accel_earth.Z > 0){ - accel_earth.Z = 0; - } - - velocity.X += (accel_earth.X * dt); // + : Forward (North) - velocity.Y += (accel_earth.Y * dt); // + : Right (East) - velocity.Z -= (accel_earth.Z * dt); // + : Up - - - //trace(Math.floor(velocity.x),Math.floor(velocity.y),Math.floor(velocity.z)); - - // ------------------------------------ - // calc inertia - // ------------------------------------ - - // work out acceleration as seen by the accelerometers. It sees the kinematic - // acceleration (ie. real movement), plus gravity - Matrix3D dcm_t = ahrs.dcm.Clone(); - dcm_t.Transpose(); - double t = accel_earth.Z; - accel_earth.Z -= gravity; - ahrs.accel = Matrix3D.Transform(dcm_t,(accel_earth)); - - ahrs.accel *= 0.01; - - //ahrs.accel = accel_earth.clone(); - ahrs.accel.X *= g.accel_bias_x; - ahrs.accel.Y *= g.accel_bias_y; - ahrs.accel.Z *= g.accel_bias_z; - - - - // ------------------------------------ - // calc Position - // ------------------------------------ - position.Y += velocity.X * dt; - position.X += velocity.Y * dt; - position.Z += velocity.Z * dt; - position.Z = Math.Min(position.Z, 4000); - - // XXX Force us to 3m above ground - //position.z = 300; - - airspeed.X = (velocity.X);// - wind.x); - airspeed.Y = (velocity.Y);// - wind.y); - airspeed.Z = velocity.Z; - - // Altitude - // -------- - if(position.Z <=.1){ - position.Z = .1; - velocity.X = 0; - velocity.Y = 0; - velocity.Z = 0; - airspeed.X = 0; - airspeed.Y = 0; - airspeed.Z = 0; - //ahrs.init(); - } - - - // get omega - the simulated Gyro output - ahrs.omega.X = radiansx100(ahrs.gyro.X); - ahrs.omega.Y = radiansx100(ahrs.gyro.Y); - ahrs.omega.Z = radiansx100(ahrs.gyro.Z); - - // get the Eulers output - v3 = new Vector3D(Math.Atan2(-ahrs.dcm.M23, ahrs.dcm.M22), Math.Asin(ahrs.dcm.M21), Math.Atan2(-ahrs.dcm.M31, ahrs.dcm.M11));// ahrs.dcm.decompose(); - ahrs.roll_sensor = Math.Floor(degrees(v3.X) * 100); - ahrs.pitch_sensor = Math.Floor(degrees(v3.Y) * 100); - ahrs.yaw_sensor = Math.Floor(degrees(v3.Z) * 100); - - // store the position for the GPS object - loc.X = position.X; - loc.Y = position.Y; - loc.Z = position.Z; - } - - public double constrain(double val, double min, double max) - { - val = Math.Max(val, min); - val = Math.Min(val, max); - return val; - } - - public double wrap_180(int error) - { - if (error > 18000) error -= 36000; - if (error < -18000) error += 36000; - return error; - } - - public int wrap_360(int error) - { - if (error > 36000) error -= 36000; - if (error < 0) error += 36000; - return error; - } - - public double radiansx100(double n) - { - return 0.000174532925 * n; - } - - public double degreesx100(double r) - { - return r * 5729.57795; - } - public double degrees(double r) - { - return r * 57.2957795; - } - public double radians(double n) - { - return 0.0174532925 * n; - } - } -} diff --git a/Tools/ArdupilotMegaPlanner/Mavlink/IMAVLink.cs b/Tools/ArdupilotMegaPlanner/Mavlink/IMAVLink.cs deleted file mode 100644 index b074f5e0ce..0000000000 --- a/Tools/ArdupilotMegaPlanner/Mavlink/IMAVLink.cs +++ /dev/null @@ -1,120 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Reactive.Subjects; -using System.Text; -using System.Runtime.InteropServices; -using System.Collections; // hashs -using System.Diagnostics; // stopwatch -using System.Reflection; -using System.Reflection.Emit; -using System.IO; -using System.Drawing; -using System.Threading; -using ArdupilotMega.Controls; -using System.ComponentModel; -using log4net; -using ArdupilotMega.Comms; -using ArdupilotMega.Utilities; -namespace ArdupilotMega -{ - public interface IMAVLink - { - // there is too much message intergration in the main code, so i dont think i will pursue this any further - // if it an't broke dont fix it - mav09 and mav10 are compile time defined - // i will probly revert this change at some point, unless someone else has a good idea. - - // Fields - - byte[][] packets { get; set; } - - - double[] packetspersecond { get; set; } - - Hashtable param { get; set; } - - BinaryWriter rawlogfile { get; set; } - - - - byte sysid { get; set; } - Subject WhenPacketLost { get; set; } - Subject WhenPacketReceived { get; set; } - PointLatLngAlt[] wps { get; set; } - - - ArdupilotMega.MAVLink.MAV_TYPE aptype { get; set; } - ICommsSerial BaseStream { get; set; } - int bps { get; set; } - - - DateTime bpstime { get; set; } - byte compid { get; set; } - - bool debugmavlink { get; set; } - - - DateTime lastlogread { get; set; } - DateTime lastvalidpacket { get; set; } - - BinaryWriter logfile { get; set; } - BinaryReader logplaybackfile { get; set; } - bool logreadmode { get; set; } - - // Events - - event EventHandler ParamListChanged; - - // Methods - - - void Close(); - object DebugPacket(byte[] datin); - object DebugPacket(byte[] datin, bool PrintToConsole); - object DebugPacket(byte[] datin, ref string text); - object DebugPacket(byte[] datin, ref string text, bool PrintToConsole, string delimeter); - - // mav 09 - bool doAction(object actionid); - // mac 10 - bool doCommand(ArdupilotMega.MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7); - - void setAPType(); - - - PointLatLngAlt getFencePoint(int no, ref int total); - - void getParamList(); - - Locationwp getWP(ushort index); - byte getWPCount(); - - - // static void modifyParamForDisplay(bool fromapm, string paramname, ref float value); - void Open(); - void Open(bool getparams); - - - - byte[] readPacket(); - void requestDatastream(byte id, byte hzrate); - void sendPacket(object indata); - bool setFencePoint(byte index, PointLatLngAlt plla, byte fencepointcount); - void setMode(string modein); - void setMode(ArdupilotMega.MAVLink.mavlink_set_mode_t mode, ArdupilotMega.MAVLink.MAV_MODE_FLAG base_mode = 0); - void setMountConfigure(ArdupilotMega.MAVLink.MAV_MOUNT_MODE mountmode, bool stabroll, bool stabpitch, bool stabyaw); - void setMountControl(double pa, double pb, double pc, bool islatlng); - bool setParam(string paramname, float value); - void setWP(Locationwp loc, ushort index, ArdupilotMega.MAVLink.MAV_FRAME frame, byte current); - void setWPACK(); - bool setWPCurrent(ushort index); - void setWPTotal(ushort wp_total); - void stopall(bool forget); - bool Write(string line); - - // Properties - - IObservable BytesReceived { get; } - IObservable BytesSent { get; } - - } -} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs b/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs deleted file mode 100644 index 06d99e0a85..0000000000 --- a/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs +++ /dev/null @@ -1,134 +0,0 @@ -namespace ArdupilotMega -{ - partial class SerialInput - { - /// - /// Required designer variable. - /// - private System.ComponentModel.IContainer components = null; - - /// - /// Clean up any resources being used. - /// - /// true if managed resources should be disposed; otherwise, false. - protected override void Dispose(bool disposing) - { - if (disposing && (components != null)) - { - components.Dispose(); - } - base.Dispose(disposing); - } - - #region Windows Form Designer generated code - - /// - /// Required method for Designer support - do not modify - /// the contents of this method with the code editor. - /// - private void InitializeComponent() - { - System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(SerialInput)); - this.CMB_serialport = new System.Windows.Forms.ComboBox(); - this.BUT_connect = new ArdupilotMega.Controls.MyButton(); - this.CMB_baudrate = new System.Windows.Forms.ComboBox(); - this.label1 = new System.Windows.Forms.Label(); - this.LBL_location = new System.Windows.Forms.Label(); - this.textBox1 = new System.Windows.Forms.TextBox(); - this.SuspendLayout(); - // - // CMB_serialport - // - this.CMB_serialport.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; - this.CMB_serialport.FormattingEnabled = true; - this.CMB_serialport.Location = new System.Drawing.Point(13, 13); - this.CMB_serialport.Name = "CMB_serialport"; - this.CMB_serialport.Size = new System.Drawing.Size(121, 21); - this.CMB_serialport.TabIndex = 0; - // - // BUT_connect - // - this.BUT_connect.Location = new System.Drawing.Point(279, 12); - this.BUT_connect.Name = "BUT_connect"; - this.BUT_connect.Size = new System.Drawing.Size(75, 23); - this.BUT_connect.TabIndex = 1; - this.BUT_connect.Text = "Connect"; - this.BUT_connect.UseVisualStyleBackColor = true; - this.BUT_connect.Click += new System.EventHandler(this.BUT_connect_Click); - // - // CMB_baudrate - // - this.CMB_baudrate.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; - this.CMB_baudrate.FormattingEnabled = true; - this.CMB_baudrate.Items.AddRange(new object[] { - "4800", - "9600", - "14400", - "19200", - "28800", - "38400", - "57600", - "115200"}); - this.CMB_baudrate.Location = new System.Drawing.Point(140, 12); - this.CMB_baudrate.Name = "CMB_baudrate"; - this.CMB_baudrate.Size = new System.Drawing.Size(121, 21); - this.CMB_baudrate.TabIndex = 2; - // - // label1 - // - this.label1.AutoSize = true; - this.label1.Location = new System.Drawing.Point(90, 47); - this.label1.Name = "label1"; - this.label1.Size = new System.Drawing.Size(187, 13); - this.label1.TabIndex = 3; - this.label1.Text = "Pick the Nmea gps port and baud rate\r\n"; - // - // LBL_location - // - this.LBL_location.Font = new System.Drawing.Font("Microsoft Sans Serif", 14.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - this.LBL_location.Location = new System.Drawing.Point(3, 64); - this.LBL_location.Name = "LBL_location"; - this.LBL_location.Size = new System.Drawing.Size(365, 59); - this.LBL_location.TabIndex = 4; - this.LBL_location.Text = "0,0,0"; - // - // textBox1 - // - this.textBox1.Enabled = false; - this.textBox1.Location = new System.Drawing.Point(19, 126); - this.textBox1.Multiline = true; - this.textBox1.Name = "textBox1"; - this.textBox1.Size = new System.Drawing.Size(335, 133); - this.textBox1.TabIndex = 5; - this.textBox1.Text = resources.GetString("textBox1.Text"); - // - // SerialInput - // - this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); - this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; - this.ClientSize = new System.Drawing.Size(369, 300); - this.Controls.Add(this.textBox1); - this.Controls.Add(this.LBL_location); - this.Controls.Add(this.label1); - this.Controls.Add(this.CMB_baudrate); - this.Controls.Add(this.BUT_connect); - this.Controls.Add(this.CMB_serialport); - this.Icon = ((System.Drawing.Icon)(resources.GetObject("$this.Icon"))); - this.Name = "SerialInput"; - this.Text = "Follow Me"; - this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.SerialOutput_FormClosing); - this.ResumeLayout(false); - this.PerformLayout(); - - } - - #endregion - - private System.Windows.Forms.ComboBox CMB_serialport; - private ArdupilotMega.Controls.MyButton BUT_connect; - private System.Windows.Forms.ComboBox CMB_baudrate; - private System.Windows.Forms.Label label1; - private System.Windows.Forms.Label LBL_location; - private System.Windows.Forms.TextBox textBox1; - } -} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.cs b/Tools/ArdupilotMegaPlanner/SerialInput.cs deleted file mode 100644 index 948257f7c1..0000000000 --- a/Tools/ArdupilotMegaPlanner/SerialInput.cs +++ /dev/null @@ -1,225 +0,0 @@ -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.Ports; - -namespace ArdupilotMega -{ - public partial class SerialInput : Form - { - System.Threading.Thread t12; - static bool threadrun = false; - static internal SerialPort comPort = new SerialPort(); - static internal PointLatLngAlt lastgotolocation = new PointLatLngAlt(0, 0, 0, "Goto last"); - static internal PointLatLngAlt gotolocation = new PointLatLngAlt(0, 0, 0, "Goto"); - static internal int intalt = 100; - - public SerialInput() - { - InitializeComponent(); - - CMB_serialport.DataSource = SerialPort.GetPortNames(); - - if (threadrun) - { - BUT_connect.Text = "Stop"; - } - } - - private void BUT_connect_Click(object sender, EventArgs e) - { - if (comPort.IsOpen) - { - threadrun = false; - comPort.Close(); - BUT_connect.Text = "Connect"; - } - else - { - try - { - comPort.PortName = CMB_serialport.Text; - } - catch { CustomMessageBox.Show("Invalid PortName"); return; } - try { - comPort.BaudRate = int.Parse(CMB_baudrate.Text); - } catch {CustomMessageBox.Show("Invalid BaudRate"); return;} - try { - comPort.Open(); - } catch {CustomMessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;} - - - string alt = "100"; - - if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) - { - alt = (10 * MainV2.cs.multiplierdist).ToString("0"); - } - else - { - alt = (100 * MainV2.cs.multiplierdist).ToString("0"); - } - if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Alt (relative to home alt)", ref alt)) - return; - - intalt = (int)(100 * MainV2.cs.multiplierdist); - if (!int.TryParse(alt, out intalt)) - { - CustomMessageBox.Show("Bad Alt"); - return; - } - - t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) - { - IsBackground = true, - Name = "Nmea Input" - }; - t12.Start(); - - BUT_connect.Text = "Stop"; - } - } - - void mainloop() - { - DateTime nextsend = DateTime.Now; - - threadrun = true; - while (threadrun) - { - try - { - string line = comPort.ReadLine(); - - //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.cs.lng < 0 ? "W" : "E", MainV2.cs.gpsstatus, MainV2.cs.satcount, MainV2.cs.gpshdop, MainV2.cs.alt, "M", 0, "M", ""); - if (line.StartsWith("$GPGGA")) // - { - string[] items = line.Trim().Split(',','*'); - - if (items[15] != GetChecksum(line.Trim())) - { - Console.WriteLine("Bad Nmea line " + items[15] + " vs " + GetChecksum(line.Trim())); - continue; - } - - if (items[6] == "0") - { - Console.WriteLine("No Fix"); - continue; - } - - gotolocation.Lat = double.Parse(items[2]) / 100.0; - - gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60); - - if (items[3] == "S") - gotolocation.Lat *= -1; - - gotolocation.Lng = double.Parse(items[4]) / 100.0; - - gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60); - - if (items[5] == "W") - gotolocation.Lng *= -1; - - gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) + - - gotolocation.Tag = "Sats "+ items[7] + " hdop " + items[8] ; - - } - - - if (DateTime.Now > nextsend && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && - { - nextsend = DateTime.Now.AddSeconds(2); - Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt); - lastgotolocation = new PointLatLngAlt(gotolocation); - - Locationwp gotohere = new Locationwp(); - - gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; - gotohere.alt = (float)(gotolocation.Alt); - gotohere.lat = (float)(gotolocation.Lat); - gotohere.lng = (float)(gotolocation.Lng); - - try - { - updateLocationLabel(gotohere); - } - catch { } - - if (MainV2.comPort.BaseStream.IsOpen && MainV2.giveComport == false) - { - try - { - MainV2.giveComport = true; - - MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT, (byte)2); - - GCSViews.FlightData.GuidedModeWP = new PointLatLngAlt(gotohere); - - MainV2.giveComport = false; - } - catch { MainV2.giveComport = false; } - } - } - } - catch { System.Threading.Thread.Sleep(2000); } - } - } - - private void updateLocationLabel(Locationwp plla) - { - this.BeginInvoke((MethodInvoker)delegate - { - LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt +" "+ gotolocation.Tag; - } - ); - - } - - private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e) - { - } - - // Calculates the checksum for a sentence - string GetChecksum(string sentence) - { - // Loop through all chars to get a checksum - int Checksum = 0; - foreach (char Character in sentence.ToCharArray()) - { - switch (Character) - { - case '$': - // Ignore the dollar sign - break; - case '*': - // Stop processing before the asterisk - return Checksum.ToString("X2"); - default: - // Is this the first value for the checksum? - if (Checksum == 0) - { - // Yes. Set the checksum to the value - Checksum = Convert.ToByte(Character); - } - else - { - // No. XOR the checksum with this character's value - Checksum = Checksum ^ Convert.ToByte(Character); - } - break; - } - } - // Return the checksum formatted as a two-character hexadecimal - return Checksum.ToString("X2"); - } - - } -} diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.es-ES.resx b/Tools/ArdupilotMegaPlanner/SerialInput.es-ES.resx deleted file mode 100644 index 4d14dfe4ec..0000000000 --- a/Tools/ArdupilotMegaPlanner/SerialInput.es-ES.resx +++ /dev/null @@ -1,123 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - text/microsoft-resx - - - 2.0 - - - System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - Lo que esto hace. 1. se las actuales coordenadas GPS de un GPS NMEA. 2. envía un WP modo guiado a la AP cada 2 segundos. Como Usar 1. conectar con la AP. 2. el despegue, el modo de prueba guiada está trabajando. 3. abrir este y escoja su puerto com, y la velocidad de su NMEA GPS. 4. ahora debe estar siguiéndote. - - \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.pl.resx b/Tools/ArdupilotMegaPlanner/SerialInput.pl.resx deleted file mode 100644 index bb1f84d234..0000000000 --- a/Tools/ArdupilotMegaPlanner/SerialInput.pl.resx +++ /dev/null @@ -1,131 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - text/microsoft-resx - - - 2.0 - - - System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - What this does. -1. gets the current gps coords from a nmea gps. -2. sends a guided mode WP to the AP every 2 seconds. - -How to use it -1. connect to ap. -2. take off, test guided mode is working. -3. open this and pick your comport, and baud rate for your nmea gps. -4. it should now be following you. - - \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.resx b/Tools/ArdupilotMegaPlanner/SerialInput.resx deleted file mode 100644 index 62276c20c7..0000000000 --- a/Tools/ArdupilotMegaPlanner/SerialInput.resx +++ /dev/null @@ -1,208 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - text/microsoft-resx - - - 2.0 - - - System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - What this does. -1. gets the current gps coords from a nmea gps. -2. sends a guided mode WP to the AP every 2 seconds. - -How to use it -1. connect to ap. -2. take off, test guided mode is working. -3. open this and pick your comport, and baud rate for your nmea gps. -4. it should now be following you. - - - - - AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAABILAAASCwAAAAAAAAAA - AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADOxkjAtnoOAKpJ4vyiK - c+8nh3D/J4Zv/yeHcP8oi3PvKpJ4vy6fg4AzsZIwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA - AAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADjGo2AyspPfLZ+D/yiQ - d/8hlXj/G6F9/xeqg/8XqYL/GKqD/xuhfv8ilnn/KZB3/y2fhP8yspPfN8ajYAAAAAAAAAAAAAAAAAAA - AAAAAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADvRrDA1vpzfL6uN/yel - hP8XvJD/DMyY/wfQl/8FzJP/A8qS/wPJkf8EypL/BsyU/wnRmP8PzZn/Gb2R/yemhP8tqoz/Mb2a3zbQ - qkAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAA4y6ZgMbWV/yin - iP8WwZP/Btqf/wDPlf8AyI7/A8aP/yfNnv9T2LP/UNax/03XsP8506b/G8ya/wHKkf8F0Zf/CNuf/xLB - kv8fpYT/J7KQ/y7IomAAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAANcajny+w - kf8hqoj/CNSd/wDRlf8Axor/Hcyd/3Lhwf+p7Nj/o+vV/57m0/+X5dD/k+TN/4/jzf+K5Mz/fuHH/0PW - rf8HzJT/ANCT/wDRlv8OpX//HayI/yrFn58AAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAADDC - nmAtro7/H62J/wPWmv8Ay47/AMaO/3XhxP+e6tT/mObP/5Pjy/+Q4sr/jODJ/4ffx/+C3MT/f9vC/3nb - wf9y2r7/adq7/2DauP8ZzZv/Fdae/8T/9/9WxKj/HKuI/y7IomAAAAAAAAAAAAAAAAD///8AAAAAAAAA - AAAiuZMwKKyM/x6ohf8C1Zr/AMmL/wHGjv+49OL///////////9+3ML/f9zD/4Dcwv9+28L/e9rA/3bZ - vv9w1rr/Z9S4/17Rs/9Qz63/Qcyn/3LewP////////////n///8MpH7/JbKP/zXQqUAAAAAAAAAAAP// - /wAAAAAAAAAAABymhN8dnn//BNGa/wDKjP8AxY3/sfHf/////////////////2nXt/9w1rv/c9e8/3TX - vP9x17z/a9W5/2TTtf9Y0K//SMyp/zXFoP9i07X/////////////////f/LR/wDQlf8epYT/Mb2a3wAA - AAAAAAAA////AAAAAAADlnJgFZR1/wq4iv8AzpH/AMCD/4rmzf//////////////////////WdGv/2PU - tf9p1rf/atS4/2nUtv9i0rT/Vc+u/0fKpv8zxZz/Ws+w//////////////////////8GyJL/ANCS/xLB - kv8tq4z/OMajYAAAAAD///8AAAAAAACHZt8NkW//ANKV/wDChP9i27r//////////////////////9Dx - 6P9MzKn/Vc+v/17Rsv9g0rP/XNCx/1XNrv9Fyaf/McSd/1fPr///////////////////////QM2m/ynK - oP8JzJX/C9yh/ymmhf80spPfAAAAAP///wAAcUwwAHtc/wCrfP8AyIv/AMKK//////////////////// - /////////////5Dgyv9Gyqb/TMyq/07Nq/9MzKn/Qcmj/y/Fnf9Wzq3//////////////////////57k - 0v8av5T/Lceg/yzOo/8M05v/Hr6T/zCghf80spIw////AABoRYAAclT/AL2H/wDBhf9R1rL///////// - ////////4vfw//////////////////H8+P9KzKn/Ocah/zTFnv8qwpj/Us2t//////////////////// - ////////DLqM/yDBlv8wxp//OM6m/xPPm/8Xz53/LZF5/y+fg4////8AAGNAvwB7Wf8Aw4j/ALyC/4bj - yP+g5tL/g93E/2HSsv9Pzqz/Us6s//////////////////////9Yzq//Gr2S/0jLp/////////////// - /////////////yrDm/8SvI//JMGY/zDHn/81zKT/Is2e/xTUnf8nl3v/LJJ5v////wAAXz3vAIlg/wDA - hf8AuoD/quzZ/5Hjyv9628D/ada2/1jRsP9Jy6f/a9a4//////////////////////+Y4s7///////// - //////////////////+c4tD/AbaH/xW8kf8jwZj/LcWd/y/Jn/8kzJ3/E9Ca/yGjgf8ri3Tv////AABd - PP8Ak2b/AL6D/w/Ekv+m6tf/j+HJ/3vawP9p1rf/W9Gx/0rNqf85yJ//Nsaf//////////////////// - /////////////////////////////wCwe/8AtoT/ELqP/xu+k/8jwZj/KMeb/yHKm/8QzZf/HqyG/ymI - cf////8AAF07/wCSZP8AvYL/GMWU/6Dn1P+K38f/ddi+/27Wuf+E3MX/leHN/6fm1f+l5tX/neLQ//// - ////////////////////////////////////////j9/J/27Vuv9Tzq7/JsKY/xa/kv8aw5T/FcaW/wvL - lf8aqoT/J4dw/////wAAXTv/AJFk/wC9gP8GwY3/mObQ/5rkz/+26dv/y/Hl/8Dt3/+06tz/pebV/5bg - zP+g5NL//////////////v///f7+//7+/v//////7fn2////////////tOnb/6Ll0v+v6Nj/jeDI/zXK - o/8IxJD/BMqS/xaqgv8lh2//////AABeO+8AgVf/AL1//wDBif/R9uv/1PPq/8Tv5P+36t3/rujY/6Lk - 0v+U4cv/jt7J//j8+///////+/38//f8+//2+/r/+Pz7//3+/v/m9/P/9Pv6//D6+P9/28L/jd7J/5jj - z/+h5dL/qOvX/4Hmyf8f1J//E596/yOJcO////8AAGA8vwB3U/8p06P/hufM/8Ty5f/D7+T/s+vb/6bm - 1P+c4c//j9/K/4vcyP/t+fb///7///j8/P/0+/r/8vr5//P7+f/1+/r/+/39///////i9fL/ZNO1/3HW - vP992sH/htzG/4vhyv+S5dD/mO7W/6X74v80noT/Io90v////wAAZkCAAHla/33ny/945cb/nunV/7Xr - 3v+l5tT/luDN/4ndxv992cL/1vLq//v9/P/1+/n/8vv4//L69//z+/j/9Pv5/7Xo2//x+vn///////// - //+y59n/aNS3/3LWvP932r//fNzD/4Ljyf+J7ND/l/bd/yORdf8knH6A////AABuRzAAdlT/Xc6x/23o - xv9s4MH/qurZ/5jiz/+I3cb/edjA/8ju5f/3/Pv/8vv4//H6+P/y+/j/6/f0/7np3v/7/fz//v7+/6fk - 1f+56tz///////////9h0bT/aNW4/23Wu/9v3L//dOLG/37w0f9m1rn/Hpt8/ymujTD///8AAAAAAACD - X98po4X/Z+7K/1vgvP+A4sf/jOHK/3rZwv+r59f/9Pv6/+/69//v+vf/8vr4/9fy6/9n0rf/VM6t/6Di - 0v/N7+f/adO4/1PMrf9t1Lr/i9zI/1/Rs/9h0rX/ZNe4/2bbvf9s5sb/ePfV/z2ylf8lrozfAAAAAP// - /wAAAAAAAJNsYAWQbf9U1rP/Vee//0rYsf993sb/pebV//P7+v/s+Pb/6/f1/+749v+s5tj/Vc2u/1jP - r/9ZzrD/btW5/1bOr/9Wza//Vs6v/1fOr/9Z0LD/WdCy/1vTtP9d1rX/Xt+8/2btyP9k4L//IaaF/y7D - nmAAAAAA////AAAAAAAAAAAAD6J9zyCjgv9S68L/P9+0/2Pevv/5////7/v6/+v59//j9/L/gtvF/1PN - r/9Wz7D/Wc+x/1nQsf9Zz7H/WM6w/1fPsP9UzrD/VM+w/1TPrv9U0a//U9Oy/1Tatv9Z5sD/Y/LL/zSx - lP8qupbPAAAAAAAAAAD///8AAAAAAAAAAAAYto4wGaeE/y23lP8+5rn/6/////j////w//3/ve/i/2bV - uP9Tzq7/Vc+v/1jPsP9Z0LL/WM+w/1fOsf9Wz7D/Us2w/1HOrf9Qzq3/T9Cu/0zSr/9M2LP/TeC5/1bt - xP9HxaX/KLKQ/zTPqDAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAkvpdgG6iF/y++m//e/////P///3rl - yf9G0K3/VdKy/1bPsf9Wz7H/Vs6w/1bPsP9Sza//Ucyu/0/Nrf9NzKz/S82s/0fOrP9G0a7/QdWv/0Le - tP9I6L7/Q8Ok/yitjP8yyKJgAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAmwJlgG6iF/yK3 - kP8k3q7/H9el/x7Pn/8tzKT/Q9Cs/1HQsP9Q0K7/TM6u/0nMrf9Hzaz/RMyp/0LNqf8+zqn/ONGo/zTV - qf833rD/O+S4/zvCof8orIv/MMSfYAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAA - AAAkvpdgG6iE/xukgv8gy53/HNql/xzRn/8czJz/HcmZ/yXJnP8qyp7/Lcqg/yzLn/8nypz/JMqc/yTO - n/8l1KT/KN2r/y3Tpv8nq4n/JaqJ/yzAm2AAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAA - AAAAAAAAAAAAAAAAAAAato8wFKN/zxCScv8RnHn/DbqM/wjIlP8GyZT/BsaS/wbFkf8GxZH/B8WR/wfH - k/8IypX/DMmV/xG3jP8WoX3/Fph2/xqkgs8ft5EwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA////AAAA - AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAJVvYACGZM8Aelr/AHlZ/wCFX/8AiWL/AJlr/wCb - bP8AlGf/AI5k/wB/W/8AeFj/AHtb/wCHZd8ClXBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA - AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAABwSzAAaESAAGI/vwBf - Pd8AXTz/AF08/wBdPP8AXz3fAGJAvwBoRIAAcUswAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA - AAAAAAAAAAAAAP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// - /wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// - /wD///8A////AP///wD///8A/+AD//+AAP/+AAA//AAAH/gAAA/wAAAH4AAAA+AAAAPAAAABwAAAAYAA - AACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAwAAAAcAAAAHgAAAD4AAAA/AA - AAf4AAAP/AAAH/4AAD//gAD//+AD//////8= - - - \ No newline at end of file