diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index ee4f024ed2..a92afc0963 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -89,6 +89,3 @@ // #define MOT_6 CH_4 // #define MOT_7 CH_7 // #define MOT_8 CH_8 - -// Enabling this will use the GPS lat/long coordinate to get the compass declination -//#define AUTOMATIC_DECLINATION ENABLED \ No newline at end of file diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d7e67518ce..78f8ce07c4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,11 +1,11 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.4.2" +#define THISFIRMWARE "ArduCopter V2.5" /* -ArduCopter Version 2.4 -Authors: Jason Short -Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen -Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier +ArduCopter Version 2.5 +Lead author: Jason Short +Based on code and ideas from the Arducopter team: Randy Mackay, Pat Hickey, Jose Julio, Jani Hirvinen, Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto Navoni +Thanks to: Chris Anderson, Mike Smith, Jordi Munoz, Doug Weibel, James Goppert, Benjamin Pelletier, Robert Lefebvre, Marco Robustini This firmware is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public @@ -526,8 +526,8 @@ int32_t pitch_axis; AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration -AverageFilterInt16_Size3 lat_rate_d_filter; // for filtering D term -AverageFilterInt16_Size3 lon_rate_d_filter; // for filtering D term +AverageFilterInt16_Size2 lat_rate_d_filter; // for filtering D term +AverageFilterInt16_Size2 lon_rate_d_filter; // for filtering D term // Barometer filter AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration @@ -1161,10 +1161,10 @@ static void fifty_hz_loop() camera_stabilization(); # if HIL_MODE == HIL_MODE_DISABLED - if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) + if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST && motor_armed) Log_Write_Attitude(); - if (g.log_bitmask & MASK_LOG_RAW) + if (g.log_bitmask & MASK_LOG_RAW && motor_armed) Log_Write_Raw(); #endif @@ -1699,8 +1699,8 @@ void update_throttle_mode(void) } // hack to remove the influence of the ground effect - if(current_loc.alt < 200 && landing_boost != 0) { - nav_throttle = min(nav_throttle, 0); + if(g.sonar_enabled && current_loc.alt < 100 && landing_boost != 0) { + nav_throttle = min(nav_throttle, 0); } #if FRAME_CONFIG == HELI_FRAME @@ -1781,12 +1781,10 @@ static void update_navigation() // go of the sticks if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ - // flag will reset when speed drops below .5 m/s loiter_override = true; } // Allow the user to take control temporarily, - // regain hold when the speed goes down to .5m/s if(loiter_override){ // this sets the copter to not try and nav while we control it wp_control = NO_NAV_MODE; @@ -1795,7 +1793,7 @@ static void update_navigation() next_WP.lat = current_loc.lat; next_WP.lng = current_loc.lng; - if((g.rc_2.control_in + g.rc_1.control_in) == 0){ + if( g.rc_2.control_in == 0 && g.rc_1.control_in == 0 ){ loiter_override = false; wp_control = LOITER_MODE; } @@ -1924,6 +1922,7 @@ static void update_altitude() // calc the vertical accel rate int temp = (baro_alt - old_baro_alt) * 10; baro_rate = (temp + baro_rate) >> 1; + baro_rate = constrain(baro_rate, -300, 300); old_baro_alt = baro_alt; // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter @@ -1942,6 +1941,7 @@ static void update_altitude() // calc the vertical accel rate // positive = going up. sonar_rate = (sonar_alt - old_sonar_alt) * 10; + sonar_rate = constrain(sonar_rate, -150, 150); old_sonar_alt = sonar_alt; #endif @@ -1953,9 +1953,8 @@ static void update_altitude() sonar_alt = (float)sonar_alt * temp; #endif - scale = (sonar_alt - 400) / 200; - scale = constrain(scale, 0, 1); - + scale = (float)(sonar_alt - 400) / 200.0; + scale = constrain(scale, 0.0, 1.0); // solve for a blended altitude current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt; diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index cdfbbe8126..64b43bd7b4 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -250,8 +250,8 @@ static void Log_Read_GPS() int8_t temp2 = DataFlash.ReadByte(); // 2 sats int32_t temp3 = DataFlash.ReadLong(); // 3 lat int32_t temp4 = DataFlash.ReadLong(); // 4 lon - float temp5 = DataFlash.ReadLong() / 100.0; // 5 gps alt - float temp6 = DataFlash.ReadLong() / 100.0; // 6 sensor alt + float temp5 = DataFlash.ReadLong() / 100.0; // 5 sensor alt + float temp6 = DataFlash.ReadLong() / 100.0; // 6 gps alt int16_t temp7 = DataFlash.ReadInt(); // 7 ground speed int32_t temp8 = DataFlash.ReadLong();// 8 ground course diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 45ef25399c..3e9fa8d90a 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -664,7 +664,7 @@ // Loiter control gains // #ifndef LOITER_P -# define LOITER_P .4 +# define LOITER_P .35 #endif #ifndef LOITER_I # define LOITER_I 0.0 @@ -680,10 +680,10 @@ # define LOITER_RATE_P 2.0 // #endif #ifndef LOITER_RATE_I -# define LOITER_RATE_I 0.10 // Wind control +# define LOITER_RATE_I 0.2 // Wind control #endif #ifndef LOITER_RATE_D -# define LOITER_RATE_D 0.0 // try 2 or 3 for LOITER_RATE 1 +# define LOITER_RATE_D 0 // try 2 or 3 for LOITER_RATE 1 #endif #ifndef LOITER_RATE_IMAX # define LOITER_RATE_IMAX 30 // degrees @@ -899,10 +899,6 @@ # define USE_CURRENT_ALT FALSE #endif - -#ifndef AUTO_RESET_LOITER -# define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input. -#endif #ifndef CUT_MOTORS # define CUT_MOTORS 1 // do we cut the motors with no throttle? #endif diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 4ce6a81d7a..7940adee98 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -204,7 +204,7 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o // rudder feed forward based on collective #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator if( !g.heli_ext_gyro_enabled ) { - yaw_offset = g.heli_collective_yaw_effect * abs(coll_out_scaled - g.heli_collective_mid); + yaw_offset = g.heli_collective_yaw_effect * abs(coll_out_scaled - heli_throttle_mid); } #endif } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index f5f5616275..221f665151 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -50,16 +50,26 @@ static void calc_XY_velocity(){ // y_GPS_speed positve = Up // x_GPS_speed positve = Right + // initialise last_longitude and last_latitude + if( last_longitude == 0 && last_latitude == 0 ) { + last_longitude = g_gps->longitude; + last_latitude = g_gps->latitude; + } + // this speed is ~ in cm because we are using 10^7 numbers from GPS float tmp = 1.0/dTnav; // straightforward approach: ///* - x_actual_speed = x_speed_old + (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp; - y_actual_speed = y_speed_old + (float)(g_gps->latitude - last_latitude) * tmp; - x_actual_speed = x_actual_speed >> 1; - y_actual_speed = y_actual_speed >> 1; + x_actual_speed = (float)(g_gps->longitude - last_longitude) * scaleLongDown * tmp; + y_actual_speed = (float)(g_gps->latitude - last_latitude) * tmp; + + x_actual_speed = (x_actual_speed + x_speed_old * 3) / 4; + y_actual_speed = (y_actual_speed + y_speed_old * 3) / 4; + + //x_actual_speed = x_actual_speed >> 1; + //y_actual_speed = y_actual_speed >> 1; x_speed_old = x_actual_speed; y_speed_old = y_actual_speed; @@ -105,21 +115,37 @@ static void calc_location_error(struct Location *next_loc) int16_t tmp; - tmp = (long_error - last_lon_error); - if(abs(abs(tmp) -last_lon_d) > 15) tmp = x_rate_d; + // ------------------------------------- + tmp = (long_error - last_lon_error); + if(abs(abs(tmp) -last_lon_d) > 20){ + tmp = x_rate_d; + }/* + if(long_error > 0){ + if(tmp < 0) tmp = 0; + }else{ + if(tmp > 0) tmp = 0; + }*/ x_rate_d = lon_rate_d_filter.apply(tmp); + x_rate_d = constrain(x_rate_d, -800, 800); last_lon_d = abs(tmp); - tmp = (lat_error - last_lat_error); - if(abs(abs(tmp) -last_lat_d) > 15) tmp = y_rate_d; - //if(abs(tmp) > 80) tmp = y_rate_d; + // ------------------------------------- + tmp = (lat_error - last_lat_error); + if(abs(abs(tmp) -last_lat_d) > 20) + tmp = y_rate_d; + /*if(lat_error > 0){ + if(tmp < 0) tmp = 0; + }else{ + if(tmp > 0) tmp = 0; + }*/ y_rate_d = lat_rate_d_filter.apply(tmp); + y_rate_d = constrain(y_rate_d, -800, 800); last_lat_d = abs(tmp); - int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); - int16_t raww = (long_error - last_lon_error); - - //Serial.printf("XX, %d, %d, %d\n", raww, x_rate_d, t22); + // debug + //int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); + //if(control_mode == LOITER) + // Serial.printf("XX, %d, %d, %d \n", long_error, t22, (int16_t)g.pid_loiter_rate_lon.get_integrator()); last_lon_error = long_error; last_lat_error = lat_error; @@ -133,24 +159,23 @@ static void calc_loiter(int x_error, int y_error) // East / West x_target_speed = g.pi_loiter_lon.get_p(x_error); // not contstrained yet - //x_target_speed = constrain(x_target_speed, -250, 250); // limit to 2.5m/s travel speed x_rate_error = x_target_speed - x_actual_speed; // calc the speed error - nav_lon = g.pid_loiter_rate_lon.get_pi(x_rate_error, dTnav); - nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); + nav_lon = g.pid_loiter_rate_lon.get_pid(x_rate_error, dTnav); + //nav_lon += x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); nav_lon = constrain(nav_lon, -3000, 3000); // 30° // North / South y_target_speed = g.pi_loiter_lat.get_p(y_error); - //y_target_speed = constrain(y_target_speed, -250, 250); y_rate_error = y_target_speed - y_actual_speed; - nav_lat = g.pid_loiter_rate_lat.get_pi(y_rate_error, dTnav); - nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav); + nav_lat = g.pid_loiter_rate_lat.get_pid(y_rate_error, dTnav); + //nav_lat += y_rate_d * (g.pid_loiter_rate_lat.kD() / dTnav); nav_lat = constrain(nav_lat, -3000, 3000); // 30° // copy over I term to Nav_Rate g.pid_nav_lon.set_integrator(g.pid_loiter_rate_lon.get_integrator()); g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator()); + //Serial.printf("XX, %d, %d, %d\n", long_error, x_actual_speed, (int16_t)g.pid_loiter_rate_lon.get_integrator()); // Wind I term based on location error, // limit windup diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 146dead877..571cfe4e92 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -1,21 +1,20 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from -// their default values, place the appropriate #define statements here. +// This file is just a placeholder for your configuration file. If +// you wish to change any of the setup parameters from their default +// values, place the appropriate #define statements here. -// For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented) -//#define SERIAL3_BAUD 38400 + +// the following 2 defines control which APM board you have. The +// 'BETA' board is only if you are developer who received a +// pre-release APM2 board with the older barometer on it. // # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 // # define APM2_BETA_HARDWARE -// You may also put an include statement here to point at another configuration file. This is convenient if you maintain -// different configuration files for different aircraft or HIL simulation. See the examples below -//#include "APM_Config_mavlink_hil.h" -//#include "Skywalker.h" - -// The following are the recommended settings for Xplane simulation. Remove the leading "/* and trailing "*/" to enable: +// The following are the recommended settings for Xplane +// simulation. Remove the leading "/* and trailing "*/" to enable: /* #define HIL_MODE HIL_MODE_ATTITUDE @@ -27,18 +26,7 @@ // Mavlink supports // 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude // 2. HIL_MODE_SENSORS: full sensor simulation -#define HIL_MODE HIL_MODE_ATTITUDE +//#define HIL_MODE HIL_MODE_ATTITUDE -// Sensors -// All sensors are supported in all modes. -// The magnetometer is not used in -// HIL_MODE_ATTITUDE but you may leave it -// enabled if you wish. -#define AIRSPEED_SENSOR ENABLED -#define MAGNETOMETER ENABLED -#define AIRSPEED_CRUISE 25 -#define THROTTLE_FAILSAFE ENABLED */ -// Enabling this will use the GPS lat/long coordinate to get the compass declination -//#define AUTOMATIC_DECLINATION ENABLED diff --git a/ArduPlane/APM_Config_mavlink_hil.h b/ArduPlane/APM_Config_mavlink_hil.h deleted file mode 100644 index 38cff467b5..0000000000 --- a/ArduPlane/APM_Config_mavlink_hil.h +++ /dev/null @@ -1,31 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -// THIS IS A SAMPLE CONFIGURATION FILE FOR DOING HARDWARE IN THE LOOP TESTING USING THE MAVLINK HIL INTERFACE -// IF YOU WANTED TO USE THIS YOU WOULD COPY THE CONTENTS INTO YOUR APM_Config.h FILE! - - -// Enable Autopilot Flight Mode -#define FLIGHT_MODE_CHANNEL 8 -#define FLIGHT_MODE_1 AUTO -#define FLIGHT_MODE_2 RTL -#define FLIGHT_MODE_3 FLY_BY_WIRE_A -#define FLIGHT_MODE_4 FLY_BY_WIRE_B -#define FLIGHT_MODE_5 STABILIZE -#define FLIGHT_MODE_6 MANUAL - -// HIL_MODE SELECTION -// -// Mavlink supports -// 1. HIL_MODE_ATTITUDE : simulated position, airspeed, and attitude -// 2. HIL_MODE_SENSORS: full sensor simulation -#define HIL_MODE HIL_MODE_ATTITUDE - -// Sensors -// All sensors are supported in all modes. -// The magnetometer is not used in -// HIL_MODE_ATTITUDE but you may leave it -// enabled if you wish. -#define AIRSPEED_SENSOR ENABLED -#define MAGNETOMETER ENABLED -#define AIRSPEED_CRUISE 25 -#define THROTTLE_FAILSAFE ENABLED diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 177a11b37e..22d5d5cf86 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduPlane V2.28" +#define THISFIRMWARE "ArduPlane V2.30" /* Authors: Doug Weibel, Jose Julio, Jordi Munoz, Jason Short, Andrew Tridgell, Randy Mackay, Pat Hickey, John Arne Birkeland, Olivier Adler Thanks to: Chris Anderson, Michael Oborne, Paul Mather, Bill Premerlani, James Cohen, JB from rotorFX, Automatik, Fefenin, Peter Meister, Remzibi, Yury Smirnov, Sandro Benigno, Max Levine, Roberto Navoni, Lorenz Meier diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder.h b/Tools/ArduPPM/Libraries/PPM_Encoder.h index af132f7d3e..640882f2b1 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder.h @@ -1,11 +1,10 @@ -// Warning : Untested - Please use version 2.2.63 for field use. // ------------------------------------------------------------- -// PPM ENCODER V2.2.65 (25-09-2011) +// PPM ENCODER V2.2.66 (15-03-2012) // ------------------------------------------------------------- -// Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p) -// and PhoneDrone (ATmega32u2) +// Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p), +// PhoneDrone and APM2 (ATmega32u2) -// By: John Arne Birkeland - 2011 +// By: John Arne Birkeland - 2012 // APM v1.x adaptation and "difficult" receiver testing by Olivier ADLER // ------------------------------------------------------------- // Changelog: @@ -50,6 +49,11 @@ // - Changed brownout detection and FailSafe handling to work with XXU2 chips // - Minor variable and define naming changes to enhance readability +// 15-03-2012 +// V2.2.66 - Added APM2 (ATmega32U2) support for using TX and RX status leds to indicate PWM and PPM traffic +// - : = no pwm input detected, = speed of toggle indicate how many channel are active, = input lost (failsafe) +// - : = ppm output not started, = normal PWM->PPM output or PPM passtrough failsafe, = PPM passtrough + // ------------------------------------------------------------- #ifndef _PPM_ENCODER_H_ @@ -219,7 +223,7 @@ const uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] = }; // ------------------------------------------------------------- -// AVR parameters for PhoneDrone PPM Encoder and future boards also using ATmega16-32u2 +// AVR parameters for PhoneDrone and APM2 boards using ATmega32u2 #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #define SERVO_DDR DDRB @@ -347,6 +351,11 @@ void ppm_start( void ) // Restore interrupt status and register flags SREG = SREG_tmp; + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Turn on TX led if PPM generator is active + PORTD &= ~( 1<< PD5 ); + #endif } // ------------------------------------------------------------------------------ @@ -372,6 +381,11 @@ void ppm_stop( void ) // Restore interrupt status and register flags SREG = SREG_tmp; + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Turn off TX led if PPM generator is off + PORTD |= ( 1<< PD5 ); + #endif } // ------------------------------------------------------------------------------ @@ -387,7 +401,8 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and { ppm[ i ] = failsafe_ppm[ i ]; } - } + + } // If we are in PPM passtrough mode and a input signal has been detected, or if chip has been reset from a brown_out then start the PPM generator. if( ( servo_input_mode == PPM_PASSTROUGH_MODE && servo_input_missing == false ) || brownout_reset ) @@ -395,13 +410,18 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and // Start PPM generator ppm_start(); brownout_reset = false; - } + } // Set missing receiver signal flag servo_input_missing = true; // Reset servo input error flag servo_input_errors = 0; + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Turn on RX led if failsafe has triggered after ppm generator i active + if( ppm_generator_active ) PORTD &= ~( 1<< PD4 ); + #endif } // ------------------------------------------------------------------------------ @@ -413,8 +433,13 @@ ISR( SERVO_INT_VECTOR ) { // Servo pulse start timing static uint16_t servo_start[ SERVO_CHANNELS ] = { 0, 0, 0, 0, 0, 0, 0, 0 }; + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Toggle LED delay + static uint8_t led_delay = 0; + #endif - // Servo input pin storage + // Servo input pin storage static uint8_t servo_pins_old = 0; // Used to store current servo input pins @@ -454,6 +479,16 @@ ISR( SERVO_INT_VECTOR ) // Set servo input missing flag false to indicate that we have received servo input signals servo_input_missing = false; + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Toggle TX LED at PPM passtrough + if( ++led_delay > 128 ) // Toggle every 128th pulse + { + // Toggle TX led + PIND |= ( 1<< PD5 ); + led_delay = 0; + } + #endif + // Leave interrupt return; } @@ -511,6 +546,8 @@ CHECK_PINS_LOOP: // Input servo pin check loop // Update ppm[..] ppm[ _ppm_channel ] = servo_width; + + } } @@ -521,7 +558,7 @@ CHECK_PINS_NEXT: // Select next servo channel servo_channel++; - + // Check channel and process if needed if( servo_channel < SERVO_CHANNELS ) goto CHECK_PINS_LOOP; @@ -531,6 +568,11 @@ CHECK_PINS_ERROR: // Used to indicate invalid servo input signals servo_input_errors++; + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Delay LED toggle + led_delay = 0; + #endif goto CHECK_PINS_NEXT; @@ -555,6 +597,15 @@ CHECK_PINS_DONE: // Clear interrupt event from already processed pin changes PCIFR |= (1 << SERVO_INT_CLEAR_FLAG); + + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Toggle RX LED when finished receiving servo pulses + if( ++led_delay > 64 ) // Toggle led every 64th time + { + PIND |= ( 1<< PD4 ); + led_delay = 0; + } + #endif } // ------------------------------------------------------------------------------ @@ -571,8 +622,15 @@ ISR( PPM_INT_VECTOR ) PPM_COMPARE += ppm[ ppm_channel ]; // Select the next ppm channel - if( ++ppm_channel >= PPM_ARRAY_MAX ) ppm_channel = 0; + if( ++ppm_channel >= PPM_ARRAY_MAX ) + { + ppm_channel = 0; + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Blink TX LED when PPM generator has finished a pulse train + PIND |= ( 1<< PD5 ); + #endif + } } // ------------------------------------------------------------------------------ @@ -720,7 +778,6 @@ void ppm_encoder_init( void ) // Enable servo input interrupt PCICR |= (1 << SERVO_INT_ENABLE); - // PPM OUTPUT PIN // ------------------------------------------------------------------------------ // Set PPM pin to output @@ -737,6 +794,9 @@ void ppm_encoder_init( void ) WDTCSR |= (1<(); - rateattitude = 10; + rateattitude = 3; rateposition = 3; - ratestatus = 1; + ratestatus = 3; ratesensors = 3; raterc = 3; datetime = DateTime.MinValue; @@ -581,9 +583,9 @@ namespace ArdupilotMega packetdropremote = sysstatus.packet_drop; - if (oldmode != mode && MainV2.speechenable && MainV2.talk != null && MainV2.getConfig("speechmodeenabled") == "True") + if (oldmode != mode && MainV2.speechEnable && MainV2.speechEngine != null && MainV2.getConfig("speechmodeenabled") == "True") { - MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); + MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechmode"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_SYS_STATUS] = null; @@ -732,9 +734,9 @@ namespace ArdupilotMega wpno = wpcur.seq; - if (oldwp != wpno && MainV2.speechenable && MainV2.talk != null && MainV2.getConfig("speechwaypointenabled") == "True") + if (oldwp != wpno && MainV2.speechEnable && MainV2.speechEngine != null && MainV2.getConfig("speechwaypointenabled") == "True") { - MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); + MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechwaypoint"))); } //MAVLink.packets[ArdupilotMega.MAVLink.MAVLINK_MSG_ID_WAYPOINT_CURRENT] = null; diff --git a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml index 8d2dd6c786..c64b47acfc 100644 --- a/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml +++ b/Tools/ArdupilotMegaPlanner/Firmware/firmware2.xml @@ -4,7 +4,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AP-2560-2.hex - ArduPlane V2.28 + ArduPlane V2.30 12 @@ -12,7 +12,7 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/APHIL-2560-2.hex - ArduPlane V2.28 HIL + ArduPlane V2.30 HIL #define FLIGHT_MODE_CHANNEL 8 #define FLIGHT_MODE_1 AUTO @@ -47,74 +47,74 @@ http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Quad-2560-2.hex - ArduCopter V2.4.1 Quad + ArduCopter V2.5 Quad #define FRAME_CONFIG QUAD_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Tri-2560-2.hex - ArduCopter V2.4.1 Tri + ArduCopter V2.5 Tri #define FRAME_CONFIG TRI_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Hexa-2560-2.hex - ArduCopter V2.4.1 Hexa X + ArduCopter V2.5 Hexa X #define FRAME_CONFIG HEXA_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Y6-2560-2.hex - ArduCopter V2.4.1 Y6 + ArduCopter V2.5 Y6 #define FRAME_CONFIG Y6_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octav-2560-2.hex - ArduCopter V2.4.1 Octa V + ArduCopter V2.5 Octa V #define FRAME_CONFIG OCTA_FRAME #define FRAME_ORIENTATION V_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Octa-2560-2.hex - ArduCopter V2.4.1 Octa X + ArduCopter V2.5 Octa X #define FRAME_CONFIG OCTA_FRAME - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-Heli-2560-2.hex - ArduCopter V2.4 Heli (2560 only) + ArduCopter V2.5 Heli (2560 only) #define AUTO_RESET_LOITER 0 #define FRAME_CONFIG HELI_FRAME @@ -156,13 +156,13 @@ #define NAV_LOITER_IMAX 10 - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-QUADHIL-2560-2.hex - ArduCopter V2.4.1 Quad Hil + ArduCopter V2.5 Quad Hil #define FRAME_CONFIG QUAD_FRAME #define FRAME_ORIENTATION PLUS_FRAME @@ -172,13 +172,13 @@ - 116 + 117 http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-1280.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560.hex http://meee146-planner.googlecode.com/git/Tools/ArdupilotMegaPlanner/Firmware/AC2-HELHIL-2560-2.hex - ArduCopter V2.4 Heli Hil + ArduCopter V2.5 Heli Hil #define HIL_MODE HIL_MODE_ATTITUDE @@ -224,6 +224,6 @@ - 116 + 117 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index 760c3222da..5ec9e1d4f9 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -287,6 +287,8 @@ this.BUT_load = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.BUT_compare = new ArdupilotMega.MyButton(); + this.label33 = new System.Windows.Forms.Label(); + this.CMB_ratesensors = new System.Windows.Forms.ComboBox(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); this.ConfigTabs.SuspendLayout(); this.TabAP.SuspendLayout(); @@ -1715,6 +1717,8 @@ // // TabPlanner // + this.TabPlanner.Controls.Add(this.label33); + this.TabPlanner.Controls.Add(this.CMB_ratesensors); this.TabPlanner.Controls.Add(this.label26); this.TabPlanner.Controls.Add(this.CMB_videoresolutions); this.TabPlanner.Controls.Add(this.label12); @@ -2132,6 +2136,25 @@ this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); // + // label33 + // + resources.ApplyResources(this.label33, "label33"); + this.label33.Name = "label33"; + // + // CMB_ratesensors + // + this.CMB_ratesensors.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.CMB_ratesensors.FormattingEnabled = true; + this.CMB_ratesensors.Items.AddRange(new object[] { + resources.GetString("CMB_ratesensors.Items"), + resources.GetString("CMB_ratesensors.Items1"), + resources.GetString("CMB_ratesensors.Items2"), + resources.GetString("CMB_ratesensors.Items3"), + resources.GetString("CMB_ratesensors.Items4")}); + resources.ApplyResources(this.CMB_ratesensors, "CMB_ratesensors"); + this.CMB_ratesensors.Name = "CMB_ratesensors"; + this.CMB_ratesensors.SelectedIndexChanged += new System.EventHandler(this.CMB_ratesensors_SelectedIndexChanged); + // // Configuration // resources.ApplyResources(this, "$this"); @@ -2521,5 +2544,7 @@ private System.Windows.Forms.Label lblSTAB_D; private System.Windows.Forms.NumericUpDown TUNE_LOW; private System.Windows.Forms.NumericUpDown TUNE_HIGH; + private System.Windows.Forms.Label label33; + private System.Windows.Forms.ComboBox CMB_ratesensors; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs index 08f7cf31c4..fa70b64583 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.cs @@ -145,6 +145,7 @@ namespace ArdupilotMega.GCSViews CMB_rateposition.Text = MainV2.cs.rateposition.ToString(); CMB_raterc.Text = MainV2.cs.raterc.ToString(); CMB_ratestatus.Text = MainV2.cs.ratestatus.ToString(); + CMB_ratesensors.Text = MainV2.cs.ratesensors.ToString(); if (MainV2.config["CHK_GDIPlus"] != null) @@ -862,10 +863,10 @@ namespace ArdupilotMega.GCSViews private void CHK_enablespeech_CheckedChanged(object sender, EventArgs e) { - MainV2.speechenable = CHK_enablespeech.Checked; + MainV2.speechEnable = CHK_enablespeech.Checked; MainV2.config["speechenable"] = CHK_enablespeech.Checked; - if (MainV2.talk != null) - MainV2.talk.SpeakAsyncCancelAll(); + if (MainV2.speechEngine != null) + MainV2.speechEngine.SpeakAsyncCancelAll(); } private void CMB_language_SelectedIndexChanged(object sender, EventArgs e) { @@ -1183,5 +1184,11 @@ namespace ArdupilotMega.GCSViews } return base.ProcessCmdKey(ref msg, keyData); } + + private void CMB_ratesensors_SelectedIndexChanged(object sender, EventArgs e) + { + MainV2.config[((ComboBox)sender).Name] = ((ComboBox)sender).Text; + MainV2.cs.ratesensors = byte.Parse(((ComboBox)sender).Text); + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index 0cf4249838..58f1fa790f 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -363,15 +363,6 @@ 0 - - 543, 297 - - - 78, 20 - - - 22 - TUNE_LOW @@ -384,15 +375,6 @@ 0 - - 627, 296 - - - 78, 20 - - - 21 - TUNE_HIGH @@ -405,23 +387,11 @@ 1 - - 534, 270 - - - 53, 23 - - - 20 - - - Ch6 Opt - myLabel2 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null TabAC @@ -429,81 +399,6 @@ 2 - - CH6_NONE - - - CH6_STABILIZE_KP - - - CH6_STABILIZE_KI - - - CH6_YAW_KP - - - CH6_RATE_KP - - - CH6_RATE_KI - - - CH6_YAW_RATE_KP - - - CH6_THROTTLE_KP - - - CH6_TOP_BOTTOM_RATIO - - - CH6_RELAY - - - CH6_TRAVERSE_SPEED - - - CH6_NAV_P - - - CH6_LOITER_P - - - CH6_HELI_EXTERNAL_GYRO - - - CH6_THR_HOLD_KP - - - CH6_Z_GAIN - - - CH6_DAMP - - - CH6_OPTFLOW_KP - - - CH6_OPTFLOW_KI - - - CH6_OPTFLOW_KD - - - CH6_NAV_I - - - CH6_RATE_KD - - - 593, 270 - - - 112, 21 - - - 19 - TUNE @@ -516,23 +411,11 @@ 3 - - 534, 322 - - - 53, 23 - - - 18 - - - Ch7 Opt - myLabel1 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null TabAC @@ -540,39 +423,6 @@ 4 - - Do Nothing - - - - - - - - - Simple Mode - - - RTL - - - - - - - - - Save WP - - - 593, 322 - - - 112, 21 - - - 17 - CH7_OPT @@ -585,114 +435,6 @@ 5 - - THR_RATE_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 0 - - - label29 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 1 - - - label14 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 2 - - - THR_RATE_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 3 - - - THR_RATE_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 4 - - - label20 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 5 - - - THR_RATE_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 6 - - - label25 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox5 - - - 7 - - - 6, 260 - - - 170, 110 - - - 16 - - - Throttle Rate - groupBox5 @@ -705,21 +447,6 @@ 6 - - True - - - 3, 240 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - CHK_lockrollpitch @@ -732,138 +459,6 @@ 7 - - NAV_LAT_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 0 - - - label27 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 1 - - - WP_SPEED_MAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 2 - - - label9 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 3 - - - NAV_LAT_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 4 - - - label13 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 5 - - - NAV_LAT_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 6 - - - label15 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 7 - - - NAV_LAT_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 8 - - - label16 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox4 - - - 9 - - - 534, 126 - - - 170, 131 - - - 0 - - - Nav WP - groupBox4 @@ -876,42 +471,6 @@ 8 - - XTRK_GAIN_SC1 - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 0 - - - label18 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox6 - - - 1 - - - 358, 260 - - - 170, 43 - - - 2 - - - Crosstrack Correction - groupBox6 @@ -924,90 +483,6 @@ 9 - - THR_ALT_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 0 - - - label19 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 1 - - - THR_ALT_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 2 - - - label21 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 3 - - - THR_ALT_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 4 - - - label22 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox7 - - - 5 - - - 182, 260 - - - 170, 110 - - - 3 - - - Altitude Hold - groupBox7 @@ -1020,90 +495,6 @@ 10 - - HLD_LAT_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 0 - - - label28 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 1 - - - HLD_LAT_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 2 - - - label30 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 3 - - - HLD_LAT_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 4 - - - label31 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox19 - - - 5 - - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - groupBox19 @@ -1116,90 +507,6 @@ 11 - - STB_YAW_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 0 - - - label32 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 1 - - - STB_YAW_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 2 - - - label34 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 3 - - - STB_YAW_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 4 - - - label35 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox20 - - - 5 - - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - groupBox20 @@ -1212,114 +519,6 @@ 12 - - STAB_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 0 - - - lblSTAB_D - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 1 - - - STB_PIT_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 2 - - - label36 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 3 - - - STB_PIT_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 4 - - - label41 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 5 - - - STB_PIT_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 6 - - - label42 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox21 - - - 7 - - - 182, 6 - - - 170, 114 - - - 8 - - - Stabilize Pitch - groupBox21 @@ -1332,90 +531,6 @@ 13 - - STB_RLL_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 0 - - - label43 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 1 - - - STB_RLL_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 2 - - - label45 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 3 - - - STB_RLL_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 4 - - - label46 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox22 - - - 5 - - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - groupBox22 @@ -1428,114 +543,6 @@ 14 - - RATE_YAW_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 0 - - - label10 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 1 - - - RATE_YAW_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 2 - - - label47 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 3 - - - RATE_YAW_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 4 - - - label77 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 5 - - - RATE_YAW_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 6 - - - label82 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox23 - - - 7 - - - 358, 126 - - - 170, 108 - - - 10 - - - Rate Yaw - groupBox23 @@ -1548,114 +555,6 @@ 15 - - RATE_PIT_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 0 - - - label11 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 1 - - - RATE_PIT_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 2 - - - label84 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 3 - - - RATE_PIT_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 4 - - - label86 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 5 - - - RATE_PIT_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 6 - - - label87 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox24 - - - 7 - - - 182, 126 - - - 170, 108 - - - 11 - - - Rate Pitch - groupBox24 @@ -1668,114 +567,6 @@ 16 - - RATE_RLL_D - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 0 - - - label17 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 1 - - - RATE_RLL_IMAX - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 2 - - - label88 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 3 - - - RATE_RLL_I - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 4 - - - label90 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 5 - - - RATE_RLL_P - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 6 - - - label91 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - groupBox25 - - - 7 - - - 6, 126 - - - 170, 108 - - - 12 - - - Rate Roll - groupBox25 @@ -1815,6 +606,84 @@ 1 + + NoControl + + + 532, 244 + + + 43, 13 + + + 42 + + + Sensor + + + label33 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + 0 + + + 1 + + + 3 + + + 10 + + + 50 + + + 579, 241 + + + 40, 21 + + + 43 + + + CMB_ratesensors + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + NoControl + + + 30, 50 + + + 100, 23 + + + 41 + + + Video Format + label26 @@ -1825,6 +694,15 @@ TabPlanner + 2 + + + 139, 47 + + + 408, 21 + + 0 @@ -1837,7 +715,22 @@ TabPlanner - 1 + 3 + + + NoControl + + + 30, 340 + + + 61, 13 + + + 39 + + + HUD label12 @@ -1849,7 +742,29 @@ TabPlanner - 2 + 4 + + + 17, 17 + + + NoControl + + + 139, 340 + + + 177, 17 + + + 40 + + + GDI+ (old type) + + + OpenGL = Disabled +GDI+ = Enabled CHK_GDIPlus @@ -1861,7 +776,22 @@ TabPlanner - 3 + 5 + + + NoControl + + + 30, 318 + + + 61, 13 + + + 37 + + + Waypoints label24 @@ -1873,7 +803,22 @@ TabPlanner - 4 + 6 + + + NoControl + + + 139, 317 + + + 177, 17 + + + 38 + + + Load Waypoints on connect? CHK_loadwponconnect @@ -1885,7 +830,22 @@ TabPlanner - 5 + 7 + + + NoControl + + + 30, 292 + + + 103, 18 + + + 36 + + + Track Length label23 @@ -1897,7 +857,19 @@ TabPlanner - 6 + 8 + + + 139, 291 + + + 67, 20 + + + 35 + + + On the Flight Data Tab NUM_tracklength @@ -1909,7 +881,22 @@ TabPlanner - 7 + 9 + + + NoControl + + + 579, 107 + + + 102, 17 + + + 34 + + + Alt Warning CHK_speechaltwarning @@ -1921,7 +908,22 @@ TabPlanner - 8 + 10 + + + NoControl + + + 30, 269 + + + 61, 13 + + + 0 + + + APM Reset label108 @@ -1933,7 +935,22 @@ TabPlanner - 9 + 11 + + + NoControl + + + 139, 267 + + + 163, 17 + + + 1 + + + Reset APM on USB Connect CHK_resetapmonconnect @@ -1945,7 +962,25 @@ TabPlanner - 10 + 12 + + + Bottom, Left + + + NoControl + + + 33, 411 + + + 144, 17 + + + 2 + + + Mavlink Message Debug CHK_mavdebug @@ -1957,7 +992,22 @@ TabPlanner - 11 + 13 + + + NoControl + + + 454, 244 + + + 22, 13 + + + 3 + + + RC label107 @@ -1969,7 +1019,28 @@ TabPlanner - 12 + 14 + + + 0 + + + 1 + + + 3 + + + 10 + + + 485, 240 + + + 40, 21 + + + 4 CMB_raterc @@ -1981,7 +1052,22 @@ TabPlanner - 13 + 15 + + + NoControl + + + 334, 244 + + + 69, 13 + + + 5 + + + Mode/Status label104 @@ -1993,7 +1079,22 @@ TabPlanner - 14 + 16 + + + NoControl + + + 234, 244 + + + 44, 13 + + + 6 + + + Position label103 @@ -2005,7 +1106,22 @@ TabPlanner - 15 + 17 + + + NoControl + + + 136, 244 + + + 43, 13 + + + 7 + + + Attitude label102 @@ -2017,7 +1133,22 @@ TabPlanner - 16 + 18 + + + NoControl + + + 27, 244 + + + 84, 13 + + + 8 + + + Telemetry Rates label101 @@ -2029,7 +1160,28 @@ TabPlanner - 17 + 19 + + + 0 + + + 1 + + + 3 + + + 10 + + + 408, 240 + + + 40, 21 + + + 9 CMB_ratestatus @@ -2041,7 +1193,28 @@ TabPlanner - 18 + 20 + + + 0 + + + 1 + + + 3 + + + 10 + + + 288, 240 + + + 40, 21 + + + 10 CMB_rateposition @@ -2053,7 +1226,28 @@ TabPlanner - 19 + 21 + + + 0 + + + 1 + + + 3 + + + 10 + + + 188, 240 + + + 40, 21 + + + 11 CMB_rateattitude @@ -2065,7 +1259,23 @@ TabPlanner - 20 + 22 + + + NoControl + + + 283, 209 + + + 402, 13 + + + 12 + + + NOTE: The Configuration Tab will NOT display these units, as those are raw values. + label99 @@ -2077,7 +1287,22 @@ TabPlanner - 21 + 23 + + + NoControl + + + 30, 217 + + + 65, 13 + + + 13 + + + Speed Units label98 @@ -2089,7 +1314,22 @@ TabPlanner - 22 + 24 + + + NoControl + + + 30, 189 + + + 52, 13 + + + 14 + + + Dist Units label97 @@ -2101,7 +1341,16 @@ TabPlanner - 23 + 25 + + + 139, 214 + + + 138, 21 + + + 15 CMB_speedunits @@ -2113,7 +1362,16 @@ TabPlanner - 24 + 26 + + + 139, 187 + + + 138, 21 + + + 16 CMB_distunits @@ -2125,7 +1383,22 @@ TabPlanner - 25 + 27 + + + NoControl + + + 30, 162 + + + 45, 13 + + + 17 + + + Joystick label96 @@ -2137,7 +1410,22 @@ TabPlanner - 26 + 28 + + + NoControl + + + 30, 111 + + + 44, 13 + + + 18 + + + Speech label95 @@ -2149,7 +1437,22 @@ TabPlanner - 27 + 29 + + + NoControl + + + 471, 107 + + + 102, 17 + + + 19 + + + Battery Warning CHK_speechbattery @@ -2161,7 +1464,22 @@ TabPlanner - 28 + 30 + + + NoControl + + + 378, 107 + + + 87, 17 + + + 20 + + + Time Interval CHK_speechcustom @@ -2173,7 +1491,22 @@ TabPlanner - 29 + 31 + + + NoControl + + + 322, 107 + + + 56, 17 + + + 21 + + + Mode CHK_speechmode @@ -2185,7 +1518,22 @@ TabPlanner - 30 + 32 + + + NoControl + + + 245, 107 + + + 71, 17 + + + 22 + + + Waypoint CHK_speechwaypoint @@ -2197,7 +1545,22 @@ TabPlanner - 31 + 33 + + + NoControl + + + 30, 83 + + + 57, 13 + + + 23 + + + OSD Color label94 @@ -2209,7 +1572,16 @@ TabPlanner - 32 + 34 + + + 139, 80 + + + 138, 21 + + + 24 CMB_osdcolor @@ -2221,7 +1593,16 @@ TabPlanner - 33 + 35 + + + 139, 131 + + + 138, 21 + + + 25 CMB_language @@ -2233,7 +1614,22 @@ TabPlanner - 34 + 36 + + + NoControl + + + 30, 135 + + + 69, 13 + + + 26 + + + UI Language label93 @@ -2245,7 +1641,22 @@ TabPlanner - 35 + 37 + + + NoControl + + + 139, 107 + + + 99, 17 + + + 27 + + + Enable Speech CHK_enablespeech @@ -2257,7 +1668,22 @@ TabPlanner - 36 + 38 + + + NoControl + + + 552, 15 + + + 125, 17 + + + 28 + + + Enable HUD Overlay CHK_hudshow @@ -2269,7 +1695,22 @@ TabPlanner - 37 + 39 + + + NoControl + + + 30, 16 + + + 100, 23 + + + 29 + + + Video Device label92 @@ -2281,7 +1722,16 @@ TabPlanner - 38 + 40 + + + 139, 13 + + + 245, 21 + + + 30 CMB_videosources @@ -2293,43 +1743,88 @@ TabPlanner - 39 + 41 + + + NoControl + + + 139, 158 + + + 99, 23 + + + 31 + + + Joystick Setup BUT_Joystick - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null TabPlanner - 40 + 42 + + + NoControl + + + 471, 11 + + + 75, 23 + + + 32 + + + Stop BUT_videostop - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null TabPlanner - 41 + 43 + + + NoControl + + + 390, 11 + + + 75, 23 + + + 33 + + + Start BUT_videostart - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null TabPlanner - 42 + 44 4, 22 @@ -5868,6 +5363,348 @@ 7 + + 543, 297 + + + 78, 20 + + + 22 + + + TUNE_LOW + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 0 + + + 627, 296 + + + 78, 20 + + + 21 + + + TUNE_HIGH + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 1 + + + 534, 270 + + + 53, 23 + + + 20 + + + Ch6 Opt + + + myLabel2 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + + + TabAC + + + 2 + + + CH6_NONE + + + CH6_STABILIZE_KP + + + CH6_STABILIZE_KI + + + CH6_YAW_KP + + + CH6_RATE_KP + + + CH6_RATE_KI + + + CH6_YAW_RATE_KP + + + CH6_THROTTLE_KP + + + CH6_TOP_BOTTOM_RATIO + + + CH6_RELAY + + + CH6_TRAVERSE_SPEED + + + CH6_NAV_P + + + CH6_LOITER_P + + + CH6_HELI_EXTERNAL_GYRO + + + CH6_THR_HOLD_KP + + + CH6_Z_GAIN + + + CH6_DAMP + + + CH6_OPTFLOW_KP + + + CH6_OPTFLOW_KI + + + CH6_OPTFLOW_KD + + + CH6_NAV_I + + + CH6_RATE_KD + + + 593, 270 + + + 112, 21 + + + 19 + + + TUNE + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 3 + + + 534, 322 + + + 53, 23 + + + 18 + + + Ch7 Opt + + + myLabel1 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null + + + TabAC + + + 4 + + + Do Nothing + + + + + + + + + Simple Mode + + + RTL + + + + + + + + + Save WP + + + 593, 322 + + + 112, 21 + + + 17 + + + CH7_OPT + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 5 + + + THR_RATE_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 0 + + + label29 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 1 + + + label14 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 2 + + + THR_RATE_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 3 + + + THR_RATE_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 4 + + + label20 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 5 + + + THR_RATE_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 6 + + + label25 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 7 + + + 6, 260 + + + 170, 110 + + + 16 + + + Throttle Rate + + + groupBox5 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 6 + 80, 60 @@ -6060,6 +5897,177 @@ 7 + + True + + + 3, 240 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 7 + + + NAV_LAT_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + WP_SPEED_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + NAV_LAT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + NAV_LAT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + NAV_LAT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 8 + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 9 + + + 534, 126 + + + 170, 131 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 8 + 80, 60 @@ -6300,6 +6308,54 @@ 9 + + XTRK_GAIN_SC1 + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 358, 260 + + + 170, 43 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 9 + 80, 13 @@ -6348,6 +6404,102 @@ 1 + + THR_ALT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + THR_ALT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + THR_ALT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 182, 260 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 10 + 80, 63 @@ -6492,6 +6644,102 @@ 5 + + HLD_LAT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + HLD_LAT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + HLD_LAT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 11 + 80, 61 @@ -6636,6 +6884,102 @@ 5 + + STB_YAW_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + STB_YAW_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + STB_YAW_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 12 + 80, 63 @@ -6780,6 +7124,126 @@ 5 + + STAB_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + lblSTAB_D + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + STB_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + STB_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + STB_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 6 + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 7 + + + 182, 6 + + + 170, 114 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 13 + 80, 88 @@ -6972,6 +7436,102 @@ 7 + + STB_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + STB_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + STB_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 14 + 80, 63 @@ -7116,6 +7676,126 @@ 5 + + RATE_YAW_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + label10 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + RATE_YAW_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + RATE_YAW_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + RATE_YAW_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 6 + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 7 + + + 358, 126 + + + 170, 108 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 15 + 80, 60 @@ -7308,6 +7988,126 @@ 7 + + RATE_PIT_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + label11 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + RATE_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + RATE_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + RATE_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 6 + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 7 + + + 182, 126 + + + 170, 108 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 16 + 80, 60 @@ -7500,6 +8300,126 @@ 7 + + RATE_RLL_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + label17 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + RATE_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + RATE_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + RATE_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 6 + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 7 + + + 6, 126 + + + 170, 108 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 17 + 80, 60 @@ -7692,1163 +8612,6 @@ 7 - - NoControl - - - 30, 50 - - - 100, 23 - - - 41 - - - Video Format - - - label26 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 0 - - - 139, 47 - - - 408, 21 - - - 0 - - - CMB_videoresolutions - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 1 - - - NoControl - - - 30, 340 - - - 61, 13 - - - 39 - - - HUD - - - label12 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 2 - - - 17, 17 - - - NoControl - - - 139, 340 - - - 177, 17 - - - 40 - - - GDI+ (old type) - - - OpenGL = Disabled -GDI+ = Enabled - - - CHK_GDIPlus - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 3 - - - NoControl - - - 30, 318 - - - 61, 13 - - - 37 - - - Waypoints - - - label24 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 4 - - - NoControl - - - 139, 317 - - - 177, 17 - - - 38 - - - Load Waypoints on connect? - - - CHK_loadwponconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 5 - - - NoControl - - - 30, 292 - - - 103, 18 - - - 36 - - - Track Length - - - label23 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 6 - - - 139, 291 - - - 67, 20 - - - 35 - - - On the Flight Data Tab - - - NUM_tracklength - - - System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 7 - - - NoControl - - - 579, 107 - - - 102, 17 - - - 34 - - - Alt Warning - - - CHK_speechaltwarning - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 8 - - - NoControl - - - 30, 269 - - - 61, 13 - - - 0 - - - APM Reset - - - label108 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 9 - - - NoControl - - - 139, 267 - - - 163, 17 - - - 1 - - - Reset APM on USB Connect - - - CHK_resetapmonconnect - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 10 - - - Bottom, Left - - - NoControl - - - 33, 411 - - - 144, 17 - - - 2 - - - Mavlink Message Debug - - - CHK_mavdebug - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 11 - - - NoControl - - - 590, 244 - - - 22, 13 - - - 3 - - - RC - - - label107 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 12 - - - 0 - - - 1 - - - 3 - - - 10 - - - 621, 240 - - - 80, 21 - - - 4 - - - CMB_raterc - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 13 - - - NoControl - - - 425, 244 - - - 69, 13 - - - 5 - - - Mode/Status - - - label104 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 14 - - - NoControl - - - 280, 244 - - - 44, 13 - - - 6 - - - Position - - - label103 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 15 - - - NoControl - - - 136, 244 - - - 43, 13 - - - 7 - - - Attitude - - - label102 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 16 - - - NoControl - - - 27, 244 - - - 84, 13 - - - 8 - - - Telemetry Rates - - - label101 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 17 - - - 0 - - - 1 - - - 3 - - - 10 - - - 499, 240 - - - 80, 21 - - - 9 - - - CMB_ratestatus - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 18 - - - 0 - - - 1 - - - 3 - - - 10 - - - 334, 240 - - - 80, 21 - - - 10 - - - CMB_rateposition - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 19 - - - 0 - - - 1 - - - 3 - - - 10 - - - 188, 240 - - - 80, 21 - - - 11 - - - CMB_rateattitude - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 20 - - - NoControl - - - 283, 209 - - - 402, 13 - - - 12 - - - NOTE: The Configuration Tab will NOT display these units, as those are raw values. - - - - label99 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 21 - - - NoControl - - - 30, 217 - - - 65, 13 - - - 13 - - - Speed Units - - - label98 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 22 - - - NoControl - - - 30, 189 - - - 52, 13 - - - 14 - - - Dist Units - - - label97 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 23 - - - 139, 214 - - - 138, 21 - - - 15 - - - CMB_speedunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 24 - - - 139, 187 - - - 138, 21 - - - 16 - - - CMB_distunits - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 25 - - - NoControl - - - 30, 162 - - - 45, 13 - - - 17 - - - Joystick - - - label96 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 26 - - - NoControl - - - 30, 111 - - - 44, 13 - - - 18 - - - Speech - - - label95 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 27 - - - NoControl - - - 471, 107 - - - 102, 17 - - - 19 - - - Battery Warning - - - CHK_speechbattery - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 28 - - - NoControl - - - 378, 107 - - - 87, 17 - - - 20 - - - Time Interval - - - CHK_speechcustom - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 29 - - - NoControl - - - 322, 107 - - - 56, 17 - - - 21 - - - Mode - - - CHK_speechmode - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 30 - - - NoControl - - - 245, 107 - - - 71, 17 - - - 22 - - - Waypoint - - - CHK_speechwaypoint - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 31 - - - NoControl - - - 30, 83 - - - 57, 13 - - - 23 - - - OSD Color - - - label94 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 32 - - - 139, 80 - - - 138, 21 - - - 24 - - - CMB_osdcolor - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 33 - - - 139, 131 - - - 138, 21 - - - 25 - - - CMB_language - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 34 - - - NoControl - - - 30, 135 - - - 69, 13 - - - 26 - - - UI Language - - - label93 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 35 - - - NoControl - - - 139, 107 - - - 99, 17 - - - 27 - - - Enable Speech - - - CHK_enablespeech - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 36 - - - NoControl - - - 552, 15 - - - 125, 17 - - - 28 - - - Enable HUD Overlay - - - CHK_hudshow - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 37 - - - NoControl - - - 30, 16 - - - 100, 23 - - - 29 - - - Video Device - - - label92 - - - System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 38 - - - 139, 13 - - - 245, 21 - - - 30 - - - CMB_videosources - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabPlanner - - - 39 - - - NoControl - - - 139, 158 - - - 99, 23 - - - 31 - - - Joystick Setup - - - BUT_Joystick - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - TabPlanner - - - 40 - - - NoControl - - - 471, 11 - - - 75, 23 - - - 32 - - - Stop - - - BUT_videostop - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - TabPlanner - - - 41 - - - NoControl - - - 390, 11 - - - 75, 23 - - - 33 - - - Start - - - BUT_videostart - - - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - TabPlanner - - - 42 - 0, 0 @@ -8889,7 +8652,7 @@ GDI+ = Enabled BUT_rerequestparams - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -8922,7 +8685,7 @@ GDI+ = Enabled BUT_writePIDS - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -8958,7 +8721,7 @@ GDI+ = Enabled BUT_save - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -8994,7 +8757,7 @@ GDI+ = Enabled BUT_load - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -9027,7 +8790,7 @@ GDI+ = Enabled BUT_compare - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -9084,6 +8847,6 @@ GDI+ = Enabled Configuration - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs index 643994b2db..e25765c0d1 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Firmware.cs @@ -22,7 +22,7 @@ namespace ArdupilotMega.GCSViews fd.ShowDialog(); if (File.Exists(fd.FileName)) { - UploadFlash(fd.FileName, ArduinoDetect.DetectBoard(MainV2.comportname)); + UploadFlash(fd.FileName, ArduinoDetect.DetectBoard(MainV2.comPortName)); } return true; } @@ -312,7 +312,7 @@ namespace ArdupilotMega.GCSViews MainV2.comPort.BaseStream.DtrEnable = false; MainV2.comPort.Close(); System.Threading.Thread.Sleep(100); - MainV2.givecomport = true; + MainV2.giveComport = true; try { @@ -326,7 +326,7 @@ namespace ArdupilotMega.GCSViews this.Refresh(); - board = ArduinoDetect.DetectBoard(MainV2.comportname); + board = ArduinoDetect.DetectBoard(MainV2.comPortName); if (board == "") { @@ -338,7 +338,7 @@ namespace ArdupilotMega.GCSViews try { - apmformat_version = ArduinoDetect.decodeApVar(MainV2.comportname, board); + apmformat_version = ArduinoDetect.decodeApVar(MainV2.comPortName, board); } catch { } @@ -480,7 +480,7 @@ namespace ArdupilotMega.GCSViews try { - port.PortName = MainV2.comportname; + port.PortName = MainV2.comPortName; port.Open(); @@ -592,7 +592,7 @@ namespace ArdupilotMega.GCSViews port.Close(); } flashing = false; - MainV2.givecomport = false; + MainV2.giveComport = false; } void port_Progress(int progress,string status) diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs index 839114ccca..295cbb8ff0 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.Designer.cs @@ -1208,6 +1208,7 @@ this.CHK_autopan.Name = "CHK_autopan"; this.toolTip1.SetToolTip(this.CHK_autopan, resources.GetString("CHK_autopan.ToolTip")); this.CHK_autopan.UseVisualStyleBackColor = true; + this.CHK_autopan.CheckedChanged += new System.EventHandler(this.CHK_autopan_CheckedChanged); // // CB_tuning // diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs index f6a6c224c9..36679c0fc0 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.cs @@ -230,6 +230,9 @@ namespace ArdupilotMega.GCSViews Zoomlevel.Minimum = gMapControl1.MinZoom; Zoomlevel.Maximum = gMapControl1.MaxZoom + 1; Zoomlevel.Value = Convert.ToDecimal(gMapControl1.Zoom); + + if (MainV2.config["CHK_autopan"] != null) + CHK_autopan.Checked = bool.Parse(MainV2.config["CHK_autopan"].ToString()); } private void mainloop() @@ -255,7 +258,7 @@ namespace ArdupilotMega.GCSViews { if (threadrun == 0) { return; } - if (MainV2.givecomport == true) + if (MainV2.giveComport == true) { System.Threading.Thread.Sleep(20); continue; @@ -353,6 +356,9 @@ namespace ArdupilotMega.GCSViews } else { + // ensure we know to stop + if (MainV2.comPort.logreadmode) + MainV2.comPort.logreadmode = false; updatePlayPauseButton(false); } @@ -911,15 +917,15 @@ namespace ArdupilotMega.GCSViews try { - MainV2.givecomport = true; + MainV2.giveComport = true; MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2); GuidedModeWP = new PointLatLngAlt(gotohere.lat, gotohere.lng, gotohere.alt,"Guided Mode"); - MainV2.givecomport = false; + MainV2.giveComport = false; } - catch (Exception ex) { MainV2.givecomport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); } + catch (Exception ex) { MainV2.giveComport = false; CustomMessageBox.Show("Error sending command : " + ex.Message); } } @@ -1765,5 +1771,10 @@ print 'Roll complete' scr.runScript(tb.Text); } } + + private void CHK_autopan_CheckedChanged(object sender, EventArgs e) + { + MainV2.config["CHK_autopan"] = CHK_autopan.Checked.ToString(); + } } } \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index 896d5a711c..2157889864 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -208,7 +208,7 @@ hud1 - hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + hud.HUD, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null SubMainLeft.Panel1 @@ -247,7 +247,7 @@ BUT_script - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -280,7 +280,7 @@ BUT_joystick - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -310,7 +310,7 @@ BUT_quickmanual - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -340,7 +340,7 @@ BUT_quickrtl - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -370,7 +370,7 @@ BUT_quickauto - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -424,7 +424,7 @@ BUT_setwp - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -475,7 +475,7 @@ BUT_setmode - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -505,7 +505,7 @@ BUT_clear_track - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -556,7 +556,7 @@ BUT_Homealt - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -586,7 +586,7 @@ BUT_RAWSensor - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -616,7 +616,7 @@ BUTrestartmission - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -646,7 +646,7 @@ BUTactiondo - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabActions @@ -700,7 +700,7 @@ Gvspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabGauges @@ -730,7 +730,7 @@ Gheading - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabGauges @@ -760,7 +760,7 @@ Galt - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabGauges @@ -793,7 +793,7 @@ Gspeed - AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + AGaugeApp.AGauge, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabGauges @@ -874,7 +874,7 @@ lbl_logpercent - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabTLogs @@ -925,7 +925,7 @@ BUT_log2kml - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabTLogs @@ -976,7 +976,7 @@ BUT_playlog - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1003,7 +1003,7 @@ BUT_loadtelem - ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null tabTLogs @@ -1189,7 +1189,7 @@ lbl_winddir - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1219,7 +1219,7 @@ lbl_windvel - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1391,7 +1391,7 @@ gMapControl1 - ArdupilotMega.myGMAP, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.myGMAP, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null splitContainer1.Panel2 @@ -1454,7 +1454,7 @@ TXT_lat - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null panel1 @@ -1511,7 +1511,7 @@ label1 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null panel1 @@ -1541,7 +1541,7 @@ TXT_long - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null panel1 @@ -1571,7 +1571,7 @@ TXT_alt - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null panel1 @@ -1772,7 +1772,7 @@ label6 - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null $this @@ -1850,6 +1850,6 @@ FlightData - System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + System.Windows.Forms.MyUserControl, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=null \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index b6aeb84394..490d6f88fa 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -1012,7 +1012,6 @@ namespace ArdupilotMega.GCSViews t1.Name = "Row number updater"; t1.IsBackground = true; t1.Start(); - MainV2.threads.Add(t1); long temp = System.Diagnostics.Stopwatch.GetTimestamp(); @@ -1225,7 +1224,7 @@ namespace ArdupilotMega.GCSViews throw new Exception("Please Connect First!"); } - MainV2.givecomport = true; + MainV2.giveComport = true; param = port.param; @@ -1262,7 +1261,7 @@ namespace ArdupilotMega.GCSViews catch (Exception exx) { log.Info(exx.ToString()); } } - MainV2.givecomport = false; + MainV2.giveComport = false; BUT_read.Enabled = true; @@ -1334,7 +1333,7 @@ namespace ArdupilotMega.GCSViews throw new Exception("Please Connect First!"); } - MainV2.givecomport = true; + MainV2.giveComport = true; Locationwp home = new Locationwp(); @@ -1414,9 +1413,9 @@ namespace ArdupilotMega.GCSViews ((Controls.ProgressReporterDialogue)sender).UpdateProgressAndStatus(100, "Done."); } - catch (Exception ex) { MainV2.givecomport = false; throw ex; } + catch (Exception ex) { MainV2.giveComport = false; throw ex; } - MainV2.givecomport = false; + MainV2.giveComport = false; } /// diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index e0491361e3..f28305e452 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -336,7 +336,6 @@ namespace ArdupilotMega.GCSViews IsBackground = true }; t11.Start(); - MainV2.threads.Add(t11); timer_servo_graph.Start(); } else diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs index 24aeb9fdd1..b11ff009d1 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Terminal.cs @@ -171,14 +171,14 @@ namespace ArdupilotMega.GCSViews { try { - MainV2.givecomport = true; + MainV2.giveComport = true; if (comPort.IsOpen) comPort.Close(); comPort.ReadBufferSize = 1024 * 1024; - comPort.PortName = MainV2.comportname; + comPort.PortName = MainV2.comPortName; comPort.Open(); @@ -232,7 +232,6 @@ namespace ArdupilotMega.GCSViews t11.IsBackground = true; t11.Name = "Terminal serial thread"; t11.Start(); - MainV2.threads.Add(t11); // doesnt seem to work on mac //comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); diff --git a/Tools/ArdupilotMegaPlanner/Log.cs b/Tools/ArdupilotMegaPlanner/Log.cs index 53953f1449..5e9717d3e5 100644 --- a/Tools/ArdupilotMegaPlanner/Log.cs +++ b/Tools/ArdupilotMegaPlanner/Log.cs @@ -118,7 +118,6 @@ namespace ArdupilotMega log.Info("Comport thread close"); }) {Name = "comport reader"}; t11.Start(); - MainV2.threads.Add(t11); // doesnt seem to work on mac //comPort.DataReceived += new SerialDataReceivedEventHandler(comPort_DataReceived); @@ -728,7 +727,6 @@ namespace ArdupilotMega System.Threading.Thread t11 = new System.Threading.Thread(delegate() { downloadthread(1, logcount); }); t11.Name = "Log Download All thread"; t11.Start(); - MainV2.threads.Add(t11); } } @@ -779,7 +777,6 @@ namespace ArdupilotMega System.Threading.Thread t11 = new System.Threading.Thread(delegate() { downloadsinglethread(); }); t11.Name = "Log download single thread"; t11.Start(); - MainV2.threads.Add(t11); } } diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index a659e0b912..a324140c75 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -172,7 +172,7 @@ namespace ArdupilotMega try { - MainV2.givecomport = true; + MainV2.giveComport = true; BaseStream.ReadBufferSize = 4 * 1024; @@ -190,8 +190,8 @@ namespace ArdupilotMega Thread.Sleep(1000); } - byte[] buffer; - byte[] buffer1; + byte[] buffer = new byte[0]; + byte[] buffer1 = new byte[0]; DateTime start = DateTime.Now; DateTime deadline = start.AddSeconds(CONNECT_TIMEOUT_SECONDS); @@ -217,7 +217,7 @@ namespace ArdupilotMega countDown.Stop(); if (BaseStream.IsOpen) BaseStream.Close(); - MainV2.givecomport = false; + MainV2.giveComport = false; return; } @@ -248,14 +248,18 @@ namespace ArdupilotMega // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); - buffer = getHeartBeat(); + // can see 2 heartbeat packets at any time, and will connect - was one after the other + + if (buffer.Length == 0) + buffer = getHeartBeat(); // incase we are in setup mode BaseStream.WriteLine("planner\rgcs\r"); System.Threading.Thread.Sleep(1); - buffer1 = getHeartBeat(); + if (buffer1.Length == 0) + buffer1 = getHeartBeat(); try { @@ -292,7 +296,7 @@ namespace ArdupilotMega if (frmProgressReporter.doWorkArgs.CancelAcknowledged == true) { - MainV2.givecomport = false; + MainV2.giveComport = false; if (BaseStream.IsOpen) BaseStream.Close(); return; @@ -305,7 +309,7 @@ namespace ArdupilotMega BaseStream.Close(); } catch { } - MainV2.givecomport = false; + MainV2.giveComport = false; // if (Progress != null) // Progress(-1, "Connect Failed\n" + e.Message); if (string.IsNullOrEmpty(progressWorkerEventArgs.ErrorMessage)) @@ -313,10 +317,11 @@ namespace ArdupilotMega throw e; } //frmProgressReporter.Close(); - MainV2.givecomport = false; + MainV2.giveComport = false; frmProgressReporter.UpdateProgressAndStatus(100, "Done."); log.Info("Done open " + sysid + " " + compid); packetslost = 0; + synclost = 0; } byte[] getHeartBeat() @@ -327,7 +332,7 @@ namespace ArdupilotMega byte[] buffer = readPacket(); if (buffer.Length > 5) { - log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer[5] ); + //log.Info("getHB packet received: " + buffer.Length + " btr " + BaseStream.BytesToRead + " type " + buffer[5] ); if (buffer[5] == MAVLINK_MSG_ID_HEARTBEAT) { return buffer; @@ -340,19 +345,19 @@ namespace ArdupilotMega public void sendPacket(object indata) { - bool run = false; + bool validPacket = false; byte a = 0; foreach (Type ty in mavstructs) { if (ty == indata.GetType()) { - run = true; + validPacket = true; generatePacket(a, indata); return; } a++; } - if (!run) + if (!validPacket) { log.Info("Mavlink : NOT VALID PACKET sendPacket() " + indata.GetType().ToString()); } @@ -486,7 +491,7 @@ namespace ArdupilotMega return false; } - MainV2.givecomport = true; + MainV2.giveComport = true; __mavlink_param_set_t req = new __mavlink_param_set_t(); req.target_system = sysid; @@ -522,7 +527,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - setParam " + paramname); } @@ -552,7 +557,7 @@ namespace ArdupilotMega param[st] = (par.param_value); - MainV2.givecomport = false; + MainV2.giveComport = false; //System.Threading.Thread.Sleep(100);//(int)(8.5 * 5)); // 8.5ms per byte return true; } @@ -597,13 +602,13 @@ namespace ArdupilotMega /// private Hashtable getParamListBG() { - MainV2.givecomport = true; + MainV2.giveComport = true; List got = new List(); // clear old param = new Hashtable(); - int retrys = 3; + int retrys = 6; int param_count = 0; int param_total = 5; @@ -618,18 +623,19 @@ namespace ArdupilotMega DateTime start = DateTime.Now; DateTime restart = DateTime.Now; - while (got.Count < param_total) + do { if (frmProgressReporter.doWorkArgs.CancelRequested) { frmProgressReporter.doWorkArgs.CancelAcknowledged = true; - MainV2.givecomport = false; + MainV2.giveComport = false; frmProgressReporter.doWorkArgs.ErrorMessage = "User Canceled"; return param; } - if (!(start.AddMilliseconds(5000) > DateTime.Now)) + // 4 seconds between valid packets + if (!(start.AddMilliseconds(4000) > DateTime.Now)) { if (retrys > 0) { @@ -639,8 +645,8 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; - throw new Exception("Timeout on read - getParamList " + param_count +" "+ param_total); + MainV2.giveComport = false; + throw new Exception("Timeout on read - getParamList " + got.Count + " " + param_total + "\n\nYour serial link isn't fast enough\n"); } byte[] buffer = readPacket(); @@ -656,9 +662,9 @@ namespace ArdupilotMega __mavlink_param_value_t par = buffer.ByteArrayToStructure<__mavlink_param_value_t>(6); // set new target - param_total = (par.param_count); + param_total = (par.param_count - 1); + - string paramID = System.Text.ASCIIEncoding.ASCII.GetString(par.param_id); int pos = paramID.IndexOf('\0'); @@ -670,20 +676,25 @@ namespace ArdupilotMega // check if we already have it if (got.Contains(par.param_index)) { - //Console.WriteLine("Already got '"+paramID+"'"); + log.Info("Already got "+(par.param_index) + " '" + paramID + "'"); + this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Already Got param " + paramID); continue; } - log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (param_total - 1) + " name: " + paramID); + log.Info(DateTime.Now.Millisecond + " got param " + (par.param_index) + " of " + (par.param_count - 2) + " name: " + paramID); modifyParamForDisplay(true, paramID, ref par.param_value); param[paramID] = (par.param_value); param_count++; got.Add(par.param_index); -// if (Progress != null) -// Progress((param.Count * 100) / param_total, "Got param " + paramID); + // if (Progress != null) + // Progress((param.Count * 100) / param_total, "Got param " + paramID); this.frmProgressReporter.UpdateProgressAndStatus((got.Count * 100) / param_total, "Got param " + paramID); + + // we have them all - lets escape eq total = 176 index = 0-175 + if (par.param_index == (param_total - 1)) + break; } else { @@ -692,7 +703,7 @@ namespace ArdupilotMega //stopwatch.Stop(); //Console.WriteLine("Time elapsed: {0}", stopwatch.Elapsed); } - } + } while (got.Count < param_total); if (got.Count != param_total) { @@ -704,7 +715,7 @@ namespace ArdupilotMega } throw new Exception("Missing Params"); } - MainV2.givecomport = false; + MainV2.giveComport = false; return param; } @@ -907,7 +918,7 @@ namespace ArdupilotMega } } #else - MainV2.givecomport = true; + MainV2.giveComport = true; byte[] buffer; __mavlink_waypoint_set_current_t req = new __mavlink_waypoint_set_current_t(); @@ -933,7 +944,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - setWPCurrent"); } @@ -942,7 +953,7 @@ namespace ArdupilotMega { if (buffer[5] == MAVLINK_MSG_ID_WAYPOINT_CURRENT) { - MainV2.givecomport = false; + MainV2.giveComport = false; return true; } } @@ -951,7 +962,7 @@ namespace ArdupilotMega public bool doAction(MAV_ACTION actionid) { - MainV2.givecomport = true; + MainV2.giveComport = true; byte[] buffer; __mavlink_action_t req = new __mavlink_action_t(); @@ -991,7 +1002,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - doAction"); } @@ -1002,12 +1013,12 @@ namespace ArdupilotMega { if (buffer[7] == 1) { - MainV2.givecomport = false; + MainV2.giveComport = false; return true; } else { - MainV2.givecomport = false; + MainV2.giveComport = false; return false; } } @@ -1054,9 +1065,18 @@ namespace ArdupilotMega } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_EXTRA3: - + if (packetspersecondbuild[MAVLINK_MSG_ID_DCM] < DateTime.Now.AddSeconds(-2)) + break; + pps = packetspersecond[MAVLINK_MSG_ID_DCM]; + if (hzratecheck(pps, hzrate)) + { + return; + } break; case (byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_POSITION: + // ac2 does not send rate position + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + return; if (packetspersecondbuild[MAVLINK_MSG_ID_GLOBAL_POSITION_INT] < DateTime.Now.AddSeconds(-2)) break; pps = packetspersecond[MAVLINK_MSG_ID_GLOBAL_POSITION_INT]; @@ -1145,7 +1165,6 @@ namespace ArdupilotMega req.req_stream_id = id; // id generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); - generatePacket(MAVLINK_MSG_ID_REQUEST_DATA_STREAM, req); } /// @@ -1154,7 +1173,7 @@ namespace ArdupilotMega /// public byte getWPCount() { - MainV2.givecomport = true; + MainV2.giveComport = true; byte[] buffer; #if MAVLINK10 __mavlink_mission_request_list_t req = new __mavlink_mission_request_list_t(); @@ -1225,13 +1244,13 @@ namespace ArdupilotMega { if (retrys > 0) { - log.Info("getWPCount Retry " + retrys + " - giv com " + MainV2.givecomport); + log.Info("getWPCount Retry " + retrys + " - giv com " + MainV2.giveComport); generatePacket(MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, req); start = DateTime.Now; retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; //return (byte)int.Parse(param["WP_TOTAL"].ToString()); throw new Exception("Timeout on read - getWPCount"); } @@ -1243,7 +1262,7 @@ namespace ArdupilotMega { log.Info("wpcount: " + buffer[9]); - MainV2.givecomport = false; + MainV2.giveComport = false; return buffer[9]; // should be ushort, but apm has limited wp count < byte } else @@ -1262,7 +1281,7 @@ namespace ArdupilotMega /// WP public Locationwp getWP(ushort index) { - MainV2.givecomport = true; + MainV2.giveComport = true; Locationwp loc = new Locationwp(); #if MAVLINK10 __mavlink_mission_request_t req = new __mavlink_mission_request_t(); @@ -1339,7 +1358,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - getWP"); } //Console.WriteLine("getwp read " + DateTime.Now.Millisecond); @@ -1438,7 +1457,7 @@ namespace ArdupilotMega } } } - MainV2.givecomport = false; + MainV2.giveComport = false; return loc; } @@ -1597,7 +1616,7 @@ namespace ArdupilotMega } } #else - MainV2.givecomport = true; + MainV2.giveComport = true; __mavlink_waypoint_count_t req = new __mavlink_waypoint_count_t(); req.target_system = sysid; @@ -1622,7 +1641,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - setWPTotal"); } byte[] buffer = readPacket(); @@ -1638,7 +1657,7 @@ namespace ArdupilotMega param["WP_TOTAL"] = (float)wp_total - 1; if (param["CMD_TOTAL"] != null) param["CMD_TOTAL"] = (float)wp_total - 1; - MainV2.givecomport = false; + MainV2.giveComport = false; return; } } @@ -1661,7 +1680,7 @@ namespace ArdupilotMega /// 0 = no , 2 = guided mode public void setWP(Locationwp loc, ushort index, MAV_FRAME frame, byte current) { - MainV2.givecomport = true; + MainV2.giveComport = true; #if MAVLINK10 __mavlink_mission_item_t req = new __mavlink_mission_item_t(); #else @@ -1767,7 +1786,7 @@ namespace ArdupilotMega retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - setWP"); } byte[] buffer = readPacket(); @@ -1820,7 +1839,7 @@ namespace ArdupilotMega if (ans.seq == (index + 1)) { log.Info("set wp doing " + index + " req " + ans.seq + " REQ 40 : " + buffer[5]); - MainV2.givecomport = false; + MainV2.giveComport = false; break; } else @@ -2079,7 +2098,7 @@ namespace ArdupilotMega if (bpstime.Second != DateTime.Now.Second && !logreadmode) { - // Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false)); + Console.Write("bps {0} loss {1} left {2} mem {3} \n", bps1, synclost, BaseStream.BytesToRead, System.GC.GetTotalMemory(false) / 1024 / 1024.0); bps2 = bps1; // prev sec bps1 = 0; // current sec bpstime = DateTime.Now; @@ -2185,7 +2204,7 @@ namespace ArdupilotMega logdata = logdata.Substring(0, ind); log.Info(DateTime.Now + " " + logdata); - if (MainV2.talk != null && MainV2.config["speechenable"] != null && MainV2.config["speechenable"].ToString() == "True") + if (MainV2.speechEngine != null && MainV2.config["speechenable"] != null && MainV2.config["speechenable"].ToString() == "True") { //MainV2.talk.SpeakAsync(logdata); } @@ -2257,7 +2276,7 @@ namespace ArdupilotMega { byte[] buffer; - MainV2.givecomport = true; + MainV2.giveComport = true; PointLatLngAlt plla = new PointLatLngAlt(); __mavlink_fence_fetch_point_t req = new __mavlink_fence_fetch_point_t(); @@ -2278,13 +2297,13 @@ namespace ArdupilotMega { if (retrys > 0) { - log.Info("getFencePoint Retry " + retrys + " - giv com " + MainV2.givecomport); + log.Info("getFencePoint Retry " + retrys + " - giv com " + MainV2.giveComport); generatePacket(MAVLINK_MSG_ID_FENCE_FETCH_POINT, req); start = DateTime.Now; retrys--; continue; } - MainV2.givecomport = false; + MainV2.giveComport = false; throw new Exception("Timeout on read - getFencePoint"); } @@ -2293,7 +2312,7 @@ namespace ArdupilotMega { if (buffer[5] == MAVLINK_MSG_ID_FENCE_POINT) { - MainV2.givecomport = false; + MainV2.giveComport = false; __mavlink_fence_point_t fp = buffer.ByteArrayToStructure<__mavlink_fence_point_t>(6); diff --git a/Tools/ArdupilotMegaPlanner/MagCalib.cs b/Tools/ArdupilotMegaPlanner/MagCalib.cs index ca36613cfd..e205c7b7ba 100644 --- a/Tools/ArdupilotMegaPlanner/MagCalib.cs +++ b/Tools/ArdupilotMegaPlanner/MagCalib.cs @@ -38,10 +38,13 @@ namespace ArdupilotMega if (openFileDialog1.ShowDialog() == DialogResult.OK) { + try + { + double[] ans = getOffsets(openFileDialog1.FileName); - double[] ans = getOffsets(openFileDialog1.FileName); - - SaveOffsets(ans); + SaveOffsets(ans); + } + catch (Exception ex) { log.Debug(ex.ToString()); } } } @@ -153,6 +156,12 @@ namespace ArdupilotMega mine.logplaybackfile.Close(); mine.logplaybackfile = null; + if (data.Count < 10) + { + CustomMessageBox.Show("Log does not contain enough data"); + throw new Exception("Not Enough Data"); + } + double[] x = LeastSq(data); System.Console.WriteLine("Old Method {0} {1} {2}", -(maxx + minx) / 2, -(maxy + miny) / 2, -(maxz + minz) / 2); diff --git a/Tools/ArdupilotMegaPlanner/MainV2.cs b/Tools/ArdupilotMegaPlanner/MainV2.cs index 6e7f4a9643..320eaf9ec2 100644 --- a/Tools/ArdupilotMegaPlanner/MainV2.cs +++ b/Tools/ArdupilotMegaPlanner/MainV2.cs @@ -39,14 +39,14 @@ namespace ArdupilotMega const int SW_HIDE = 0; public static MAVLink comPort = new MAVLink(); - public static string comportname = ""; + public static string comPortName = ""; public static Hashtable config = new Hashtable(); - public static bool givecomport = false; + public static bool giveComport = false; public static Firmwares APMFirmware = Firmwares.ArduPlane; public static bool MONO = false; - public static bool speechenable = false; - public static Speech talk = null; + public static bool speechEnable = false; + public static Speech speechEngine = null; public static Joystick joystick = null; DateTime lastjoystick = DateTime.Now; @@ -55,22 +55,14 @@ namespace ArdupilotMega public static CurrentState cs = new CurrentState(); - bool serialthread = false; + bool serialThread = false; TcpListener listener; - DateTime heatbeatsend = DateTime.Now; + DateTime heatbeatSend = DateTime.Now; - public static List threads = new List(); public static MainV2 instance = null; - /* - * "PITCH_KP", -"PITCH_KI", -"PITCH_LIM", - - */ - public enum Firmwares { ArduPlane, @@ -106,9 +98,7 @@ namespace ArdupilotMega var t = Type.GetType("Mono.Runtime"); MONO = (t != null); - talk = new Speech(); - - //talk.SpeakAsync("Welcome to APM Planner"); + speechEngine = new Speech(); MyRenderer.currentpressed = MenuFlightData; @@ -187,12 +177,14 @@ namespace ArdupilotMega this.Location = startpos; } + if (config["MainMaximised"] != null) + this.WindowState = (FormWindowState)Enum.Parse(typeof(FormWindowState), config["MainMaximised"].ToString()); + if (config["MainHeight"] != null) this.Height = int.Parse(config["MainHeight"].ToString()); if (config["MainWidth"] != null) this.Width = int.Parse(config["MainWidth"].ToString()); - if (config["MainMaximised"] != null) - this.WindowState = (FormWindowState)Enum.Parse(typeof(FormWindowState), config["MainMaximised"].ToString()); + if (config["CMB_rateattitude"] != null) MainV2.cs.rateattitude = byte.Parse(config["CMB_rateattitude"].ToString()); @@ -202,10 +194,12 @@ namespace ArdupilotMega MainV2.cs.ratestatus = byte.Parse(config["CMB_ratestatus"].ToString()); if (config["CMB_rateattitude"] != null) MainV2.cs.raterc = byte.Parse(config["CMB_raterc"].ToString()); + if (config["CMB_ratesensors"] != null) + MainV2.cs.raterc = byte.Parse(config["CMB_ratesensors"].ToString()); if (config["speechenable"] != null) - MainV2.speechenable = bool.Parse(config["speechenable"].ToString()); - + MainV2.speechEnable = bool.Parse(config["speechenable"].ToString()); + //int fixme; /* MainV2.cs.rateattitude = 50; @@ -277,7 +271,7 @@ namespace ArdupilotMega } string[] ports = SerialPort.GetPortNames() - .Select(p=>p.TrimEnd()) + .Select(p => p.TrimEnd()) .Select(FixBlueToothPortNameBug) .ToArray(); @@ -289,26 +283,26 @@ namespace ArdupilotMega return allPorts; } - // .NET bug: sometimes bluetooth ports are enumerated with bogus characters - // eg 'COM10' becomes 'COM10c' - one workaround is to remove the non numeric - // char. Annoyingly, sometimes a numeric char is added, which means this - // does not work in all cases. - // See http://connect.microsoft.com/VisualStudio/feedback/details/236183/system-io-ports-serialport-getportnames-error-with-bluetooth - private string FixBlueToothPortNameBug(string portName) - { - if (!portName.StartsWith("COM")) - return portName; - var newPortName = "COM"; // Start over with "COM" - foreach (var portChar in portName.Substring(3).ToCharArray()) // Remove "COM", put the rest in a character array - { - if (char.IsDigit(portChar)) - newPortName += portChar.ToString(); // Good character, append to portName - else - log.WarnFormat("Bad (Non Numeric) character in port name '{0}' - removing", portName); - } - - return newPortName; - } + // .NET bug: sometimes bluetooth ports are enumerated with bogus characters + // eg 'COM10' becomes 'COM10c' - one workaround is to remove the non numeric + // char. Annoyingly, sometimes a numeric char is added, which means this + // does not work in all cases. + // See http://connect.microsoft.com/VisualStudio/feedback/details/236183/system-io-ports-serialport-getportnames-error-with-bluetooth + private string FixBlueToothPortNameBug(string portName) + { + if (!portName.StartsWith("COM")) + return portName; + var newPortName = "COM"; // Start over with "COM" + foreach (var portChar in portName.Substring(3).ToCharArray()) // Remove "COM", put the rest in a character array + { + if (char.IsDigit(portChar)) + newPortName += portChar.ToString(); // Good character, append to portName + else + log.WarnFormat("Bad (Non Numeric) character in port name '{0}' - removing", portName); + } + + return newPortName; + } internal void ScreenShot() { @@ -457,7 +451,7 @@ namespace ArdupilotMega MenuConnect_Click(sender, e); } - givecomport = true; + giveComport = true; MyView.Controls.Clear(); @@ -489,7 +483,7 @@ namespace ArdupilotMega private void MenuConnect_Click(object sender, EventArgs e) { - givecomport = false; + giveComport = false; if (comPort.BaseStream.IsOpen && cs.groundspeed > 4) { @@ -505,8 +499,8 @@ namespace ArdupilotMega { try { - if (talk != null) // cancel all pending speech - talk.SpeakAsyncCancelAll(); + if (speechEngine != null) // cancel all pending speech + speechEngine.SpeakAsyncCancelAll(); } catch { } @@ -544,11 +538,7 @@ namespace ArdupilotMega comPort.BaseStream.StopBits = (StopBits)Enum.Parse(typeof(StopBits), "1"); comPort.BaseStream.Parity = (Parity)Enum.Parse(typeof(Parity), "None"); - if (config["CHK_resetapmonconnect"] == null || bool.Parse(config["CHK_resetapmonconnect"].ToString()) == true) - comPort.BaseStream.toggleDTR(); - comPort.BaseStream.DtrEnable = false; - comPort.BaseStream.RtsEnable = false; try { @@ -557,7 +547,7 @@ namespace ArdupilotMega try { Directory.CreateDirectory(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs"); - comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".tlog", FileMode.CreateNew,FileAccess.ReadWrite,FileShare.Read)); + comPort.logfile = new BinaryWriter(File.Open(Path.GetDirectoryName(Application.ExecutablePath) + Path.DirectorySeparatorChar + @"logs" + Path.DirectorySeparatorChar + DateTime.Now.ToString("yyyy-MM-dd hh-mm-ss") + ".tlog", FileMode.CreateNew, FileAccess.ReadWrite, FileShare.Read)); } catch { CustomMessageBox.Show("Failed to create log - wont log this session"); } // soft fail @@ -639,13 +629,13 @@ namespace ArdupilotMega private void CMB_serialport_SelectedIndexChanged(object sender, EventArgs e) { - comportname = CMB_serialport.Text; - if (comportname == "UDP" || comportname == "TCP") + comPortName = CMB_serialport.Text; + if (comPortName == "UDP" || comPortName == "TCP") { CMB_baudrate.Enabled = false; - if (comportname == "TCP") + if (comPortName == "TCP") MainV2.comPort.BaseStream = new TcpSerial(); - if (comportname == "UDP") + if (comPortName == "UDP") MainV2.comPort.BaseStream = new UdpSerial(); } else @@ -679,7 +669,7 @@ namespace ArdupilotMega GCSViews.FlightData.threadrun = 0; GCSViews.Simulation.threadrun = 0; - serialthread = false; + serialThread = false; try { @@ -722,7 +712,7 @@ namespace ArdupilotMega xmlwriter.WriteStartElement("Config"); - xmlwriter.WriteElementString("comport", comportname); + xmlwriter.WriteElementString("comport", comPortName); xmlwriter.WriteElementString("baudrate", CMB_baudrate.Text); @@ -776,7 +766,7 @@ namespace ArdupilotMega CMB_serialport.Text = temp; // allows ports that dont exist - yet } comPort.BaseStream.PortName = temp; - comportname = temp; + comPortName = temp; break; case "baudrate": string temp2 = xmlreader.ReadString(); @@ -873,64 +863,68 @@ namespace ArdupilotMega } } + private void updateConnectIcon() + { + DateTime menuupdate = DateTime.Now; + if ((DateTime.Now - menuupdate).Milliseconds > 500) + { + // Console.WriteLine(DateTime.Now.Millisecond); + if (comPort.BaseStream.IsOpen) + { + if ((string)this.MenuConnect.BackgroundImage.Tag != "Disconnect") + { + this.Invoke((MethodInvoker)delegate + { + this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect; + this.MenuConnect.BackgroundImage.Tag = "Disconnect"; + CMB_baudrate.Enabled = false; + CMB_serialport.Enabled = false; + }); + } + } + else + { + if ((string)this.MenuConnect.BackgroundImage.Tag != "Connect") + { + this.Invoke((MethodInvoker)delegate + { + this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect; + this.MenuConnect.BackgroundImage.Tag = "Connect"; + CMB_baudrate.Enabled = true; + CMB_serialport.Enabled = true; + }); + } + } + menuupdate = DateTime.Now; + } + } private void SerialReader() { - if (serialthread == true) + if (serialThread == true) return; - serialthread = true; + serialThread = true; int minbytes = 10; if (MONO) minbytes = 0; - DateTime menuupdate = DateTime.Now; - DateTime speechcustomtime = DateTime.Now; DateTime linkqualitytime = DateTime.Now; - while (serialthread) + while (serialThread) { try { System.Threading.Thread.Sleep(5); - if ((DateTime.Now - menuupdate).Milliseconds > 500) - { - // Console.WriteLine(DateTime.Now.Millisecond); - if (comPort.BaseStream.IsOpen) - { - if ((string)this.MenuConnect.BackgroundImage.Tag != "Disconnect") - { - this.Invoke((MethodInvoker)delegate - { - this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.disconnect; - this.MenuConnect.BackgroundImage.Tag = "Disconnect"; - CMB_baudrate.Enabled = false; - CMB_serialport.Enabled = false; - }); - } - } - else - { - if ((string)this.MenuConnect.BackgroundImage.Tag != "Connect") - { - this.Invoke((MethodInvoker)delegate - { - this.MenuConnect.BackgroundImage = global::ArdupilotMega.Properties.Resources.connect; - this.MenuConnect.BackgroundImage.Tag = "Connect"; - CMB_baudrate.Enabled = true; - CMB_serialport.Enabled = true; - }); - } - } - menuupdate = DateTime.Now; - } + + updateConnectIcon(); - if (speechenable && talk != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) + if (speechEnable && speechEngine != null && (DateTime.Now - speechcustomtime).TotalSeconds > 30 && MainV2.cs.lat != 0 && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { //speechbatteryvolt float warnvolt = 0; @@ -938,12 +932,12 @@ namespace ArdupilotMega if (MainV2.getConfig("speechbatteryenabled") == "True" && MainV2.cs.battery_voltage <= warnvolt) { - MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery"))); + MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechbattery"))); } if (MainV2.getConfig("speechcustomenabled") == "True") { - MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom"))); + MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechcustom"))); } speechcustomtime = DateTime.Now; @@ -967,7 +961,7 @@ namespace ArdupilotMega GC.Collect(); } - if (speechenable && talk != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) + if (speechEnable && speechEngine != null && (MainV2.comPort.logreadmode || comPort.BaseStream.IsOpen)) { float warnalt = float.MaxValue; float.TryParse(MainV2.getConfig("speechaltheight"), out warnalt); @@ -975,20 +969,20 @@ namespace ArdupilotMega { if (MainV2.getConfig("speechaltenabled") == "True" && (MainV2.cs.alt - (int)double.Parse(MainV2.getConfig("TXT_homealt"))) <= warnalt) { - if (MainV2.talk.State == SynthesizerState.Ready) - MainV2.talk.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt"))); + if (MainV2.speechEngine.State == SynthesizerState.Ready) + MainV2.speechEngine.SpeakAsync(Common.speechConversion(MainV2.getConfig("speechalt"))); } } catch { } // silent fail } - if (!comPort.BaseStream.IsOpen || givecomport == true) + if (!comPort.BaseStream.IsOpen || giveComport == true) { System.Threading.Thread.Sleep(100); continue; } - if (heatbeatsend.Second != DateTime.Now.Second) + if (heatbeatSend.Second != DateTime.Now.Second) { // Console.WriteLine("remote lost {0}", cs.packetdropremote); @@ -1005,22 +999,22 @@ namespace ArdupilotMega #endif comPort.sendPacket(htb); - heatbeatsend = DateTime.Now; + heatbeatSend = DateTime.Now; } // data loss warning if ((DateTime.Now - comPort.lastvalidpacket).TotalSeconds > 10) { - if (speechenable && talk != null) + if (speechEnable && speechEngine != null) { - if (MainV2.talk.State == SynthesizerState.Ready) - MainV2.talk.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); + if (MainV2.speechEngine.State == SynthesizerState.Ready) + MainV2.speechEngine.SpeakAsync("WARNING No Data for " + (int)(DateTime.Now - comPort.lastvalidpacket).TotalSeconds + " Seconds"); } } - //Console.WriteLine(comPort.BaseStream.BytesToRead); + //Console.WriteLine(DateTime.Now.Millisecond + " " + comPort.BaseStream.BytesToRead); - while (comPort.BaseStream.BytesToRead > minbytes && givecomport == false) + while (comPort.BaseStream.BytesToRead > minbytes && giveComport == false) comPort.readPacket(); } catch (Exception e) @@ -1082,12 +1076,7 @@ namespace ArdupilotMega private void MainV2_Load(object sender, EventArgs e) { - // generate requestall for jani and sandro - comPort.sysid = 7; - comPort.compid = 1; - //comPort.requestDatastream((byte)MAVLink.MAV_DATA_STREAM.MAV_DATA_STREAM_ALL,3); - // - + // init button depressed MenuFlightData_Click(sender, e); // for long running tasks using own threads. @@ -1096,13 +1085,11 @@ namespace ArdupilotMega try { listener = new TcpListener(IPAddress.Any, 56781); - var t13 = new Thread(listernforclients) + new Thread(listernforclients) { - Name = "motion jpg stream", + Name = "motion jpg stream-network kml", IsBackground = true - }; - // wait for tcp connections - t13.Start(); + }.Start(); } catch (Exception ex) { @@ -1110,20 +1097,18 @@ namespace ArdupilotMega CustomMessageBox.Show(ex.ToString()); } - var t12 = new Thread(new ThreadStart(joysticksend)) + new Thread(new ThreadStart(joysticksend)) { IsBackground = true, Priority = ThreadPriority.AboveNormal, Name = "Main joystick sender" - }; - t12.Start(); + }.Start(); - var t11 = new Thread(SerialReader) + new Thread(SerialReader) { IsBackground = true, Name = "Main Serial reader" - }; - t11.Start(); + }.Start(); if (Debugger.IsAttached) { @@ -1466,12 +1451,12 @@ namespace ArdupilotMega } - public static void updatecheck(ProgressReporterDialogue frmProgressReporter) + public static void updateCheckMain(ProgressReporterDialogue frmProgressReporter) { var baseurl = ConfigurationManager.AppSettings["UpdateLocation"]; try { - bool update = updatecheck(frmProgressReporter, baseurl, ""); + bool update = updateCheck(frmProgressReporter, baseurl, ""); var process = new Process(); string exePath = Path.GetDirectoryName(Application.ExecutablePath); if (MONO) @@ -1503,7 +1488,8 @@ namespace ArdupilotMega log.Info("Quitting existing process"); try { - MainV2.instance.BeginInvoke((MethodInvoker)delegate() { + MainV2.instance.BeginInvoke((MethodInvoker)delegate() + { Application.Exit(); }); } @@ -1614,20 +1600,20 @@ namespace ArdupilotMega ThemeManager.ApplyThemeTo(frmProgressReporter); - frmProgressReporter.DoWork += new Controls.ProgressReporterDialogue.DoWorkEventHandler(frmProgressReporter_DoWork); + frmProgressReporter.DoWork += new Controls.ProgressReporterDialogue.DoWorkEventHandler(DoUpdateWorker_DoWork); frmProgressReporter.UpdateProgressAndStatus(-1, "Checking for Updates"); frmProgressReporter.RunBackgroundOperationAsync(); } - static void frmProgressReporter_DoWork(object sender, Controls.ProgressWorkerEventArgs e) + static void DoUpdateWorker_DoWork(object sender, Controls.ProgressWorkerEventArgs e) { ((ProgressReporterDialogue)sender).UpdateProgressAndStatus(-1, "Getting Base URL"); - MainV2.updatecheck((ProgressReporterDialogue)sender); + MainV2.updateCheckMain((ProgressReporterDialogue)sender); } - private static bool updatecheck(ProgressReporterDialogue frmProgressReporter, string baseurl, string subdir) + private static bool updateCheck(ProgressReporterDialogue frmProgressReporter, string baseurl, string subdir) { bool update = false; List files = new List(); @@ -1688,7 +1674,7 @@ namespace ArdupilotMega } if (file.EndsWith("/")) { - update = updatecheck(frmProgressReporter, baseurl + file, subdir.Replace('/', Path.DirectorySeparatorChar) + file) && update; + update = updateCheck(frmProgressReporter, baseurl + file, subdir.Replace('/', Path.DirectorySeparatorChar) + file) && update; continue; } if (frmProgressReporter != null) @@ -1786,7 +1772,7 @@ namespace ArdupilotMega ((HttpWebRequest)request).AutomaticDecompression = DecompressionMethods.GZip | DecompressionMethods.Deflate; - request.Headers.Add("Accept-Encoding", "gzip,deflate"); + request.Headers.Add("Accept-Encoding", "gzip,deflate"); // Get the response. response = request.GetResponse(); diff --git a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs index 4b01b971ff..36e972eff2 100644 --- a/Tools/ArdupilotMegaPlanner/MavlinkLog.cs +++ b/Tools/ArdupilotMegaPlanner/MavlinkLog.cs @@ -366,7 +366,11 @@ namespace ArdupilotMega { MAVLink mine = new MAVLink(); - mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + try + { + mine.logplaybackfile = new BinaryReader(File.Open(logfile, FileMode.Open, FileAccess.Read, FileShare.Read)); + } + catch (Exception ex) { log.Debug(ex.ToString()); CustomMessageBox.Show("Log Can not be opened. Are you still connected?"); return; } mine.logreadmode = true; mine.packets.Initialize(); // clear @@ -399,8 +403,8 @@ namespace ArdupilotMega try { - if (MainV2.talk != null) - MainV2.talk.SpeakAsyncCancelAll(); + if (MainV2.speechEngine != null) + MainV2.speechEngine.SpeakAsyncCancelAll(); } catch { } // ignore because of this Exception System.PlatformNotSupportedException: No voice installed on the system or none available with the current security setting. diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 58eeae5426..556a38e668 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.1.51")] +[assembly: AssemblyFileVersion("1.1.54")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.cs b/Tools/ArdupilotMegaPlanner/SerialInput.cs index 4955b4ba3a..5a1d5542e3 100644 --- a/Tools/ArdupilotMegaPlanner/SerialInput.cs +++ b/Tools/ArdupilotMegaPlanner/SerialInput.cs @@ -153,19 +153,19 @@ namespace ArdupilotMega } catch { } - if (MainV2.comPort.BaseStream.IsOpen && MainV2.givecomport == false) + if (MainV2.comPort.BaseStream.IsOpen && MainV2.giveComport == false) { try { - MainV2.givecomport = true; + MainV2.giveComport = true; MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2); GCSViews.FlightData.GuidedModeWP = new PointLatLngAlt(gotohere); - MainV2.givecomport = false; + MainV2.giveComport = false; } - catch { MainV2.givecomport = false; } + catch { MainV2.giveComport = false; } } } } diff --git a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs index 3537a9c809..f10943bfd3 100644 --- a/Tools/ArdupilotMegaPlanner/Setup/Setup.cs +++ b/Tools/ArdupilotMegaPlanner/Setup/Setup.cs @@ -510,7 +510,7 @@ namespace ArdupilotMega.Setup if (tabControl1.SelectedTab == tabHeli) { - if (MainV2.comPort.param["GYR_ENABLE_"] == null) + if (MainV2.comPort.param["GYR_ENABLE"] == null) { tabHeli.Enabled = false; return; @@ -518,6 +518,12 @@ namespace ArdupilotMega.Setup startup = true; try { + if (MainV2.comPort.param.ContainsKey("H1_ENABLE")) + { + CCPM.Checked = MainV2.comPort.param["H1_ENABLE"].ToString() == "0" ? true : false; + H1_ENABLE.Checked = !CCPM.Checked; + } + foreach (string value in MainV2.comPort.param.Keys) { if (value == "") @@ -915,7 +921,7 @@ namespace ArdupilotMega.Setup } catch { CustomMessageBox.Show("Set SYSID_SW_MREV Failed"); return; } - MainV2.givecomport = true; + MainV2.giveComport = true; ICommsSerial comPortT = MainV2.comPort.BaseStream; @@ -931,7 +937,7 @@ namespace ArdupilotMega.Setup comPortT.DtrEnable = true; comPortT.Open(); } - catch (Exception ex) { MainV2.givecomport = false; CustomMessageBox.Show("Invalid Comport Settings : " + ex.Message); return; } + catch (Exception ex) { MainV2.giveComport = false; CustomMessageBox.Show("Invalid Comport Settings : " + ex.Message); return; } BUT_reset.Text = "Rebooting (17 sec)"; BUT_reset.Refresh(); @@ -951,7 +957,7 @@ namespace ArdupilotMega.Setup comPortT.Close(); - MainV2.givecomport = false; + MainV2.giveComport = false; try { MainV2.comPort.Open(true); @@ -1087,9 +1093,9 @@ namespace ArdupilotMega.Setup try { - MainV2.comPort.setParam("COL_MID_", MainV2.cs.ch3in); + MainV2.comPort.setParam("COL_MID", MainV2.cs.ch3in); - COL_MID.Text = MainV2.comPort.param["COL_MID_"].ToString(); + COL_MID.Text = MainV2.comPort.param["COL_MID"].ToString(); } catch { CustomMessageBox.Show("Set COL_MID_ failed"); } } @@ -1290,8 +1296,8 @@ namespace ArdupilotMega.Setup { if (MainV2.comPort.param["HSV_MAN"].ToString() == "1") { - MainV2.comPort.setParam("COL_MIN_", int.Parse(COL_MIN.Text)); - MainV2.comPort.setParam("COL_MAX_", int.Parse(COL_MAX.Text)); + MainV2.comPort.setParam("COL_MIN", int.Parse(COL_MIN.Text)); + MainV2.comPort.setParam("COL_MAX", int.Parse(COL_MAX.Text)); MainV2.comPort.setParam("HSV_MAN", 0); // randy request - last BUT_swash_manual.Text = "Manual"; diff --git a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb index 4df910db41..1026981e5b 100644 Binary files a/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb and b/Tools/ArdupilotMegaPlanner/bin/Release/ArdupilotMegaPlanner.pdb differ diff --git a/Tools/ArdupilotMegaPlanner/temp.cs b/Tools/ArdupilotMegaPlanner/temp.cs index a7cf78d898..a6a6bbbaae 100644 --- a/Tools/ArdupilotMegaPlanner/temp.cs +++ b/Tools/ArdupilotMegaPlanner/temp.cs @@ -151,7 +151,7 @@ namespace ArdupilotMega port.Parity = Parity.None; port.DtrEnable = true; - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; try { port.Open(); @@ -222,7 +222,7 @@ namespace ArdupilotMega port.Parity = Parity.None; port.DtrEnable = true; - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; try { port.Open(); @@ -285,7 +285,7 @@ namespace ArdupilotMega port.Parity = Parity.None; port.DtrEnable = true; - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; try { port.Open(); @@ -392,7 +392,7 @@ namespace ArdupilotMega try { - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; port.Open(); @@ -536,7 +536,7 @@ namespace ArdupilotMega try { - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; log.Info("Open Port"); port.Open(); @@ -587,7 +587,7 @@ namespace ArdupilotMega try { - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; log.Info("Open Port"); port.Open(); @@ -651,7 +651,7 @@ namespace ArdupilotMega try { - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; log.Info("Open Port"); port.Open(); @@ -728,7 +728,7 @@ namespace ArdupilotMega try { - port.PortName = ArdupilotMega.MainV2.comportname; + port.PortName = ArdupilotMega.MainV2.comPortName; port.Open(); diff --git a/Tools/VARTest/VARTest.pde b/Tools/VARTest/VARTest.pde index fb05d54114..9c4b5de900 100644 --- a/Tools/VARTest/VARTest.pde +++ b/Tools/VARTest/VARTest.pde @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include // Inertial Sensor (uncalibated IMU) Library #include // ArduPilot Mega IMU Library @@ -26,6 +28,10 @@ #include #include +#if 0 +#include +#endif + static Parameters g; static AP_ADC_ADS7844 adc; diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index e6755ffdc1..50a0ac6d88 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -172,30 +172,6 @@ void AP_Baro_BMP085::ReadPress() } RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); - - //if (_offset_press == 0){ - // _offset_press = RawPress; - // RawPress = 0; - //} else{ - // RawPress -= _offset_press; - //} - - // filter - //_press_filter[_press_index++] = RawPress; - - //if(_press_index >= PRESS_FILTER_SIZE) - // _press_index = 0; - - //RawPress = 0; - - // sum our filter - //for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){ - // RawPress += _press_filter[i]; - //} - - // grab result - //RawPress /= PRESS_FILTER_SIZE; - //RawPress += _offset_press; } // Send Command to Read Temperature @@ -225,25 +201,21 @@ void AP_Baro_BMP085::ReadTemp() _temp_sensor = buf[0]; _temp_sensor = (_temp_sensor << 8) | buf[1]; - if (RawTemp == 0){ - RawTemp = _temp_sensor; - }else{ - RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99; - } + RawTemp = _temp_filter.apply(_temp_sensor); } // Calculate Temperature and Pressure in real units. void AP_Baro_BMP085::Calculate() { - long x1, x2, x3, b3, b5, b6, p; - unsigned long b4, b7; + int32_t x1, x2, x3, b3, b5, b6, p; + uint32_t b4, b7; int32_t tmp; // See Datasheet page 13 for this formulas // Based also on Jee Labs BMP085 example code. Thanks for share. // Temperature calculations - x1 = ((long)RawTemp - ac6) * ac5 >> 15; - x2 = ((long) mc << 11) / (x1 + md); + x1 = ((int32_t)RawTemp - ac6) * ac5 >> 15; + x2 = ((int32_t) mc << 11) / (x1 + md); b5 = x1 + x2; Temp = (b5 + 8) >> 4; diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index b7b329edd9..a07833e7fb 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -5,14 +5,13 @@ #define PRESS_FILTER_SIZE 2 #include "AP_Baro.h" +#include class AP_Baro_BMP085 : public AP_Baro { public: AP_Baro_BMP085(bool apm2_hardware): - _apm2_hardware(apm2_hardware), - _temp_index(0), - _press_index(0){}; // Constructor + _apm2_hardware(apm2_hardware) {}; // Constructor /* AP_Baro public interface: */ @@ -27,7 +26,6 @@ class AP_Baro_BMP085 : public AP_Baro private: int32_t RawPress; - int32_t _offset_press; int32_t RawTemp; int16_t Temp; uint32_t Press; @@ -40,10 +38,8 @@ class AP_Baro_BMP085 : public AP_Baro int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; uint16_t ac4, ac5, ac6; - int32_t _press_filter[PRESS_FILTER_SIZE]; + AverageFilterInt16_Size4 _temp_filter; - uint8_t _temp_index; - uint8_t _press_index; uint32_t _retry_time; void Command_ReadPress(); diff --git a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde index 7bd8b4b18a..3cc61c9058 100644 --- a/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde @@ -12,6 +12,7 @@ #include #include #include +#include #ifndef APM2_HARDWARE # define APM2_HARDWARE 0 diff --git a/libraries/Desktop/support/main.cpp b/libraries/Desktop/support/main.cpp index 6207bdd38b..57dc7d618d 100644 --- a/libraries/Desktop/support/main.cpp +++ b/libraries/Desktop/support/main.cpp @@ -83,6 +83,12 @@ int main(int argc, char * const argv[]) struct timeval tv; fd_set fds; int fd_high = 0; + +#ifdef __CYGWIN__ + // under windows if this loop is using alot of cpu, + // the alarm gets called at a slower rate. + sleep(5); +#endif FD_ZERO(&fds); loop(); diff --git a/libraries/Desktop/support/sitl.cpp b/libraries/Desktop/support/sitl.cpp index bea1f18fd2..7e0976620a 100644 --- a/libraries/Desktop/support/sitl.cpp +++ b/libraries/Desktop/support/sitl.cpp @@ -231,6 +231,17 @@ static void timer_handler(int signum) if (kill(parent_pid, 0) != 0) { exit(1); } +#else + + static uint16_t count = 0; + static uint32_t last_report; + + count++; + if (millis() - last_report > 1000) { + printf("TH %u cps\n", count); + count = 0; + last_report = millis(); + } #endif /* check for packet from flight sim */