diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index ccd82ba138..7658c48942 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -245,7 +245,8 @@ namespace ArdupilotMega GUIDED = 4, // AUTO control LOITER = 5, // Hold a single location RTL = 6, // AUTO control - CIRCLE = 7 + CIRCLE = 7, + POSITION = 8 } public static bool translateMode(string modein, ref MAVLink.__mavlink_set_nav_mode_t navmode, ref MAVLink.__mavlink_set_mode_t mode) diff --git a/Tools/ArdupilotMegaPlanner/CurrentState.cs b/Tools/ArdupilotMegaPlanner/CurrentState.cs index 4bba5e8269..8febc24fd3 100644 --- a/Tools/ArdupilotMegaPlanner/CurrentState.cs +++ b/Tools/ArdupilotMegaPlanner/CurrentState.cs @@ -26,7 +26,7 @@ namespace ArdupilotMega float _airspeed; float _groundspeed; float _verticalspeed; - public float verticalspeed { get { return _verticalspeed; } set { _verticalspeed = _verticalspeed * 0.8f + value * 0.2f; } } + public float verticalspeed { get { if (float.IsNaN(_verticalspeed)) _verticalspeed = 0; return _verticalspeed; } set { _verticalspeed = _verticalspeed * 0.8f + value * 0.2f; } } public float wind_dir { get; set; } public float wind_vel { get; set; } /// diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index d24f7c0f12..e7d970530f 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -388,7 +388,7 @@ namespace ArdupilotMega.GCSViews { lbl_ACY6.Text = temp.name; } - else if (temp.url.ToLower().Contains("firmware/ac2-heli".ToLower())) + else if (temp.url.ToLower().Contains("firmware/ac2-heli-1".ToLower())) { lbl_Heli.Text = temp.name; } @@ -396,6 +396,10 @@ namespace ArdupilotMega.GCSViews { lbl_ACHil.Text = temp.name; } + else + { + Console.WriteLine("No Home "+ temp.name + " " + temp.url); + } } void findfirmware(string findwhat) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs index e5577aa145..39e165613e 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs @@ -12,6 +12,7 @@ System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); this.contextMenuStrip1 = new System.Windows.Forms.ContextMenuStrip(this.components); this.goHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); + this.pointCameraHereToolStripMenuItem = new System.Windows.Forms.ToolStripMenuItem(); this.MainH = new System.Windows.Forms.SplitContainer(); this.SubMainLeft = new System.Windows.Forms.SplitContainer(); this.hud1 = new hud.HUD(); @@ -91,7 +92,8 @@ // contextMenuStrip1 // this.contextMenuStrip1.Items.AddRange(new System.Windows.Forms.ToolStripItem[] { - this.goHereToolStripMenuItem}); + this.goHereToolStripMenuItem, + this.pointCameraHereToolStripMenuItem}); this.contextMenuStrip1.Name = "contextMenuStrip1"; resources.ApplyResources(this.contextMenuStrip1, "contextMenuStrip1"); // @@ -101,6 +103,12 @@ resources.ApplyResources(this.goHereToolStripMenuItem, "goHereToolStripMenuItem"); this.goHereToolStripMenuItem.Click += new System.EventHandler(this.goHereToolStripMenuItem_Click); // + // pointCameraHereToolStripMenuItem + // + this.pointCameraHereToolStripMenuItem.Name = "pointCameraHereToolStripMenuItem"; + resources.ApplyResources(this.pointCameraHereToolStripMenuItem, "pointCameraHereToolStripMenuItem"); + this.pointCameraHereToolStripMenuItem.Click += new System.EventHandler(this.pointCameraHereToolStripMenuItem_Click); + // // MainH // this.MainH.BorderStyle = System.Windows.Forms.BorderStyle.FixedSingle; @@ -1311,5 +1319,6 @@ private System.Windows.Forms.ToolStripMenuItem recordHudToAVIToolStripMenuItem; private System.Windows.Forms.ToolStripMenuItem stopRecordToolStripMenuItem; private MyLabel lbl_logpercent; + private System.Windows.Forms.ToolStripMenuItem pointCameraHereToolStripMenuItem; } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index bc7a00d89a..705de52256 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -1339,5 +1339,33 @@ namespace ArdupilotMega.GCSViews } } } + + private void pointCameraHereToolStripMenuItem_Click(object sender, EventArgs e) + { + if (!MainV2.comPort.BaseStream.IsOpen) + { + MessageBox.Show("Please Connect First"); + return; + } + + string alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + Common.InputBox("Enter Alt", "Enter Target Alt (absolute)", ref alt); + + int intalt = (int)(100 * MainV2.cs.multiplierdist); + if (!int.TryParse(alt, out intalt)) + { + MessageBox.Show("Bad Alt"); + return; + } + + if (gotolocation.Lat == 0 || gotolocation.Lng == 0) + { + MessageBox.Show("Bad Lat/Long"); + return; + } + + MainV2.comPort.setMountConfigure(MAVLink.MAV_MOUNT_MODE.MAV_MOUNT_MODE_GPS_POINT, true, true, true); + MainV2.comPort.setMountControl(gotolocation.Lat, gotolocation.Lng, (int)(intalt / MainV2.cs.multiplierdist), true); + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index a8ce828065..d44ff71539 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -122,13 +122,19 @@ - 134, 22 + 174, 22 Fly To Here + + 174, 22 + + + Point Camera Here + - 135, 26 + 175, 70 contextMenuStrip1 @@ -1735,6 +1741,12 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + pointCameraHereToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + recordHudToAVIToolStripMenuItem diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index b45c37da3a..8a84ccf784 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -682,7 +682,7 @@ namespace ArdupilotMega.GCSViews return Math.Cos(rad); } - //float oldax =0, olday =0, oldaz = 0; + float oldax =0, olday =0, oldaz = 0; DateTime oldtime = DateTime.Now; ArdupilotMega.MAVLink.__mavlink_attitude_t oldatt = new ArdupilotMega.MAVLink.__mavlink_attitude_t(); @@ -742,7 +742,7 @@ namespace ArdupilotMega.GCSViews float rdiff = (float)((att.roll - oldatt.roll) / timediff.TotalSeconds); float ydiff = (float)((att.yaw - oldatt.yaw) / timediff.TotalSeconds); - // Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); + //Console.WriteLine("{0:0.00000} {1:0.00000} {2:0.00000} \t {3:0.00000} {4:0.00000} {5:0.00000}", pdiff, rdiff, ydiff, DATA[17][0], DATA[17][1], DATA[17][2]); oldatt = att; @@ -758,7 +758,21 @@ namespace ArdupilotMega.GCSViews YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1], DATA[18][0], 0, -9.8); //DATA[18][2] - //accel3D += new YLScsDrawing.Drawing3d.Vector3d(0, 0, -9.8); + float turnrad = (float)(((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704) * 1.943844) / (float)(11.26 * Math.Tan(att.roll))) * ft2m; + + float centripaccel = (float)((DATA[3][7] * 0.44704) * (DATA[3][7] * 0.44704)) / turnrad; + + //Console.WriteLine("old {0} {1} {2}",accel3D.X,accel3D.Y,accel3D.Z); + + YLScsDrawing.Drawing3d.Vector3d cent3D = HIL.QuadCopter.RPY_to_XYZ(DATA[18][1] - 90, 0, 0, centripaccel); + + accel3D -= cent3D; + + //Console.WriteLine("new {0} {1} {2}", accel3D.X, accel3D.Y, accel3D.Z); + + oldax = DATA[4][5]; + olday = DATA[4][6]; + oldaz = DATA[4][4]; double head = DATA[18][2] - 90; @@ -861,9 +875,13 @@ namespace ArdupilotMega.GCSViews imu.zgyro = (short)(aeroin.Model_fAngVelZ * 1000); //imu.zmag = 0; - imu.xacc = (Int16)(aeroin.Model_fAccelX * 1000); // pitch - imu.yacc = (Int16)(aeroin.Model_fAccelY * 1000); // roll - imu.zacc = (Int16)(aeroin.Model_fAccelZ * 1000); + YLScsDrawing.Drawing3d.Vector3d accel3D = HIL.QuadCopter.RPY_to_XYZ(att.roll, att.pitch, 0, -9.8); //DATA[18][2] + + imu.xacc = (Int16)((accel3D.X + aeroin.Model_fAccelX) * 1000); // pitch + imu.yacc = (Int16)((accel3D.Y + aeroin.Model_fAccelY) * 1000); // roll + imu.zacc = (Int16)((accel3D.Z + aeroin.Model_fAccelZ) * 1000); + + Console.WriteLine("x {0} y {1} z {2}",imu.xacc,imu.yacc,imu.zacc); gps.alt = ((float)(aeroin.Model_fPosZ)); diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 480a4376ff..01b198c91c 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -1436,6 +1436,68 @@ namespace ArdupilotMega } } + public void setMountConfigure(MAV_MOUNT_MODE mountmode,bool stabroll,bool stabpitch,bool stabyaw) + { + __mavlink_mount_configure_t req = new __mavlink_mount_configure_t(); + + req.target_system = sysid; + req.target_component = compid; + req.mount_mode = (byte)mountmode; + req.stab_pitch = (stabpitch == true) ? (byte)1 : (byte)0; + req.stab_roll = (stabroll == true) ? (byte)1 : (byte)0; + req.stab_yaw = (stabyaw == true) ? (byte)1 : (byte)0; + + generatePacket(MAVLINK_MSG_ID_MOUNT_CONFIGURE, req); + System.Threading.Thread.Sleep(20); + generatePacket(MAVLINK_MSG_ID_MOUNT_CONFIGURE, req); + } + + public void setMountControl(double pa,double pb,double pc,bool islatlng) + { + __mavlink_mount_control_t req = new __mavlink_mount_control_t(); + + req.target_system = sysid; + req.target_component = compid; + if (!islatlng) + { + req.input_a = (int)pa; + req.input_b = (int)pb; + req.input_c = (int)pc; + } + else + { + req.input_a = (int)(pa * 10000000.0); + req.input_b = (int)(pb * 10000000.0); + req.input_c = (int)(pc * 100.0); + } + + generatePacket(MAVLINK_MSG_ID_MOUNT_CONTROL, req); + System.Threading.Thread.Sleep(20); + generatePacket(MAVLINK_MSG_ID_MOUNT_CONTROL, req); + } + + public void setMode(string modein) + { + try + { + MAVLink.__mavlink_set_nav_mode_t navmode = new MAVLink.__mavlink_set_nav_mode_t(); + + MAVLink.__mavlink_set_mode_t mode = new MAVLink.__mavlink_set_mode_t(); + + if (Common.translateMode(modein, ref navmode, ref mode)) + { + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_NAV_MODE, navmode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + System.Threading.Thread.Sleep(10); + MainV2.comPort.generatePacket((byte)MAVLink.MAVLINK_MSG_ID_SET_MODE, mode); + } + } + catch { System.Windows.Forms.MessageBox.Show("Failed to change Modes"); } + } + /// /// used for last bad serial characters /// diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs index d5bae2043a..9d16f0c04d 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypes.cs @@ -7,8 +7,69 @@ namespace ArdupilotMega { partial class MAVLink { - public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}; - public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}; + public byte[] MAVLINK_MESSAGE_LENGTHS = new byte[] {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5}; + public byte[] MAVLINK_MESSAGE_CRCS = new byte[] {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7}; + public enum MAV_MOUNT_MODE + { + MAV_MOUNT_MODE_RETRACT=0, /* Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization | */ + MAV_MOUNT_MODE_NEUTRAL=1, /* Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM. | */ + MAV_MOUNT_MODE_MAVLINK_TARGETING=2, /* Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ + MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ + MAV_MOUNT_MODE_ENUM_END=5, /* | */ + }; + + public const byte MAVLINK_MSG_ID_AP_ADC = 153; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_ap_adc_t + { + public ushort adc1; /// ADC output 1 + public ushort adc2; /// ADC output 2 + public ushort adc3; /// ADC output 3 + public ushort adc4; /// ADC output 4 + public ushort adc5; /// ADC output 5 + public ushort adc6; /// ADC output 6 + }; + + public const byte MAVLINK_MSG_ID_AP_ADC_LEN = 12; + public const byte MAVLINK_MSG_ID_153_LEN = 12; + public const byte MAVLINK_MSG_ID_DIGICAM_CONFIGURE = 154; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_digicam_configure_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte mode; /// Mode enumeration from 1 to N //P, TV, AV, M, Etc (0 means ignore) + public ushort shutter_speed; /// Divisor number //e.g. 1000 means 1/1000 (0 means ignore) + public byte aperture; /// F stop number x 10 //e.g. 28 means 2.8 (0 means ignore) + public byte iso; /// ISO enumeration from 1 to N //e.g. 80, 100, 200, Etc (0 means ignore) + public byte exposure_type; /// Exposure type enumeration from 1 to N (0 means ignore) + public byte command_id; /// Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + public byte engine_cut_off; /// Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off) + public byte extra_param; /// Extra parameters enumeration (0 means ignore) + public float extra_value; /// Correspondent value to given extra_param + }; + + public const byte MAVLINK_MSG_ID_DIGICAM_CONFIGURE_LEN = 15; + public const byte MAVLINK_MSG_ID_154_LEN = 15; + public const byte MAVLINK_MSG_ID_DIGICAM_CONTROL = 155; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_digicam_control_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte session; /// 0: stop, 1: start or keep it up //Session control e.g. show/hide lens + public byte zoom_pos; /// 1 to N //Zoom's absolute position (0 means ignore) + public byte zoom_step; /// -100 to 100 //Zooming step value to offset zoom from the current position + public byte focus_lock; /// 0: unlock focus or keep unlocked, 1: lock focus or keep locked, 3: re-lock focus + public byte shot; /// 0: ignore, 1: shot or start filming + public byte command_id; /// Command Identity (incremental loop: 0 to 255)//A command sent multiple times will be executed or pooled just once + public byte extra_param; /// Extra parameters enumeration (0 means ignore) + public float extra_value; /// Correspondent value to given extra_param + }; + + public const byte MAVLINK_MSG_ID_DIGICAM_CONTROL_LEN = 13; + public const byte MAVLINK_MSG_ID_155_LEN = 13; public const byte MAVLINK_MSG_ID_MEMINFO = 152; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_meminfo_t @@ -19,6 +80,47 @@ namespace ArdupilotMega public const byte MAVLINK_MSG_ID_MEMINFO_LEN = 4; public const byte MAVLINK_MSG_ID_152_LEN = 4; + public const byte MAVLINK_MSG_ID_MOUNT_CONFIGURE = 156; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_configure_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public byte mount_mode; /// mount operating mode (see MAV_MOUNT_MODE enum) + public byte stab_roll; /// (1 = yes, 0 = no) + public byte stab_pitch; /// (1 = yes, 0 = no) + public byte stab_yaw; /// (1 = yes, 0 = no) + }; + + public const byte MAVLINK_MSG_ID_MOUNT_CONFIGURE_LEN = 6; + public const byte MAVLINK_MSG_ID_156_LEN = 6; + public const byte MAVLINK_MSG_ID_MOUNT_CONTROL = 157; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_control_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public int input_a; /// pitch(deg*100) or lat, depending on mount mode + public int input_b; /// roll(deg*100) or lon depending on mount mode + public int input_c; /// yaw(deg*100) or alt (in cm) depending on mount mode + public byte save_position; /// if "1" it will save current trimmed position on EEPROM (just valid for NEUTRAL and LANDING) + }; + + public const byte MAVLINK_MSG_ID_MOUNT_CONTROL_LEN = 15; + public const byte MAVLINK_MSG_ID_157_LEN = 15; + public const byte MAVLINK_MSG_ID_MOUNT_STATUS = 158; + [StructLayout(LayoutKind.Sequential,Pack=1)] + public struct __mavlink_mount_status_t + { + public byte target_system; /// System ID + public byte target_component; /// Component ID + public int pointing_a; /// pitch(deg*100) or lat, depending on mount mode + public int pointing_b; /// roll(deg*100) or lon depending on mount mode + public int pointing_c; /// yaw(deg*100) or alt (in cm) depending on mount mode + }; + + public const byte MAVLINK_MSG_ID_MOUNT_STATUS_LEN = 14; + public const byte MAVLINK_MSG_ID_158_LEN = 14; public const byte MAVLINK_MSG_ID_SENSOR_OFFSETS = 150; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_sensor_offsets_t @@ -87,6 +189,10 @@ namespace ArdupilotMega system to control the vehicle attitude and the attitude of various devices such as cameras. |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ + DO_DIGICAM_CONFIGURE=202, /* Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)| */ + DO_DIGICAM_CONTROL=203, /* Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty| */ + DO_MOUNT_CONFIGURE=204, /* Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| */ + DO_MOUNT_CONTROL=205, /* Mission command to control a camera or antenna mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty| */ DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Empty| Empty| Empty| */ PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| */ @@ -1346,7 +1452,7 @@ namespace ArdupilotMega MAV_FRAME_LOCAL_ENU = 4 }; -Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; +Type[] mavstructs = new Type[] {typeof( __mavlink_heartbeat_t) ,typeof( __mavlink_boot_t) ,null ,typeof( __mavlink_ping_t) ,null ,typeof( __mavlink_change_operator_control_t) ,typeof( __mavlink_change_operator_control_ack_t) ,typeof( __mavlink_auth_key_t) ,null ,typeof( __mavlink_action_ack_t) ,typeof( __mavlink_action_t) ,typeof( __mavlink_set_mode_t) ,typeof( __mavlink_set_nav_mode_t) ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_param_request_read_t) ,typeof( __mavlink_param_request_list_t) ,typeof( __mavlink_param_value_t) ,typeof( __mavlink_param_set_t) ,null ,typeof( __mavlink_gps_raw_int_t) ,typeof( __mavlink_scaled_imu_t) ,typeof( __mavlink_gps_status_t) ,typeof( __mavlink_raw_imu_t) ,typeof( __mavlink_raw_pressure_t) ,typeof( __mavlink_attitude_new_t ) ,typeof( __mavlink_local_position_t) ,typeof( __mavlink_gps_raw_t) ,typeof( __mavlink_global_position_t) ,typeof( __mavlink_sys_status_t) ,typeof( __mavlink_rc_channels_raw_t) ,typeof( __mavlink_rc_channels_scaled_t) ,typeof( __mavlink_servo_output_raw_t) ,typeof( __mavlink_scaled_pressure_t) ,typeof( __mavlink_waypoint_t) ,typeof( __mavlink_waypoint_request_t) ,typeof( __mavlink_waypoint_set_current_t) ,typeof( __mavlink_waypoint_current_t) ,typeof( __mavlink_waypoint_request_list_t) ,typeof( __mavlink_waypoint_count_t) ,typeof( __mavlink_waypoint_clear_all_t) ,typeof( __mavlink_waypoint_reached_t) ,typeof( __mavlink_waypoint_ack_t) ,typeof( __mavlink_waypoint_set_global_reference_t ) ,typeof( __mavlink_gps_local_origin_set_t) ,typeof( __mavlink_local_position_setpoint_set_t) ,typeof( __mavlink_local_position_setpoint_t) ,typeof( __mavlink_control_status_t) ,typeof( __mavlink_safety_set_allowed_area_t) ,typeof( __mavlink_safety_allowed_area_t) ,typeof( __mavlink_set_roll_pitch_yaw_thrust_t) ,typeof( __mavlink_set_roll_pitch_yaw_speed_thrust_t) ,typeof( __mavlink_roll_pitch_yaw_thrust_setpoint_t) ,typeof( __mavlink_roll_pitch_yaw_speed_thrust_setpoint_t) ,null ,typeof( __mavlink_attitude_controller_output_t ) ,typeof( __mavlink_position_controller_output_t ) ,typeof( __mavlink_nav_controller_output_t) ,typeof( __mavlink_position_target_t) ,typeof( __mavlink_state_correction_t) ,typeof( __mavlink_set_altitude_t) ,typeof( __mavlink_request_data_stream_t) ,typeof( __mavlink_request_dynamic_gyro_calibration_t ) ,typeof( __mavlink_request_static_calibration_t ) ,typeof( __mavlink_manual_control_t) ,typeof( __mavlink_rc_channels_override_t) ,null ,null ,typeof( __mavlink_global_position_int_t) ,typeof( __mavlink_vfr_hud_t) ,typeof( __mavlink_command_t) ,typeof( __mavlink_command_ack_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_optical_flow_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_object_detection_event_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_sensor_offsets_t) ,typeof( __mavlink_set_mag_offsets_t) ,typeof( __mavlink_meminfo_t) ,typeof( __mavlink_ap_adc_t) ,typeof( __mavlink_digicam_configure_t) ,typeof( __mavlink_digicam_control_t) ,typeof( __mavlink_mount_configure_t) ,typeof( __mavlink_mount_control_t) ,typeof( __mavlink_mount_status_t) ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,null ,typeof( __mavlink_debug_vect_t) ,typeof( __mavlink_named_value_float_t) ,typeof( __mavlink_named_value_int_t) ,typeof( __mavlink_statustext_t) ,typeof( __mavlink_debug_t) ,null ,}; } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs index 864b322f5d..6f146c83c8 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypesenum.cs @@ -6,8 +6,8 @@ using System.Runtime.InteropServices; namespace ArdupilotMega { partial class MAVLink - { - + { + } } diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index aa806d298a..04800efbd9 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.0.82")] +[assembly: AssemblyFileVersion("1.0.83")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index bd9f307fa9..408b405505 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - DxfINYOSh55dWio5h4coAIWRotU= + xG04b9X6TXBWirLuQOgI+3TR8H0= diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx index a8ce828065..d44ff71539 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/GCSViews/FlightData.resx @@ -122,13 +122,19 @@ - 134, 22 + 174, 22 Fly To Here + + 174, 22 + + + Point Camera Here + - 135, 26 + 175, 70 contextMenuStrip1 @@ -1735,6 +1741,12 @@ System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + pointCameraHereToolStripMenuItem + + + System.Windows.Forms.ToolStripMenuItem, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + recordHudToAVIToolStripMenuItem diff --git a/Tools/ArdupilotMegaPlanner/mavlinklist.pl b/Tools/ArdupilotMegaPlanner/mavlinklist.pl index 43b0b6219e..ba4f5a59dd 100644 --- a/Tools/ArdupilotMegaPlanner/mavlinklist.pl +++ b/Tools/ArdupilotMegaPlanner/mavlinklist.pl @@ -2,8 +2,8 @@ $dir = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/common/"; $dir2 = "C:/Users/hog/Documents/Arduino/libraries/GCS_MAVLink/include/ardupilotmega/"; # mavlink 1.0 with old structs -#$dir = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/common/"; -#$dir2 = "C:/Users/hog/Desktop/DIYDrones&avr/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/"; +$dir = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/common/"; +$dir2 = "C:/Users/hog/Desktop/DIYDrones/ardupilot-mega/libraries/GCS_MAVLink/include/ardupilotmega/"; opendir(DIR,$dir) || die print $!; @files2 = readdir(DIR);