From 657dc8c12a49fb548ade9c41794613b8d608c027 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Nov 2011 08:03:27 +1100 Subject: [PATCH 1/7] autotest: added --viewerip option this allows you to specify an IP that will receive all MAVLink logs and fg data for realtime viewing --- Tools/autotest/arducopter.py | 21 ++++++++++++++------- Tools/autotest/autotest.py | 4 +++- 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index c13d29e7ce..b017f68656 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -275,8 +275,12 @@ def setup_rc(mavproxy): mavproxy.send('rc 3 1000\n') -def fly_ArduCopter(): - '''fly ArduCopter in SIL''' +def fly_ArduCopter(viewerip=None): + '''fly ArduCopter in SIL + + you can pass viewerip as an IP address to optionally send fg and + mavproxy packets too for local viewing of the flight in real time + ''' global expect_list, homeloc sil = util.start_SIL('ArduCopter', wipe=True) @@ -295,12 +299,13 @@ def fly_ArduCopter(): mavproxy.expect('Loaded [0-9]+ parameters') # reboot with new parameters - print("CLOSING") util.pexpect_close(mavproxy) util.pexpect_close(sil) - print("CLOSED THEM") sil = util.start_SIL('ArduCopter') - mavproxy = util.start_MAVProxy_SIL('ArduCopter', options='--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --out=192.168.2.15:14550 --quadcopter --streamrate=1') + options = '--fgout=127.0.0.1:5502 --fgin=127.0.0.1:5501 --out=127.0.0.1:19550 --quadcopter --streamrate=1' + if viewerip: + options += ' --out=%s:14550' % viewerip + mavproxy = util.start_MAVProxy_SIL('ArduCopter', options=options) mavproxy.expect('Logging to (\S+)') logfile = mavproxy.match.group(1) print("LOGFILE %s" % logfile) @@ -317,8 +322,10 @@ def fly_ArduCopter(): util.expect_setup_callback(mavproxy, expect_callback) # start hil_quad.py - hquad = pexpect.spawn(util.reltopdir('../HILTest/hil_quad.py') + ' --fgout=192.168.2.15:9123 --fgrate=200 --home=%s' % HOME_LOCATION, - logfile=sys.stdout, timeout=10) + cmd = util.reltopdir('../HILTest/hil_quad.py') + ' --fgrate=200 --home=%s' % HOME_LOCATION + if viewerip: + cmd += ' --fgout=192.168.2.15:9123' + hquad = pexpect.spawn(cmd, logfile=sys.stdout, timeout=10) util.pexpect_autoclose(hquad) hquad.expect('Starting at') diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 4353aff36a..3a374843f7 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -81,6 +81,7 @@ You can get it from git://git.samba.org/tridge/UAV/HILTest.git parser = optparse.OptionParser("autotest") parser.add_option("--skip", type='string', default='', help='list of steps to skip (comma separated)') parser.add_option("--list", action='store_true', default=False, help='list the available steps') +parser.add_option("--viewerip", default=None, help='IP address to send MAVLink and fg packets to') opts, args = parser.parse_args() @@ -145,7 +146,7 @@ def run_step(step): return dump_logs('ArduCopter') if step == 'fly.ArduCopter': - return arducopter.fly_ArduCopter() + return arducopter.fly_ArduCopter(viewerip=opts.viewerip) if step == 'convertgpx': return convert_gpx() @@ -264,6 +265,7 @@ try: if not run_tests(steps): sys.exit(1) except KeyboardInterrupt: + print("INTERRUPT: Stopping child processes ....") util.pexpect_close_all() sys.exit(1) except Exception: From 8a3ea5bfff5a3b46b8fd6aa2153c924c5336fc3d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Nov 2011 09:53:31 +1100 Subject: [PATCH 2/7] desktop: when in the CLI, use blocking writes this prevents us missing information from the logs --- libraries/Desktop/support/FastSerial.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/Desktop/support/FastSerial.cpp b/libraries/Desktop/support/FastSerial.cpp index 60542d51b3..fbb12ff6c7 100644 --- a/libraries/Desktop/support/FastSerial.cpp +++ b/libraries/Desktop/support/FastSerial.cpp @@ -295,11 +295,15 @@ void FastSerial::flush(void) void FastSerial::write(uint8_t c) { struct tcp_state *s = &tcp_state[_u2x]; + int flags = MSG_NOSIGNAL; check_connection(s); if (!s->connected) { return; } - send(s->fd, &c, 1, MSG_DONTWAIT | MSG_NOSIGNAL); + if (!desktop_state.slider) { + flags |= MSG_DONTWAIT; + } + send(s->fd, &c, 1, flags); } // Buffer management /////////////////////////////////////////////////////////// From 4b89bc174fcee58344087962bdb661dadb7c154a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 11 Nov 2011 20:05:39 +1100 Subject: [PATCH 3/7] create a kmz file thanks to Michael for the tip --- Tools/autotest/autotest.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 3a374843f7..d76b078932 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -59,6 +59,7 @@ def convert_gpx(): gpx = m + '.gpx' kml = m + '.kml' util.run_cmd('gpsbabel -i gpx -f %s -o kml,units=m,floating=1 -F %s' % (gpx, kml), checkfail=False) + util.run_cmd('zip %s.kmz %s.kml' % (m, m), checkfail=False) return True @@ -241,7 +242,7 @@ def run_tests(steps): results.addglob('DataFlash Log', '*.flashlog') results.addglob("MAVLink log", '*.mavlog') results.addglob("GPX track", '*.gpx') - results.addglob("KML track", '*.kml') + results.addglob("KMZ track", '*.kmz') results.addfile('ArduPlane build log', 'ArduPlane.txt') results.addfile('ArduPlane code size', 'ArduPlane.sizes.txt') results.addfile('ArduPlane stack sizes', 'ArduPlane.framesizes.txt') From b2494e498e91045fe4a14933cdd4f1d3a6d12aa1 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 12 Nov 2011 21:17:26 +0800 Subject: [PATCH 4/7] APM Planner 1.0.91 Heli swash override fix HIL --- Tools/ArdupilotMegaPlanner/ArduinoDetect.cs | 23 ++++++++++++++- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 1 + .../GCSViews/Configuration.cs | 6 ++-- .../ArdupilotMegaPlanner/GCSViews/Firmware.cs | 7 ++++- Tools/ArdupilotMegaPlanner/HIL/QuadCopter.cs | 2 +- Tools/ArdupilotMegaPlanner/MAVLink.cs | 6 ++++ Tools/ArdupilotMegaPlanner/MAVLinkTypes0.9.cs | 16 +---------- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Setup/Setup.cs | 28 ++++++++++++++++--- Tools/ArdupilotMegaPlanner/Setup/Setup.resx | 4 +-- .../Release/ArdupilotMegaPlanner.application | 2 +- .../bin/Release/Setup/Setup.resx | 4 +-- 12 files changed, 70 insertions(+), 31 deletions(-) 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 From 4fcf11cf6ed37ea19b0cde5e0c51d872cabe790f Mon Sep 17 00:00:00 2001 From: unknown Date: Sat, 12 Nov 2011 23:23:07 +0900 Subject: [PATCH 5/7] TradHeli - new parameter HSV_MAN to allow better set-up from APMissionPlanner. Also changed swash movement to use radio_out which fixes some setup bugs --- ArduCopter/Parameters.h | 5 +++- ArduCopter/heli.pde | 54 ++++++++++++++++++----------------------- ArduCopter/radio.pde | 4 +-- ArduCopter/setup.pde | 31 ++++++++++++----------- ArduCopter/system.pde | 1 + 5 files changed, 46 insertions(+), 49 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b1fee3418e..eab4008393 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -75,7 +75,8 @@ public: k_param_heli_collective_mid, k_param_heli_ext_gyro_enabled, k_param_heli_ext_gyro_gain, - k_param_heli_servo_averaging, // 94 + k_param_heli_servo_averaging, + k_param_heli_servo_manual, // 95 #endif // 110: Telemetry control @@ -253,6 +254,7 @@ public: AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7) AP_Int8 heli_servo_averaging; // 0 or 1 = no averaging (250hz) for **digital servos**, 2=average of two samples (125hz), 3=three samples (83.3hz) **analog servos**, 4=four samples (62.5hz), 5=5 samples(50hz) + AP_Int8 heli_servo_manual; // 0 = normal mode, 1 = radio inputs directly control swash. required for swash set-up #endif // RC channels @@ -361,6 +363,7 @@ public: heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")), heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")), heli_servo_averaging (0, k_param_heli_servo_averaging, PSTR("SV_AVG")), + heli_servo_manual (0, k_param_heli_servo_manual, PSTR("HSV_MAN")), #endif // RC channel group key name diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 0d0cad434c..b445762ccd 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -5,7 +5,6 @@ #define HELI_SERVO_AVERAGING_DIGITAL 0 // 250Hz #define HELI_SERVO_AVERAGING_ANALOG 2 // 125Hz -static int heli_manual_override = false; static float heli_throttle_scaler = 0; // heli_servo_averaging: @@ -81,25 +80,18 @@ static void heli_move_servos_to_mid() // yaw: -4500 ~ 4500 // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) -{ +{ // ensure values are acceptable: - roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); - pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); - coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); + if( g.heli_servo_manual != 1) { + roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); + pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); + coll_out = constrain(coll_out, (int)g.heli_coll_min, (int)g.heli_coll_max); + } // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); - if( g.heli_servo_1.get_reverse() ) - g.heli_servo_1.servo_out = 3000 - g.heli_servo_1.servo_out; - - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); - if( g.heli_servo_2.get_reverse() ) - g.heli_servo_2.servo_out = 3000 - g.heli_servo_2.servo_out; - - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); - if( g.heli_servo_3.get_reverse() ) - g.heli_servo_3.servo_out = 3000 - g.heli_servo_3.servo_out; - + g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out - 1000 + (g.heli_servo_3.radio_trim-1500); g.heli_servo_4.servo_out = yaw_out; // use servo_out to calculate pwm_out and radio_out @@ -109,9 +101,9 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o g.heli_servo_4.calc_pwm(); // add the servo values to the averaging - heli_servo_out[0] += g.heli_servo_1.servo_out; - heli_servo_out[1] += g.heli_servo_2.servo_out; - heli_servo_out[2] += g.heli_servo_3.servo_out; + heli_servo_out[0] += g.heli_servo_1.radio_out; + heli_servo_out[1] += g.heli_servo_2.radio_out; + heli_servo_out[2] += g.heli_servo_3.radio_out; heli_servo_out[3] += g.heli_servo_4.radio_out; heli_servo_out_count++; @@ -125,13 +117,13 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o heli_servo_out[2] /= g.heli_servo_averaging; heli_servo_out[3] /= g.heli_servo_averaging; } - + // actually move the servos APM_RC.OutputCh(CH_1, heli_servo_out[0]); APM_RC.OutputCh(CH_2, heli_servo_out[1]); APM_RC.OutputCh(CH_3, heli_servo_out[2]); APM_RC.OutputCh(CH_4, heli_servo_out[3]); - + // output gyro value if( g.heli_ext_gyro_enabled ) { APM_RC.OutputCh(CH_7, g.heli_ext_gyro_gain); @@ -164,19 +156,21 @@ static void init_motors_out() // these are not really motors, they're servos but we don't rename the function because it fits with the rest of the code better static void output_motors_armed() { + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if( g.heli_servo_manual == 1 ) { + g.rc_1.servo_out = g.rc_1.control_in; + g.rc_2.servo_out = g.rc_2.control_in; + g.rc_3.servo_out = g.rc_3.control_in; + g.rc_4.servo_out = g.rc_4.control_in; + } + //static int counter = 0; g.rc_1.calc_pwm(); g.rc_2.calc_pwm(); g.rc_3.calc_pwm(); g.rc_4.calc_pwm(); - if( heli_manual_override ) { - // straight pass through from radio inputs to swash plate - heli_move_swash( g.rc_1.control_in, g.rc_2.control_in, g.rc_3.radio_in, g.rc_4.control_in ); - }else{ - // source inputs from attitude controller - heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); - } + heli_move_swash( g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.radio_out, g.rc_4.servo_out ); } // for helis - armed or disarmed we allow servos to move @@ -200,7 +194,7 @@ static void output_motor_test() static int heli_get_scaled_throttle(int throttle) { float scaled_throttle = (g.heli_coll_min - 1000) + throttle * heli_throttle_scaler; - return g.heli_coll_min - 1000 + (throttle * heli_throttle_scaler); + return scaled_throttle; } // heli_angle_boost - takes servo_out position diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index f0b3d11d7a..a5d3e1af28 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -9,8 +9,8 @@ static void default_dead_zones() g.rc_1.set_dead_zone(60); g.rc_2.set_dead_zone(60); #if FRAME_CONFIG == HELI_FRAME - g.rc_3.set_dead_zone(0); - g.rc_4.set_dead_zone(60); + g.rc_3.set_dead_zone(20); + g.rc_4.set_dead_zone(30); #else g.rc_3.set_dead_zone(60); g.rc_4.set_dead_zone(200); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 937ea16daa..4ee4179ac5 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -469,8 +469,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv) // initialise swash plate heli_init_swash(); - // source swash plate movements directly from - heli_manual_override = true; + // source swash plate movements directly from radio + g.heli_servo_manual = true; // display initial settings report_heli(); @@ -494,6 +494,9 @@ setup_heli(uint8_t argc, const Menu::arg *argv) // read radio although we don't use it yet read_radio(); + + // allow swash plate to move + output_motors_armed(); // record min/max if( state == 1 ) { @@ -501,13 +504,12 @@ setup_heli(uint8_t argc, const Menu::arg *argv) max_roll = abs(g.rc_1.control_in); if( abs(g.rc_2.control_in) > max_pitch ) max_pitch = abs(g.rc_2.control_in); - if( g.rc_3.radio_in < min_coll ) - min_coll = g.rc_3.radio_in; - if( g.rc_3.radio_in > max_coll ) - max_coll = g.rc_3.radio_in; - min_tail = min(g.rc_4.radio_in, min_tail); - max_tail = max(g.rc_4.radio_in, max_tail); - //Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out); + if( g.rc_3.radio_out < min_coll ) + min_coll = g.rc_3.radio_out; + if( g.rc_3.radio_out > max_coll ) + max_coll = g.rc_3.radio_out; + min_tail = min(g.rc_4.radio_out, min_tail); + max_tail = max(g.rc_4.radio_out, max_tail); } if( Serial.available() ) { @@ -533,8 +535,8 @@ setup_heli(uint8_t argc, const Menu::arg *argv) break; case 'c': case 'C': - if( g.rc_3.radio_in >= 900 && g.rc_3.radio_in <= 2100 ) { - g.heli_coll_mid = g.rc_3.radio_in; + if( g.rc_3.radio_out >= 900 && g.rc_3.radio_out <= 2100 ) { + g.heli_coll_mid = g.rc_3.radio_out; Serial.printf_P(PSTR("Collective when blade pitch at zero: %d\n"),(int)g.heli_coll_mid); } break; @@ -561,7 +563,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) max_pitch = abs(g.rc_2.control_in); min_coll = 2000; max_coll = 1000; - min_tail = max_tail = abs(g.rc_4.radio_in); + min_tail = max_tail = abs(g.rc_4.radio_out); }else{ state = 0; // switch back to normal mode // double check values aren't totally terrible @@ -639,9 +641,6 @@ setup_heli(uint8_t argc, const Menu::arg *argv) } } - // allow swash plate to move - output_motors_armed(); - delay(20); } @@ -664,7 +663,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv) g.heli_servo_averaging.save(); // return swash plate movements to attitude controller - heli_manual_override = false; + g.heli_servo_manual = false; return(0); } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 3edcd92a55..64746e0b27 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -178,6 +178,7 @@ static void init_ardupilot() #endif #if FRAME_CONFIG == HELI_FRAME + g.heli_servo_manual = false; heli_init_swash(); // heli initialisation #endif From 9aeda9f702126a9d85a48ff8b62e6a2905675086 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 13 Nov 2011 08:24:56 +0800 Subject: [PATCH 6/7] fix loiter radius --- ArduCopter/Parameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index eab4008393..098a3685bf 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -322,7 +322,7 @@ public: command_index (0, k_param_command_index, PSTR("WP_INDEX")), command_must_index (0, k_param_command_must_index, PSTR("WP_MUST_INDEX")), waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), - loiter_radius (LOITER_RADIUS * 100, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), + loiter_radius (LOITER_RADIUS, k_param_loiter_radius, PSTR("WP_LOITER_RAD")), waypoint_speed_max (WAYPOINT_SPEED_MAX, k_param_waypoint_speed_max, PSTR("WP_SPEED_MAX")), throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), From 0a2f67a235b7062aa9f79e7858164fd20b3c91e0 Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 13 Nov 2011 08:25:24 +0800 Subject: [PATCH 7/7] modify crosstrack calc --- ArduPlane/navigation.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 2f1e803c7a..2b6620009b 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -183,7 +183,7 @@ static void update_crosstrack(void) static void reset_crosstrack() { - crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following + crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following } static long get_distance(struct Location *loc1, struct Location *loc2)