diff --git a/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs b/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs index 033a117421..7b70d4036d 100644 --- a/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs +++ b/Tools/ArdupilotMegaPlanner/ArduinoDetect.cs @@ -2,6 +2,7 @@ using System.Collections.Generic; using System.Linq; using System.Text; +using System.Management; namespace ArdupilotMega { @@ -54,6 +55,26 @@ namespace ArdupilotMega System.Threading.Thread.Sleep(500); + //HKEY_LOCAL_MACHINE\SYSTEM\CurrentControlSet\Enum\USB\VID_2341&PID_0010\640333439373519060F0\Device Parameters + if (!MainV2.MAC) + { + ObjectQuery query = new ObjectQuery("SELECT * FROM Win32_USBControllerDevice"); + ManagementObjectSearcher searcher = new ManagementObjectSearcher(query); + foreach (ManagementObject obj2 in searcher.Get()) + { + Console.WriteLine("Dependant : " + obj2["Dependent"]); + + if (obj2["Dependent"].ToString().Contains(@"USB\\VID_2341&PID_0010")) + { + return "2560-2"; + } + } + } + else + { + int fixme; + } + serialPort.DtrEnable = true; serialPort.BaudRate = 115200; serialPort.Open(); @@ -92,7 +113,7 @@ namespace ArdupilotMega port = new ArduinoSTK(); port.BaudRate = 57600; } - else if (version == "2560") + else if (version == "2560" || version == "2560-2") { port = new ArduinoSTKv2(); port.BaudRate = 115200; diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index 61a1b27bfa..5e535fbb28 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -166,6 +166,7 @@ ..\..\..\..\..\Desktop\DIYDrones\myquad\greatmaps_e1bb830a18a3\Demo.WindowsForms\bin\Debug\x86\System.Data.SQLite.DLL + True diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 42d454a35a..21c0014764 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -508,7 +508,7 @@ namespace ArdupilotMega.GCSViews ofd.AddExtension = true; ofd.DefaultExt = ".param"; ofd.RestoreDirectory = true; - ofd.Filter = "Param List|*.param"; + ofd.Filter = "Param List|*.param;*.parm"; DialogResult dr = ofd.ShowDialog(); if (dr == DialogResult.OK) { @@ -562,7 +562,7 @@ namespace ArdupilotMega.GCSViews sfd.AddExtension = true; sfd.DefaultExt = ".param"; sfd.RestoreDirectory = true; - sfd.Filter = "Param List|*.param"; + sfd.Filter = "Param List|*.param;*.parm"; DialogResult dr = sfd.ShowDialog(); if (dr == DialogResult.OK) { @@ -1005,7 +1005,7 @@ namespace ArdupilotMega.GCSViews ofd.AddExtension = true; ofd.DefaultExt = ".param"; ofd.RestoreDirectory = true; - ofd.Filter = "Param List|*.param"; + ofd.Filter = "Param List|*.param;*.parm"; DialogResult dr = ofd.ShowDialog(); if (dr == DialogResult.OK) { diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index a5a8129599..43ac300243 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -514,10 +514,15 @@ namespace ArdupilotMega.GCSViews { baseurl = temp.url2560.ToString(); } - else + else if (board == "1280") { baseurl = temp.url.ToString(); } + else + { + MessageBox.Show("Invalid Board Type"); + return; + } // Create a request using a URL that can receive a post. WebRequest request = WebRequest.Create(baseurl); diff --git a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs index 31e2cc662d..2b9854d3ad 100644 --- a/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs +++ b/Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs @@ -112,7 +112,7 @@ namespace ArdupilotMega.HIL double pitch_accel = (m[2] - m[3]) * 5000.0; double yaw_accel = -((m[2] + m[3]) - (m[0] + m[1])) * 400.0; - // Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); + //Console.WriteLine("roll {0} {1} {2}", roll_accel, roll_rate, roll); //# update rotational rates roll_rate += roll_accel * delta_time.TotalSeconds; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index fdfeeae1f7..a74ee6fc1d 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -348,16 +348,22 @@ namespace ArdupilotMega public void sendPacket(object indata) { + bool run = false; byte a = 0; foreach (Type ty in mavstructs) { if (ty == indata.GetType()) { + run = true; generatePacket(a, indata); return; } a++; } + if (!run) + { + Console.WriteLine("Mavlink : NOT VALID PACKET sendPacket() "+indata.GetType().ToString()); + } } /// diff --git a/Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs b/Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs index 9736a4dd6c..8a0d729592 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs @@ -305,20 +305,6 @@ namespace ArdupilotMega }; - public const byte MAVLINK_MSG_ID_ATTITUDE_NEW = 30; - [StructLayout(LayoutKind.Sequential,Pack=1)] - public struct __mavlink_attitude_new_t - { - public ulong usec; /// Timestamp (microseconds since UNIX epoch or microseconds since system boot) - public float roll; /// Roll angle (rad) - public float pitch; /// Pitch angle (rad) - public float yaw; /// Yaw angle (rad) - public float rollspeed; /// Roll angular speed (rad/s) - public float pitchspeed; /// Pitch angular speed (rad/s) - public float yawspeed; /// Yaw angular speed (rad/s) - - }; - public const byte MAVLINK_MSG_ID_AUTH_KEY = 7; [StructLayout(LayoutKind.Sequential,Pack=1)] public struct __mavlink_auth_key_t @@ -1486,7 +1472,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) ,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 ,}; +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_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 ,}; } #endif diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 604c2a7441..a04e8a5a7d 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.90")] +[assembly: AssemblyFileVersion("1.0.91")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 5d028719d6..addc743c4c 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -442,6 +442,12 @@ namespace ArdupilotMega.Setup } } } + + HS1_REV.Checked = MainV2.comPort.param["HS1_REV"].ToString() == "-1"; + HS2_REV.Checked = MainV2.comPort.param["HS2_REV"].ToString() == "-1"; + HS3_REV.Checked = MainV2.comPort.param["HS3_REV"].ToString() == "-1"; + HS4_REV.Checked = MainV2.comPort.param["HS4_REV"].ToString() == "-1"; + } catch { } startup = false; @@ -910,28 +916,28 @@ namespace ArdupilotMega.Setup { if (startup) return; - MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == true ? 1.0f : -1.0f); + MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == false ? 1.0f : -1.0f); } private void HS2_REV_CheckedChanged(object sender, EventArgs e) { if (startup) return; - MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == true ? 1.0f : -1.0f); + MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == false ? 1.0f : -1.0f); } private void HS3_REV_CheckedChanged(object sender, EventArgs e) { if (startup) return; - MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == true ? 1.0f : -1.0f); + MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == false ? 1.0f : -1.0f); } private void HS4_REV_CheckedChanged(object sender, EventArgs e) { if (startup) return; - MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == true ? 1.0f : -1.0f); + MainV2.comPort.setParam(((CheckBox)sender).Name, ((CheckBox)sender).Checked == false ? 1.0f : -1.0f); } private void HS1_TRIM_Scroll(object sender, EventArgs e) @@ -1016,6 +1022,20 @@ namespace ArdupilotMega.Setup { try { + try + { + if (MainV2.comPort.param["HSV_MAN"].ToString() == "1") + { + MainV2.comPort.setParam("HSV_MAN", 0); // randy request + } + else + { + MainV2.comPort.setParam("HSV_MAN", 1); // randy request + return; + } + } + catch { MessageBox.Show("Failed to set HSV_MAN"); } + MainV2.comPort.setParam("COL_MIN_", HS3.minline); MainV2.comPort.setParam("COL_MAX_", HS3.maxline); diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx index d62fdbd971..f118878f21 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.resx +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.resx @@ -3008,13 +3008,13 @@ will work with hexa's etc 430, 307 - 90, 23 + 90, 41 111 - Save Travel + Manual Travel / Save Travel BUT_saveheliconfig diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application index 83a884928b..b9ed0e421e 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application +++ b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.application @@ -11,7 +11,7 @@ - asA4p3xM8idcyyuyecIXR3bVsug= + dszV+o57dCwksPcpzEBhuhMS3T8= diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/Setup/Setup.resx b/Tools/ArdupilotMegaPlanner/bin/Release/Setup/Setup.resx index 6deb0ac7f6..37fe264dce 100644 --- a/Tools/ArdupilotMegaPlanner/bin/Release/Setup/Setup.resx +++ b/Tools/ArdupilotMegaPlanner/bin/Release/Setup/Setup.resx @@ -3008,13 +3008,13 @@ will work with hexa's etc 430, 307 - 90, 23 + 90, 41 111 - Save Travel + Manual Travel / Save Travel BUT_saveheliconfig