diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 58f240169a..ad6f19c6e4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.2 b4" +#define THISFIRMWARE "ArduCopter V2.2 b6" /* ArduCopter Version 2.2 Authors: Jason Short @@ -69,7 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // TimerProcess is the scheduler for MPU6000 reads. #include // ArduPilot Mega DCM Library #include // PI library -#include // PID library +#include // PID library #include // RC Channel Library #include // Range finder library #include // Optical Flow library @@ -312,11 +312,6 @@ static const char* flight_mode_strings[] = { //////////////////////////////////////////////////////////////////////////////// // The GPS based velocity calculated by offsetting the Latitude and Longitude // updated after GPS read - 5-10hz -static int16_t x_GPS_speed; -static int16_t y_GPS_speed; - -// The synthesized velocity calculated by fancy filtering and fusion -// updated at 50hz static int16_t x_actual_speed; static int16_t y_actual_speed; @@ -325,9 +320,6 @@ static int16_t y_actual_speed; static int16_t x_rate_error; static int16_t y_rate_error; -//static int16_t my_max_speed; // used for debugging logs -//static int16_t target_x_rate; - //////////////////////////////////////////////////////////////////////////////// // Radio //////////////////////////////////////////////////////////////////////////////// @@ -469,6 +461,8 @@ static uint8_t wp_verify_byte; // used for tracking state of navigating wa static int16_t waypoint_speed_gov; // Used to track how many cm we are from the "next_WP" location static int32_t long_error, lat_error; +// Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s +static boolean loiter_override; //////////////////////////////////////////////////////////////////////////////// @@ -557,19 +551,16 @@ static int32_t altitude_error; static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt; -// The previous altitude as reported by Sonar in cm for calculation of Climb Rate -static int16_t old_sonar_alt; // The climb_rate as reported by sonar in cm/s static int16_t sonar_rate; // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt; -// The previous altitude as reported by Baro in cm for calculation of Climb Rate -static int32_t old_baro_alt; // The climb_rate as reported by Baro in cm/s static int16_t baro_rate; // static boolean reset_throttle_flag; + //////////////////////////////////////////////////////////////////////////////// // flight modes //////////////////////////////////////////////////////////////////////////////// @@ -601,6 +592,10 @@ static int16_t manual_boost; static int16_t angle_boost; // Push copter down for clean landing static int16_t landing_boost; +// for controlling the landing throttle curve +//verifies landings +static int16_t ground_detector; + //////////////////////////////////////////////////////////////////////////////// // Navigation general @@ -826,7 +821,7 @@ void loop() uint32_t timer = micros(); // We want this to execute fast // ---------------------------- - if ((timer - fast_loopTimer) >= 5000) { + if ((timer - fast_loopTimer) >= 4500) { //PORTK |= B00010000; G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops fast_loopTimer = timer; @@ -852,7 +847,7 @@ void loop() // update our velocity estimate based on IMU at 50hz // ------------------------------------------------- - estimate_velocity(); + //estimate_velocity(); // check for new GPS messages // -------------------------- @@ -903,6 +898,12 @@ static void fast_loop() // IMU DCM Algorithm read_AHRS(); + if(takeoff_complete == false){ + // reset these I terms to prevent awkward tipping on takeoff + reset_rate_I(); + reset_stability_I(); + } + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1518,11 +1519,6 @@ void update_throttle_mode(void) if ((millis() - takeoff_timer) > 5000){ // we must be in the air by now takeoff_complete = true; - land_complete = false; - }else{ - // reset these I terms to prevent awkward tipping on takeoff - reset_rate_I(); - reset_stability_I(); } }else{ // we are on the ground @@ -1535,11 +1531,6 @@ void update_throttle_mode(void) // reset_baro(); } - // reset out i terms and takeoff timer - // ----------------------------------- - reset_rate_I(); - reset_stability_I(); - // remember our time since takeoff // ------------------------------- takeoff_timer = millis(); @@ -1596,12 +1587,8 @@ void update_throttle_mode(void) // how far off are we altitude_error = get_altitude_error(); - // get the AP throttle, if landing boost > 0 we are actually Landing - // This allows us to grab just the compensation. - if(landing_boost > 0) - nav_throttle = get_nav_throttle(-200); - else - nav_throttle = get_nav_throttle(altitude_error); + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error); // clear the new data flag invalid_throttle = false; @@ -1615,6 +1602,11 @@ 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 FRAME_CONFIG == HELI_FRAME throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost); #else @@ -1662,24 +1654,18 @@ static void update_navigation() case RTL: // We have reached Home if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - // if this value > 0, we are set to trigger auto_land after 30 seconds + // if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds set_mode(LOITER); auto_land_timer = millis(); break; } - // We wait until we've reached out new altitude before coming home - // Arg doesn't work, it - //if(alt_change_flag != REACHED_ALT){ - // wp_control = NO_NAV_MODE; - //}else{ - wp_control = WP_MODE; + wp_control = WP_MODE; - // calculates desired Yaw - #if FRAME_CONFIG == HELI_FRAME - update_auto_yaw(); - #endif - //} + // calculates desired Yaw + #if FRAME_CONFIG == HELI_FRAME + update_auto_yaw(); + #endif // calculates the desired Roll and Pitch update_nav_wp(); @@ -1690,16 +1676,27 @@ static void update_navigation() case POSITION: // This feature allows us to reposition the quad when the user lets // 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; + wp_control = NO_NAV_MODE; // reset LOITER to current position - next_WP.lat = current_loc.lat; - next_WP.lng = current_loc.lng; + next_WP.lat = current_loc.lat; + next_WP.lng = current_loc.lng; + if((g.rc_2.control_in + g.rc_1.control_in) == 0){ + loiter_override = false; + wp_control = LOITER_MODE; + } }else{ - // this is also set by GPS in update_nav wp_control = LOITER_MODE; } @@ -1715,7 +1712,10 @@ static void update_navigation() break; case LAND: - verify_land(); + if(g.sonar_enabled) + verify_land_sonar(); + else + verify_land_baro(); // calculates the desired Roll and Pitch update_nav_wp(); @@ -1798,6 +1798,10 @@ static void update_trig(void){ // updated at 10hz static void update_altitude() { + static int16_t old_sonar_alt = 0; + static int32_t old_baro_alt = 0; + static int16_t old_climb_rate = 0; + #if HIL_MODE == HIL_MODE_ATTITUDE // we are in the SIM, fake out the baro and Sonar int fake_relative_alt = g_gps->altitude - gps_base_alt; @@ -1818,7 +1822,7 @@ static void update_altitude() baro_rate = (temp + baro_rate) >> 1; old_baro_alt = baro_alt; - // sonar_alt is calculated in a faster loop and filtered with a mode filter + // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter #endif @@ -1868,9 +1872,15 @@ static void update_altitude() climb_rate = baro_rate; } + // simple smoothing + climb_rate = (climb_rate + old_climb_rate)>>1; + // manage bad data climb_rate = constrain(climb_rate, -300, 300); + // save for filtering + old_climb_rate = climb_rate; + // update the target altitude next_WP.alt = get_new_altitude(); } @@ -1883,6 +1893,7 @@ adjust_altitude() manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); update_throttle_cruise(); + }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; @@ -1898,14 +1909,18 @@ static void tuning(){ switch(g.radio_tuning){ case CH6_DAMP: - g.rc_6.set_range(0,80); // 0 to 1 + g.rc_6.set_range(0,300); // 0 to 1 g.stablize_d.set(tuning_value); + + //g.rc_6.set_range(0,60); // 0 to 1 + //g.pid_rate_roll.kD(tuning_value); + //g.pid_rate_pitch.kD(tuning_value); break; case CH6_STABILIZE_KP: g.rc_6.set_range(0,8000); // 0 to 8 - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_STABILIZE_KI: @@ -1917,16 +1932,14 @@ static void tuning(){ case CH6_RATE_KP: g.rc_6.set_range(40,300); // 0 to .3 - g.pi_rate_roll.kP(tuning_value); - g.pi_rate_pitch.kP(tuning_value); - g.pi_acro_roll.kP(tuning_value); - g.pi_acro_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_RATE_KI: g.rc_6.set_range(0,300); // 0 to .3 - g.pi_rate_roll.kI(tuning_value); - g.pi_rate_pitch.kI(tuning_value); + g.pid_rate_roll.kI(tuning_value); + g.pid_rate_pitch.kI(tuning_value); break; case CH6_YAW_KP: @@ -1936,12 +1949,12 @@ static void tuning(){ case CH6_YAW_RATE_KP: g.rc_6.set_range(0,1000); - g.pi_rate_yaw.kP(tuning_value); + g.pid_rate_yaw.kP(tuning_value); break; case CH6_THROTTLE_KP: g.rc_6.set_range(0,1000); // 0 to 1 - g.pi_throttle.kP(tuning_value); + g.pid_throttle.kP(tuning_value); break; case CH6_TOP_BOTTOM_RATIO: @@ -1961,15 +1974,15 @@ static void tuning(){ break; case CH6_LOITER_P: - g.rc_6.set_range(0,1000); + g.rc_6.set_range(0,2000); g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break; case CH6_NAV_P: g.rc_6.set_range(0,6000); - g.pi_nav_lat.kP(tuning_value); - g.pi_nav_lon.kP(tuning_value); + g.pid_nav_lat.kP(tuning_value); + g.pid_nav_lon.kP(tuning_value); break; #if FRAME_CONFIG == HELI_FRAME @@ -1986,20 +1999,20 @@ static void tuning(){ case CH6_OPTFLOW_KP: g.rc_6.set_range(0,5000); // 0 to 5 - g.pi_optflow_roll.kP(tuning_value); - g.pi_optflow_pitch.kP(tuning_value); + g.pid_optflow_roll.kP(tuning_value); + g.pid_optflow_pitch.kP(tuning_value); break; case CH6_OPTFLOW_KI: g.rc_6.set_range(0,10000); // 0 to 10 - g.pi_optflow_roll.kI(tuning_value); - g.pi_optflow_pitch.kI(tuning_value); + g.pid_optflow_roll.kI(tuning_value); + g.pid_optflow_pitch.kI(tuning_value); break; case CH6_OPTFLOW_KD: g.rc_6.set_range(0,200); // 0 to 0.2 - g.pi_optflow_roll.kD(tuning_value); - g.pi_optflow_pitch.kD(tuning_value); + g.pid_optflow_roll.kD(tuning_value); + g.pid_optflow_pitch.kD(tuning_value); break; } } @@ -2077,8 +2090,9 @@ static void update_nav_wp() }else if(wp_control == NO_NAV_MODE){ // clear out our nav so we can do things like land straight down + // or change Loiter position - // We bring in our iterms for wind control, but we don't navigate + // We bring copy over our Iterms for wind control, but we don't navigate nav_lon = g.pi_loiter_lon.get_integrator(); nav_lat = g.pi_loiter_lat.get_integrator(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0851d90333..d7c1c75c70 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -3,134 +3,131 @@ static int get_stabilize_roll(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.roll_sensor); + target_angle = wrap_180(target_angle - dcm.roll_sensor); #if FRAME_CONFIG == HELI_FRAME + // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_roll.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_roll.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else + // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_roll.get_p(error); + int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); + int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt); - - // rate control - error = rate - (omega.x * DEGX100); - rate = g.pi_rate_roll.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_roll(target_rate) + iterm; #endif } static int get_stabilize_pitch(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.pitch_sensor); + target_angle = wrap_180(target_angle - dcm.pitch_sensor); #if FRAME_CONFIG == HELI_FRAME // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else - // angle error - error = constrain(error, -2500, 2500); + // limit the error we're feeding to the PID + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_pitch.get_p(error); + int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle); + int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt); - - // rate control - error = rate - (omega.y * DEGX100); - - //error = rate - (omega.y * DEGX100); - rate = g.pi_rate_pitch.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_pitch(target_rate) + iterm; #endif } - -#define YAW_ERROR_MAX 2000 static int get_stabilize_yaw(int32_t target_angle) { - int32_t error; - int32_t rate; - // angle error - error = wrap_180(target_angle - dcm.yaw_sensor); + target_angle = wrap_180(target_angle - dcm.yaw_sensor); // limit the error we're feeding to the PID - error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX); + target_angle = constrain(target_angle, -2000, 2000); - // convert to desired Rate: - rate = g.pi_stabilize_yaw.get_p(error); - - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt); + // conver to desired Rate: + int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle); + int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters - if( !g.heli_ext_gyro_enabled ) { - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + if(!g.heli_ext_gyro_enabled){ + return get_rate_yaw(target_rate) + iterm; + }else{ + return constrain((target_rate + iterm), -4500, 4500); } - // output control: - rate = constrain(rate, -4500, 4500); #else - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + return get_rate_yaw(target_rate) + iterm; +#endif +} + +static int +get_rate_roll(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.x * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; // output control: - int16_t yaw_input = 1400 + abs(g.rc_4.control_in); - // smoother Yaw control: - rate = constrain(rate, -yaw_input, yaw_input); -#endif + return constrain(target_rate, -2500, 2500); +} - return (int)rate + iterm; +static int +get_rate_pitch(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.y * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; + + // output control: + return constrain(target_rate, -2500, 2500); +} + +static int +get_rate_yaw(int32_t target_rate) +{ + // rate control + target_rate = target_rate - (omega.z * DEGX100); + target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt); + + // output control: + int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); + + // smoother Yaw control: + return constrain(target_rate, -yaw_limit, yaw_limit); } #define ALT_ERROR_MAX 400 @@ -138,10 +135,8 @@ static int16_t get_nav_throttle(int32_t z_error) { static int16_t old_output = 0; - //static int16_t rate_d = 0; - - int16_t rate_error; - int16_t output; + int16_t rate_error = 0; + int16_t output = 0; // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -155,17 +150,10 @@ get_nav_throttle(int32_t z_error) // calculate rate error rate_error = rate_error - climb_rate; - // limit the rate - iterm is not used - output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180); - - // a positive climb rate means we're going up - //rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain - - // slight adjustment to alt hold output - //output -= constrain(rate_d, -25, 25); + // limit the rate + output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180); // light filter of output - //output = (old_output * 3 + output) / 4; output = (old_output + output) / 2; // save our output @@ -175,33 +163,6 @@ get_nav_throttle(int32_t z_error) return output + iterm; } -static int -get_rate_roll(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.x * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_roll.get_pi(error, G_Dt); -} - -static int -get_rate_pitch(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.y * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_pitch.get_pi(error, G_Dt); -} - -static int -get_rate_yaw(int32_t target_rate) -{ - - int32_t error = (target_rate * 4.5) - (omega.z * DEGX100); - target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); -} - // Keeps old data out of our calculation / logs static void reset_nav_params(void) { @@ -247,17 +208,15 @@ static void reset_I_all(void) static void reset_rate_I() { - g.pi_rate_roll.reset_I(); - g.pi_rate_pitch.reset_I(); - g.pi_acro_roll.reset_I(); - g.pi_acro_pitch.reset_I(); - g.pi_rate_yaw.reset_I(); + g.pid_rate_roll.reset_I(); + g.pid_rate_pitch.reset_I(); + g.pid_rate_yaw.reset_I(); } static void reset_optflow_I(void) { - g.pi_optflow_roll.reset_I(); - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_roll.reset_I(); + g.pid_optflow_pitch.reset_I(); of_roll = 0; of_pitch = 0; } @@ -272,15 +231,15 @@ static void reset_wind_I(void) static void reset_nav_I(void) { // Rate control for WP navigation - g.pi_nav_lat.reset_I(); - g.pi_nav_lon.reset_I(); + g.pid_nav_lat.reset_I(); + g.pid_nav_lon.reset_I(); } static void reset_throttle_I(void) { // For Altitude Hold g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); + g.pid_throttle.reset_I(); } static void reset_stability_I(void) @@ -325,7 +284,7 @@ static int get_angle_boost(int value) float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); int16_t output = temp * value; - return constrain(output, 0, 100); + return constrain(output, 0, 200); // return (int)(temp * value); } @@ -482,9 +441,9 @@ get_of_roll(int32_t control_roll) // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ - g.pi_optflow_roll.reset_I(); + g.pid_optflow_roll.reset_I(); tot_x_cm = 0; } // limit amount of change and maximum angle @@ -516,10 +475,10 @@ get_of_pitch(int32_t control_pitch) // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ tot_y_cm = 0; - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_pitch.reset_I(); } // limit amount of change diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index 047a7061ba..f0f3df9f48 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -4,8 +4,9 @@ static void init_camera() { - APM_RC.enable_out(CH_CAM_PITCH); - APM_RC.enable_out(CH_CAM_ROLL); + APM_RC.enable_out(CH_CAM_PITCH); + APM_RC.enable_out(CH_CAM_ROLL); + // ch 6 high(right) is down. g.rc_camera_pitch.set_angle(4500); g.rc_camera_roll.set_angle(4500); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 44e74f21bf..355038a9c2 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -824,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); + set_mode(LAND); break; case MAV_ACTION_HALT: @@ -1529,6 +1529,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) imu.set_gyro(gyros); */ //imu.set_accel(accels); + + // rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM + Vector3f gyros; + gyros.x = (float)packet.rollspeed / 1000.0; + gyros.y = (float)packet.pitchspeed / 1000.0; + gyros.z = (float)packet.yawspeed / 1000.0; + + imu.set_gyro(gyros); + + // m/s/s + Vector3f accels; + accels.x = (float)packet.roll * gravity / 1000.0; + accels.y = (float)packet.pitch * gravity / 1000.0; + accels.z = (float)packet.yaw * gravity / 1000.0; + + imu.set_accel(accels); + break; } #endif diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 090e2b3501..9b1511c07e 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -590,7 +590,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt(climb_rate); // 10 DataFlash.WriteInt(g.rc_3.servo_out); // 11 DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12 - DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13 + DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13 DataFlash.WriteByte(END_BYTE); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4c061a6026..9d356f9877 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 114; + static const uint16_t k_format_version = 115; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -26,7 +26,6 @@ public: // static const uint16_t k_software_type = 10; // 0 for APM trunk - // // Parameter identities. // // The enumeration defined here is used to ensure that every parameter @@ -168,23 +167,20 @@ public: // 235: PI/D Controllers // k_param_stabilize_d = 234, - k_param_pi_rate_roll = 235, - k_param_pi_rate_pitch, - k_param_pi_rate_yaw, + k_param_pid_rate_roll = 235, + k_param_pid_rate_pitch, + k_param_pid_rate_yaw, k_param_pi_stabilize_roll, k_param_pi_stabilize_pitch, k_param_pi_stabilize_yaw, k_param_pi_loiter_lat, k_param_pi_loiter_lon, - k_param_pi_nav_lat, - k_param_pi_nav_lon, + k_param_pid_nav_lat, + k_param_pid_nav_lon, k_param_pi_alt_hold, - k_param_pi_throttle, - k_param_pi_acro_roll, - k_param_pi_acro_pitch, - k_param_pi_optflow_roll, - k_param_pi_optflow_pitch, // 250 - k_param_loiter_d, + k_param_pid_throttle, + k_param_pid_optflow_roll, + k_param_pid_optflow_pitch, // 250 // 254,255: reserved }; @@ -286,31 +282,24 @@ public: AP_Float camera_pitch_gain; AP_Float camera_roll_gain; AP_Float stablize_d; - AP_Float loiter_d; // PI/D controllers - APM_PI pi_rate_roll; - APM_PI pi_rate_pitch; - APM_PI pi_rate_yaw; + AC_PID pid_rate_roll; + AC_PID pid_rate_pitch; + AC_PID pid_rate_yaw; + AC_PID pid_nav_lat; + AC_PID pid_nav_lon; - APM_PI pi_stabilize_roll; - APM_PI pi_stabilize_pitch; - APM_PI pi_stabilize_yaw; + AC_PID pid_throttle; + AC_PID pid_optflow_roll; + AC_PID pid_optflow_pitch; APM_PI pi_loiter_lat; APM_PI pi_loiter_lon; - - APM_PI pi_nav_lat; - APM_PI pi_nav_lon; - + APM_PI pi_stabilize_roll; + APM_PI pi_stabilize_pitch; + APM_PI pi_stabilize_yaw; APM_PI pi_alt_hold; - APM_PI pi_throttle; - - APM_PI pi_acro_roll; - APM_PI pi_acro_pitch; - - PID pi_optflow_roll; - PID pi_optflow_pitch; uint8_t junk; @@ -369,7 +358,7 @@ public: radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), - ch7_option (CH7_SAVE_WP, k_param_ch7_option, PSTR("CH7_OPT")), + ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")), #if FRAME_CONFIG == HELI_FRAME heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")), @@ -409,34 +398,32 @@ public: //------------------------------------------------------------------------------------------------------------------- camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")), camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), - stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")), - loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")), + + // PID controller group key name initial P initial I initial D initial imax + //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- + pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100), + pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100), + pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100), + + pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + + pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), + pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), + pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100), + // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- - pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100), - pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100), - pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100), - pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100), pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100), pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100), + pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX), pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), - pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100), - pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100), - - pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX), - pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), - - pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), - pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), - - pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), - pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100), junk(0) // XXX just so that we can add things without worrying about the trailing comma { diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index d8bd2bbd36..8a09f52a71 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -16,6 +16,7 @@ static void process_nav_command() break; case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint + yaw_mode = YAW_HOLD; do_land(); break; @@ -121,8 +122,6 @@ static void process_now_command() static bool verify_must() { - //Serial.printf("vmust: %d\n", command_nav_ID); - switch(command_nav_queue.id) { case MAV_CMD_NAV_TAKEOFF: @@ -130,7 +129,11 @@ static bool verify_must() break; case MAV_CMD_NAV_LAND: - return verify_land(); + if(g.sonar_enabled == true){ + return verify_land_sonar(); + }else{ + return verify_land_baro(); + } break; case MAV_CMD_NAV_WAYPOINT: @@ -265,13 +268,21 @@ static void do_land() { wp_control = LOITER_MODE; + // just to make sure land_complete = false; + // landing boost lowers the main throttle to mimmick + // the effect of a user's hand + landing_boost = 0; + + // A counter that goes up if our climb rate stalls out. + ground_detector = 0; + // hold at our current location set_next_WP(¤t_loc); - // Set a new target altitude - set_new_altitude(-200); + // Set a new target altitude very low, incase we are landing on a hill! + set_new_altitude(-1000); } static void do_loiter_unlimited() @@ -341,46 +352,71 @@ static bool verify_takeoff() } // called at 10hz -static bool verify_land() +static bool verify_land_sonar() { - static int16_t velocity_land = -1; - static float icount = 0; + static float icount = 1; - // landing detector - if (current_loc.alt > 300){ - velocity_land = 2000; + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; icount = 1; - + ground_detector = 0; }else{ - // a LP filter used to tell if we have landed - // will drive to 0 if we are on the ground - maybe, the baro is noisy - velocity_land = ((velocity_land * 7) + climb_rate ) / 8; + // begin to pull down on the throttle + landing_boost++; } - if ((current_loc.alt - home.alt) < 200){ - // don't bank to hold position - wp_control = NO_NAV_MODE; - // try and come down faster - //landing_boost++; - //landing_boost = min(landing_boost, 15); - float tmp = (1.75 * icount * icount) - (7.2 * icount); - landing_boost = tmp; - landing_boost = constrain(landing_boost, 1, 200); - icount += 0.4; - - //Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount); - - }else{ - landing_boost = 0; - wp_control = LOITER_MODE; + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; } - //if(/*(current_loc.alt < 100) && */ (velocity_land < 20)){ - if((landing_boost > 150) || (velocity_land < 20)){ - icount = 1; - land_complete = true; - landing_boost = 0; - return true; + if(current_loc.alt < 150 ){ + //rapid throttle reduction + int16_t lb = (1.75 * icount * icount) - (7.2 * icount); + icount++; + lb = constrain(lb, 0, 180); + landing_boost += lb; + //Serial.printf("%0.0f, %d, %d, %d\n", icount, current_loc.alt, landing_boost, lb); + + if(current_loc.alt < 40 || abs(climb_rate) < 20) { + if(ground_detector++ > 20) { + land_complete = true; + ground_detector = 0; + icount = 1; + // init disarm motors + init_disarm_motors(); + return true; + } + } + } + return false; +} + +static bool verify_land_baro() +{ + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; + ground_detector = 0; + }else{ + // begin to pull down on the throttle + landing_boost++; + landing_boost = min(landing_boost, 40); + } + + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; + } + + if(current_loc.alt < 150 ){ + if(abs(climb_rate) < 20) { + landing_boost++; + if(ground_detector++ > 30) { + land_complete = true; + ground_detector = 0; + // init disarm motors + init_disarm_motors(); + return true; + } + } } return false; } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 929c5c35b6..4c3b94eda7 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -62,14 +62,7 @@ static void update_commands() g.command_index = command_nav_index = 255; // if we are on the ground, enter stabilize, else Land if (land_complete == true){ - // So what state does this leave us in? - // We are still in the same mode as what landed us, - // so maybe we try to continue to descend just in case we are still in the air - // This will also drive down the Iterm to -300 - - // We can't disarm the motors easily. We could very well be wrong - // - //init_disarm_motors(); + // we will disarm the motors after landing. } else { set_mode(LAND); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4d07062990..cbe6cab5e3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -525,114 +525,98 @@ #ifdef MOTORS_JD880 # define STABILIZE_ROLL_P 3.6 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 3.6 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifdef MOTORS_JD850 # define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifndef STABILIZE_D -# define STABILIZE_D .03 +# define STABILIZE_D .12 #endif // Jasons default values that are good for smaller payload motors. #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.6 +# define STABILIZE_ROLL_P 4.5 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.1 +# define STABILIZE_ROLL_I 0.0 #endif #ifndef STABILIZE_ROLL_IMAX # define STABILIZE_ROLL_IMAX 40 // degrees #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.6 +# define STABILIZE_PITCH_P 4.5 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.1 +# define STABILIZE_PITCH_I 0.0 #endif #ifndef STABILIZE_PITCH_IMAX # define STABILIZE_PITCH_IMAX 40 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// Acro Rate Control -// -#ifndef ACRO_ROLL_P -# define ACRO_ROLL_P 0.155 +#ifndef STABILIZE_YAW_P +# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif -#ifndef ACRO_ROLL_I -# define ACRO_ROLL_I 0.0 +#ifndef STABILIZE_YAW_I +# define STABILIZE_YAW_I 0.01 #endif -#ifndef ACRO_ROLL_IMAX -# define ACRO_ROLL_IMAX 15 // degrees +#ifndef STABILIZE_YAW_IMAX +# define STABILIZE_YAW_IMAX 8 // degrees * 100 #endif -#ifndef ACRO_PITCH_P -# define ACRO_PITCH_P 0.155 -#endif -#ifndef ACRO_PITCH_I -# define ACRO_PITCH_I 0 //0.18 -#endif -#ifndef ACRO_PITCH_IMAX -# define ACRO_PITCH_IMAX 15 // degrees -#endif ////////////////////////////////////////////////////////////////////////////// // Stabilize Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.155 +# define RATE_ROLL_P 0.14 #endif #ifndef RATE_ROLL_I -# define RATE_ROLL_I 0.0 +# define RATE_ROLL_I 0.0 +#endif +#ifndef RATE_ROLL_D +# define RATE_ROLL_D 0 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 15 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.155 +# define RATE_PITCH_P 0.14 #endif #ifndef RATE_PITCH_I -# define RATE_PITCH_I 0 //0.18 +# define RATE_PITCH_I 0 // 0.18 +#endif +#ifndef RATE_PITCH_D +# define RATE_PITCH_D 0 // 0.002 #endif #ifndef RATE_PITCH_IMAX # define RATE_PITCH_IMAX 15 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// YAW Control -// -#ifndef STABILIZE_YAW_P -# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy -#endif -#ifndef STABILIZE_YAW_I -# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance -#endif -#ifndef STABILIZE_YAW_IMAX -# define STABILIZE_YAW_IMAX 8 // degrees * 100 -#endif - #ifndef RATE_YAW_P -# define RATE_YAW_P .13 // used to control response in turning +# define RATE_YAW_P .13 #endif #ifndef RATE_YAW_I -# define RATE_YAW_I 0.0 +# define RATE_YAW_I 0.0 +#endif +#ifndef RATE_YAW_D +# define RATE_YAW_D .002 #endif #ifndef RATE_YAW_IMAX -# define RATE_YAW_IMAX 50 +# define RATE_YAW_IMAX 50 // degrees #endif @@ -640,37 +624,37 @@ // Loiter control gains // #ifndef LOITER_P -# define LOITER_P 1.5 // was .25 in previous +# define LOITER_P .4 // was .25 in previous #endif #ifndef LOITER_I -# define LOITER_I 0.09 // Wind control +# define LOITER_I 0.2 // Wind control #endif #ifndef LOITER_IMAX -# define LOITER_IMAX 30 // degrees° -#endif -#ifndef LOITER_D -# define LOITER_D 2.2 // rate control +# define LOITER_IMAX 30 // degrees #endif ////////////////////////////////////////////////////////////////////////////// // WP Navigation control gains // #ifndef NAV_P -# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter +# define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter #endif #ifndef NAV_I # define NAV_I 0 // #endif +#ifndef NAV_D +# define NAV_D 0.015 // +#endif #ifndef NAV_IMAX # define NAV_IMAX 30 // degrees #endif #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph #endif #ifndef WAYPOINT_SPEED_MIN -# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MIN 100 // 1m/s #endif @@ -678,28 +662,31 @@ // Throttle control gains // #ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 350 // +# define THROTTLE_CRUISE 350 // #endif -#ifndef THR_HOLD_P -# define THR_HOLD_P 0.4 // +#ifndef ALT_HOLD_P +# define ALT_HOLD_P 0.4 // #endif -#ifndef THR_HOLD_I -# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup +#ifndef ALT_HOLD_I +# define ALT_HOLD_I 0.02 #endif -#ifndef THR_HOLD_IMAX -# define THR_HOLD_IMAX 300 +#ifndef ALT_HOLD_IMAX +# define ALT_HOLD_IMAX 300 #endif // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.5 // +# define THROTTLE_P 0.35 // #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.0 // +# define THROTTLE_I 0.0 // +#endif +#ifndef THROTTLE_D +# define THROTTLE_D 0.02 // #endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 300 +# define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 7beab99fbe..87965160eb 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -114,6 +114,7 @@ static void read_trim_switch() // reset the mission CH7_wp_index = 0; g.command_total.set_and_save(1); + set_mode(RTL); return; } @@ -154,7 +155,6 @@ static void read_trim_switch() // 1 = takeoff // 2 = WP 2 // 3 = command total - } } } diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 810d57e605..e2900234e3 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -7,6 +7,7 @@ static bool heli_swash_initialised = false; static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000) +static float heli_coll_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) // heli_servo_averaging: // 0 or 1 = no averaging, 250hz @@ -20,27 +21,12 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1 static void heli_reset_swash() { // free up servo ranges - if( g.heli_servo_1.get_reverse() ) { - g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); - }else{ - g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); - } - if( g.heli_servo_2.get_reverse() ) { - g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); - }else{ - g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); - } - if( g.heli_servo_3.get_reverse() ) { - g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); - }else{ - g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); - } + g.heli_servo_1.radio_min = 1000; + g.heli_servo_1.radio_max = 2000; + g.heli_servo_2.radio_min = 1000; + g.heli_servo_2.radio_max = 2000; + g.heli_servo_3.radio_min = 1000; + g.heli_servo_3.radio_max = 2000; // pitch factors heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); @@ -52,6 +38,9 @@ static void heli_reset_swash() heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); + // set throttle scaling + heli_coll_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0; + // we must be in set-up mode so mark swash as uninitialised heli_swash_initialised = false; } @@ -60,7 +49,6 @@ static void heli_reset_swash() static void heli_init_swash() { int i; - float coll_range_comp = 1; // factor to negate collective range's effect on roll & pitch range // swash servo initialisation g.heli_servo_1.set_range(0,1000); @@ -75,45 +63,29 @@ static void heli_init_swash() } g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); - // calculate compensation for collective range on roll & pitch range - if( g.heli_coll_max - g.heli_coll_min > 100 ) - coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min); - // calculate throttle mid point - heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min)); + heli_throttle_mid = ((float)(g.heli_coll_mid-g.heli_coll_min))/((float)(g.heli_coll_max-g.heli_coll_min))*1000.0; + + // determine scalar throttle input + heli_coll_scalar = ((float)(g.heli_coll_max-g.heli_coll_min))/1000.0; // pitch factors - heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp; - heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp; - heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp; + heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); + heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)); + heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)); // roll factors - heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp; - heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)) * coll_range_comp; - heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp; + heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)); + heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); + heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); // servo min/max values - if( g.heli_servo_1.get_reverse() ) { - g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); - }else{ - g.heli_servo_1.radio_min = g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); - } - if( g.heli_servo_2.get_reverse() ) { - g.heli_servo_2.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); - }else{ - g.heli_servo_2.radio_min = g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); - } - if( g.heli_servo_3.get_reverse() ) { - g.heli_servo_3.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); - }else{ - g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); - } + g.heli_servo_1.radio_min = 1000; + g.heli_servo_1.radio_max = 2000; + g.heli_servo_2.radio_min = 1000; + g.heli_servo_2.radio_max = 2000; + g.heli_servo_3.radio_min = 1000; + g.heli_servo_3.radio_max = 2000; // reset the servo averaging for( i=0; i<=3; i++ ) @@ -148,13 +120,15 @@ static void heli_move_servos_to_mid() // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) { - int yaw_offset = 0; + int yaw_offset = 0; + int coll_out_scaled; if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? // check if we need to freeup the swash if( heli_swash_initialised ) { heli_reset_swash(); } + coll_out_scaled = coll_out * heli_coll_scalar + g.rc_3.radio_min - 1000; }else{ // regular flight mode // check if we need to reinitialise the swash @@ -166,19 +140,20 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); coll_out = constrain(coll_out, 0, 1000); + coll_out_scaled = coll_out * heli_coll_scalar + g.heli_coll_min - 1000; // 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_coll_yaw_effect * (coll_out - g.heli_coll_mid); + yaw_offset = g.heli_coll_yaw_effect * (coll_out_scaled - g.heli_coll_mid); } #endif } // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_3.radio_trim-1500); g.heli_servo_4.servo_out = yaw_out + yaw_offset; // use servo_out to calculate pwm_out and radio_out diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 69a2d8a4c6..97b05f746e 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -19,7 +19,11 @@ static void update_GPS_light(void) switch (g_gps->status()){ case(2): - digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. + if(home_is_set) { // JLN update + digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. + } else { + digitalWrite(C_LED_PIN, LED_OFF); + } break; case(1): diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7cba786cae..0744b52855 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // 10 = 1 second -#define ARM_DELAY 30 +#define ARM_DELAY 20 #define DISARM_DELAY 20 #define LEVEL_DELAY 100 diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a8795de1eb..a0eb0e00d7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -7,13 +7,11 @@ static byte navigate() { // waypoint distance from plane in meters // --------------------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); - home_distance = get_distance(¤t_loc, &home); + wp_distance = get_distance(¤t_loc, &next_WP); + home_distance = get_distance(¤t_loc, &home); if (wp_distance < 0){ - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); + // something went very wrong return 0; } @@ -61,9 +59,8 @@ static void calc_XY_velocity(){ int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; // slight averaging filter - x_GPS_speed = (x_GPS_speed + x_estimate) >> 1; - y_GPS_speed = (y_GPS_speed + y_estimate) >> 1; - //*/ + x_actual_speed = (x_actual_speed + x_estimate) >> 1; + y_actual_speed = (y_actual_speed + y_estimate) >> 1; /* // Ryan Beall's forward estimator: @@ -99,67 +96,7 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North } -/* -//static void calc_loiter3(int x_error, int y_error) -{ - static int32_t gps_lat_I = 0; - static int32_t gps_lon_I = 0; - - // If close to goal <1m reset the I term - if (abs(x_error) < 50) - gps_lon_I = 0; - if (abs(y_error) < 50) - gps_lat_I = 0; - - gps_lon_I += x_error; - gps_lat_I += y_error; - - gps_lon_I = constrain(gps_lon_I,-3000,3000); - gps_lat_I = constrain(gps_lat_I,-3000,3000); - - int16_t lon_P = 1.2 * (float)x_error; - int16_t lon_I = 0.1 * (float)gps_lon_I; //.1 - int16_t lon_D = 3 * x_GPS_speed ; // this controls the small bumps - - int16_t lat_P = 1.2 * (float)y_error; - int16_t lat_I = 0.1 * (float)gps_lat_I; - int16_t lat_D = 3 * y_GPS_speed ; - - //limit of terms - lon_I = constrain(lon_I,-3000,3000); - lat_I = constrain(lat_I,-3000,3000); - lon_D = constrain(lon_D,-500,500); //this controls the long distance dampimg - lat_D = constrain(lat_D,-500,500); //this controls the long distance dampimg - - nav_lon = lon_P + lon_I - lon_D; - nav_lat = lat_P + lat_I - lat_D; - - Serial.printf("%d, %d, %d, %d, %d, %d\n", - lon_P, lat_P, - lon_I, lat_I, - lon_D, lat_D); - -} -*/ - #define NAV_ERR_MAX 800 -static void calc_loiter1(int x_error, int y_error) -{ - int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); - int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps - - int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav); - int16_t lat_D = g.loiter_d * y_actual_speed ; - - //limit of terms - lon_D = constrain(lon_D,-500,500); - lat_D = constrain(lat_D,-500,500); - - nav_lon = constrain(lon_PI - lon_D, -2500, 2500); - nav_lat = constrain(lat_PI - lat_D, -2500, 2500); -} - - static void calc_loiter(int x_error, int y_error) { // East/West @@ -167,22 +104,20 @@ static void calc_loiter(int x_error, int y_error) int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); x_rate_error = x_target_speed - x_actual_speed; - nav_lon_p = x_rate_error * g.loiter_d; - - nav_lon_p = constrain(nav_lon_p, -1200, 1200); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -2500, 2500); // North/South y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); y_rate_error = y_target_speed - y_actual_speed; - nav_lat_p = y_rate_error * g.loiter_d; - nav_lat_p = constrain(nav_lat_p, -1200, 1200); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -2500, 2500); + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lat = nav_lat + y_iterm; + nav_lon = nav_lon + x_iterm; + + /* int8_t ttt = 1.0/dTnav; @@ -204,7 +139,7 @@ static void calc_loiter(int x_error, int y_error) //*/ /* - int16_t t1 = g.pi_nav_lon.get_integrator(); // X + int16_t t1 = g.pid_nav_lon.get_integrator(); // X Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n", wp_distance, //1 dTnav, //2 @@ -219,33 +154,91 @@ static void calc_loiter(int x_error, int y_error) //*/ } +static void calc_nav_rate(int max_speed) +{ + // push us towards the original track + update_crosstrack(); + + // nav_bearing includes crosstrack + float temp = (9000l - nav_bearing) * RADX100; + + x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 + x_rate_error = constrain(x_rate_error, -1000, 1000); + int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); + + y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); + + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lon = nav_lon + x_iterm; + nav_lat = nav_lat + y_iterm; + + /* + Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat, + x_iterm, + y_iterm, + crosstrack_error); + //*/ + + // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() + + /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat);*/ +} + + +static void calc_nav_lon(int rate) +{ + nav_lon = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lon = get_corrected_angle(rate, nav_lon); + nav_lon = constrain(nav_lon, -3000, 3000); +} + +static void calc_nav_lat(int rate) +{ + nav_lat = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lat = get_corrected_angle(rate, nav_lat); + nav_lat = constrain(nav_lat, -3000, 3000); +} + +static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out) +{ + int16_t tt = desired_rate; + // scale down the desired rate and square it + desired_rate = desired_rate / 20; + desired_rate = desired_rate * desired_rate; + int16_t tmp = 0; + + if (tt > 0){ + tmp = rate_out + (rate_out - desired_rate); + tmp = max(tmp, rate_out); + }else if (tt < 0){ + tmp = rate_out + (rate_out + desired_rate); + tmp = min(tmp, rate_out); + } + //Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp); + return tmp; +} + //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2 -#define ERR_GAIN .01 -// called at 50hz -static void estimate_velocity() -{ - // we need to extimate velocity when below GPS threshold of 1.5m/s - //if(g_gps->ground_speed < 120){ - // some smoothing to prevent bumpy rides - x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16; - y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16; - - // integration of nav_p angle - //x_actual_speed += (nav_lon_p >>2); - //y_actual_speed += (nav_lat_p >>2); - - // this is just what worked best in SIM - //x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4; - //y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4; - - //}else{ - // less smoothing needed since the GPS already filters - // x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4; - // y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4; - //} -} // this calculation rotates our World frame of reference to the copter's frame of reference // We use the DCM's matrix to precalculate these trig values at 50hz @@ -291,55 +284,6 @@ static int16_t calc_desired_speed(int16_t max_speed) return max_speed; } -static void calc_nav_rate(int max_speed) -{ - // push us towards the original track - update_crosstrack(); - - // nav_bearing includes crosstrack - float temp = (9000l - nav_bearing) * RADX100; - - x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 - x_rate_error = constrain(x_rate_error, -1000, 1000); - int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); - nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -3000, 3000); - - y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum - int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); - nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -3000, 3000); - - /* - Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat, - x_iterm, - y_iterm, - crosstrack_error); - //*/ - - - // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() - - /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat);*/ -} - static void update_crosstrack(void) { @@ -433,7 +377,7 @@ static int32_t get_new_altitude() } int32_t diff = abs(next_WP.alt - target_altitude); - int8_t _scale = 3; + int8_t _scale = 4; if (next_WP.alt < target_altitude){ // we are below the target alt @@ -444,12 +388,12 @@ static int32_t get_new_altitude() } }else { // we are above the target, going down - if(diff < 600){ - _scale = 4; - } - if(diff < 300){ + if(diff < 400){ _scale = 5; } + if(diff < 100){ + _scale = 6; + } } int32_t change = (millis() - alt_change_timer) >> _scale; diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 271c57ed37..2e1ed8ecd5 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -52,8 +52,8 @@ static void init_barometer(void) static void reset_baro(void) { - ground_pressure = barometer.get_pressure(); - ground_temperature = barometer.get_temperature(); + ground_pressure = barometer.get_pressure(); + ground_temperature = barometer.get_temperature(); } static int32_t read_barometer(void) @@ -107,8 +107,8 @@ static void read_battery(void) battery_voltage1 = 0; return; } - - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) + + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; if(g.battery_monitoring == 4) { current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 96e65b9c29..682e450229 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -1096,7 +1096,7 @@ static void print_enabled(boolean b) static void init_esc() { - motors_output_enable(); + motors_output_enable(); while(1){ read_radio(); delay(100); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d4ad437754..9cf32e522a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -354,8 +354,8 @@ static void init_ardupilot() Log_Write_Startup(); Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP()); - Log_Write_Data(12, g.pi_rate_roll.kP()); - Log_Write_Data(13, g.pi_rate_pitch.kP()); + Log_Write_Data(12, g.pid_rate_roll.kP()); + Log_Write_Data(13, g.pid_rate_pitch.kP()); #endif SendDebug("\nReady to FLY "); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index e1fc53df4d..9bca37873a 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -94,7 +94,7 @@ test_mode(uint8_t argc, const Menu::arg *argv) { //Serial.printf_P(PSTR("Test Mode\n\n")); test_menu.run(); - return 0; + return 0; } static int8_t @@ -469,7 +469,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) static int8_t test_imu(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included print_test_disabled(); return (0); #else @@ -515,7 +515,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) //dcm.kp_yaw(0.02); //dcm.ki_yaw(0); - imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); + imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); report_imu(); imu.init_gyro(delay, flash_leds); @@ -555,7 +555,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) compass.calculate(dcm.get_dcm_matrix()); } } - fast_loopTimer = millis(); + fast_loopTimer = millis(); } if(Serial.available() > 0){ return (0); @@ -731,59 +731,68 @@ test_tuning(uint8_t argc, const Menu::arg *argv) static int8_t test_battery(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - //delta_ms_medium_loop = 100; + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + print_hit_enter(); + while(1){ + delay(100); + read_radio(); + read_battery(); + if (g.battery_monitoring == 3){ + Serial.printf_P(PSTR("V: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } else { + Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } + APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); - while(1){ - delay(100); - read_radio(); - read_battery(); - if (g.battery_monitoring == 3){ - Serial.printf_P(PSTR("V: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } else { - Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); + if(Serial.available() > 0){ + return (0); + } } - APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); - - if(Serial.available() > 0){ - return (0); - } - } - return (0); + return (0); + #endif } - static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else - while(1){ - Serial.printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } + print_hit_enter(); + delay(1000); - Serial.printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(Serial.available() > 0){ - return (0); + while(1){ + Serial.printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } } - } + #endif } + static int8_t test_wp(uint8_t argc, const Menu::arg *argv) { @@ -810,20 +819,20 @@ test_wp(uint8_t argc, const Menu::arg *argv) /* print_hit_enter(); delay(1000); - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LED_OFF); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LED_OFF); - } - if(Serial.available() > 0){ - return (0); - } + while(1){ + if (Serial3.available()){ + digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS + Serial1.write(Serial3.read()); + digitalWrite(B_LED_PIN, LED_OFF); + } + if (Serial1.available()){ + digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS + Serial3.write(Serial1.read()); + digitalWrite(C_LED_PIN, LED_OFF); + } + if(Serial.available() > 0){ + return (0); + } } */ //} @@ -836,7 +845,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); while(1){ - if (Serial3.available()) + if (Serial3.available()) Serial3.write(Serial3.read()); if(Serial.available() > 0){ @@ -850,30 +859,30 @@ test_wp(uint8_t argc, const Menu::arg *argv) static int8_t test_baro(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - init_barometer(); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + print_hit_enter(); + init_barometer(); - while(1){ - delay(100); - int32_t alt = read_barometer(); // calls barometer.read() + while(1){ + delay(100); + int32_t alt = read_barometer(); // calls barometer.read() - #if defined( __AVR_ATmega1280__ ) - Serial.printf_P(PSTR("alt: %ldcm\n"),alt); - - #else - int32_t pres = barometer.get_pressure(); - int16_t temp = barometer.get_temperature(); - int32_t raw_pres = barometer.get_raw_pressure(); - int32_t raw_temp = barometer.get_raw_temp(); - Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," - " raw pres: %ld, raw temp: %ld\n"), - alt, pres ,temp, raw_pres, raw_temp); - #endif - if(Serial.available() > 0){ - return (0); + int32_t pres = barometer.get_pressure(); + int16_t temp = barometer.get_temperature(); + int32_t raw_pres = barometer.get_raw_pressure(); + int32_t raw_temp = barometer.get_raw_temp(); + Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," + " raw pres: %ld, raw temp: %ld\n"), + alt, pres ,temp, raw_pres, raw_temp); + if(Serial.available() > 0){ + return (0); + } } - } - return 0; + return 0; + #endif } #endif @@ -881,35 +890,38 @@ test_baro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - if(g.compass_enabled) { - //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - - print_hit_enter(); - - while(1){ - delay(100); - if (compass.read()) { - compass.calculate(dcm.get_dcm_matrix()); - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z); - } else { - Serial.println_P(PSTR("not healthy")); - } - - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); return (0); - } - return (0); + #else + if(g.compass_enabled) { + print_hit_enter(); + + while(1){ + delay(100); + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z); + } else { + Serial.println_P(PSTR("not healthy")); + } + + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + return (0); + #endif } /* @@ -1023,25 +1035,31 @@ test_optflow(uint8_t argc, const Menu::arg *argv) /* test the dataflash is working */ + static int8_t test_logging(uint8_t argc, const Menu::arg *argv) { - Serial.println_P(PSTR("Testing dataflash logging")); - if (!DataFlash.CardInserted()) { - Serial.println_P(PSTR("ERR: No dataflash inserted")); - return 0; - } - DataFlash.ReadManufacturerID(); - Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), - (unsigned)DataFlash.df_manufacturer, - (unsigned)DataFlash.df_device); - Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), - (unsigned)DataFlash.df_NumPages+1, - (unsigned)DataFlash.df_PageSize); - DataFlash.StartRead(DataFlash.df_NumPages+1); - Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), - (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); - return 0; + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + Serial.println_P(PSTR("Testing dataflash logging")); + if (!DataFlash.CardInserted()) { + Serial.println_P(PSTR("ERR: No dataflash inserted")); + return 0; + } + DataFlash.ReadManufacturerID(); + Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), + (unsigned)DataFlash.df_manufacturer, + (unsigned)DataFlash.df_device); + Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), + (unsigned)DataFlash.df_NumPages+1, + (unsigned)DataFlash.df_PageSize); + DataFlash.StartRead(DataFlash.df_NumPages+1); + Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), + (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); + return 0; + #endif } @@ -1061,11 +1079,11 @@ static int8_t //} // clear home - {Location t = {0, 0, 0, 0, 0, 0}; + {Location t = {0, 0, 0, 0, 0, 0}; set_cmd_with_index(t,0);} // CMD opt pitch alt/cm - {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; + {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; set_cmd_with_index(t,1);} if (!strcmp_P(argv[1].str, PSTR("wp"))) { @@ -1074,25 +1092,25 @@ static int8_t {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; set_cmd_with_index(t,2);} // CMD opt - {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; set_cmd_with_index(t,3);} // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; set_cmd_with_index(t,4);} } else { //2250 = 25 meteres // CMD opt p1 //alt //NS //WE - {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 + {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 set_cmd_with_index(t,2);} // CMD opt dir angle/deg deg/s relative - {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; + {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; set_cmd_with_index(t,3);} // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; set_cmd_with_index(t,4);} } diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index ca896af5e0..a0ee6c833b 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -253,6 +253,12 @@ XorPlus.cs + + Form + + + SerialInput.cs + Firmware.cs @@ -432,6 +438,9 @@ XorPlus.cs + + SerialInput.cs + Configuration.cs diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index 014f0498e7..bc4bfa801c 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -198,11 +198,43 @@ namespace ArdupilotMega this.Lng = pll.Lng; } + public PointLatLngAlt(PointLatLngAlt plla) + { + this.Lat = plla.Lat; + this.Lng = plla.Lng; + this.Alt = plla.Alt; + this.color = plla.color; + this.Tag = plla.Tag; + } + public PointLatLng Point() { return new PointLatLng(Lat, Lng); } + public override bool Equals(Object pllaobj) + { + PointLatLngAlt plla = (PointLatLngAlt)pllaobj; + + if (plla == null) + return false; + + if (this.Lat == plla.Lat && + this.Lng == plla.Lng && + this.Alt == plla.Alt && + this.color == plla.color && + this.Tag == plla.Tag) + { + return true; + } + return false; + } + + public override int GetHashCode() + { + return (int)((Lat + Lng + Alt) * 100); + } + /// /// Calc Distance in M /// @@ -263,7 +295,8 @@ namespace ArdupilotMega RTL = 6, // AUTO control CIRCLE = 7, POSITION = 8, - LAND = 9 // AUTO control + LAND = 9, // AUTO control + OF_LOITER = 10 } public static void linearRegression() diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index b41cf713b8..55fa2ecbeb 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -30,8 +30,8 @@ { this.components = new System.ComponentModel.Container(); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration)); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle(); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle(); this.Params = new System.Windows.Forms.DataGridView(); this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn(); @@ -285,6 +285,8 @@ this.BUT_load = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.BUT_compare = new ArdupilotMega.MyButton(); + this.myLabel2 = new ArdupilotMega.MyLabel(); + this.TUNE = new System.Windows.Forms.ComboBox(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); this.ConfigTabs.SuspendLayout(); this.TabAP.SuspendLayout(); @@ -405,14 +407,14 @@ this.Params.AllowUserToAddRows = false; this.Params.AllowUserToDeleteRows = false; resources.ApplyResources(this.Params, "Params"); - dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon; - dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White; - dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1; + dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon; + dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White; + dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3; this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize; this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] { this.Command, @@ -421,14 +423,14 @@ this.mavScale, this.RawValue}); this.Params.Name = "Params"; - dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption; - dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText; - dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; + dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption; + dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText; + dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4; this.Params.RowHeadersVisible = false; this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); // @@ -1089,6 +1091,8 @@ // // TabAC // + this.TabAC.Controls.Add(this.myLabel2); + this.TabAC.Controls.Add(this.TUNE); this.TabAC.Controls.Add(this.myLabel1); this.TabAC.Controls.Add(this.CH7_OPT); this.TabAC.Controls.Add(this.groupBox17); @@ -2092,6 +2096,40 @@ this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); // + // myLabel2 + // + resources.ApplyResources(this.myLabel2, "myLabel2"); + this.myLabel2.Name = "myLabel2"; + this.myLabel2.resize = false; + // + // TUNE + // + this.TUNE.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.TUNE.FormattingEnabled = true; + this.TUNE.Items.AddRange(new object[] { + resources.GetString("TUNE.Items"), + resources.GetString("TUNE.Items1"), + resources.GetString("TUNE.Items2"), + resources.GetString("TUNE.Items3"), + resources.GetString("TUNE.Items4"), + resources.GetString("TUNE.Items5"), + resources.GetString("TUNE.Items6"), + resources.GetString("TUNE.Items7"), + resources.GetString("TUNE.Items8"), + resources.GetString("TUNE.Items9"), + resources.GetString("TUNE.Items10"), + resources.GetString("TUNE.Items11"), + resources.GetString("TUNE.Items12"), + resources.GetString("TUNE.Items13"), + resources.GetString("TUNE.Items14"), + resources.GetString("TUNE.Items15"), + resources.GetString("TUNE.Items16"), + resources.GetString("TUNE.Items17"), + resources.GetString("TUNE.Items18"), + resources.GetString("TUNE.Items19")}); + resources.ApplyResources(this.TUNE, "TUNE"); + this.TUNE.Name = "TUNE"; + // // Configuration // resources.ApplyResources(this, "$this"); @@ -2479,5 +2517,7 @@ private System.Windows.Forms.Label label48; private MyLabel myLabel1; private System.Windows.Forms.ComboBox CH7_OPT; + private MyLabel myLabel2; + private System.Windows.Forms.ComboBox TUNE; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index e77e622e34..3b22247b59 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -192,6 +192,2346 @@ Top, Bottom, Left, Right + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 0 + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 1 + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 2 + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 3 + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 4 + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 5 + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 6 + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 7 + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 8 + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 9 + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 10 + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + ArduPlane + + + TabAP + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 0 + + + 358, 297 + + + 53, 23 + + + 20 + + + Ch6 Opt + + + myLabel2 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabAC + + + 0 + + + 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 + + + 417, 297 + + + 112, 21 + + + 19 + + + TUNE + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 1 + + + 358, 270 + + + 53, 23 + + + 18 + + + Ch7 Opt + + + myLabel1 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabAC + + + 2 + + + Do Nothing + + + + + + + + + Simple Mode + + + RTL + + + + + + + + + Save WP + + + 417, 270 + + + 112, 21 + + + 17 + + + CH7_OPT + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 3 + + + ACRO_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 0 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 1 + + + ACRO_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 2 + + + label29 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 3 + + + ACRO_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 4 + + + label33 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 5 + + + 182, 337 + + + 170, 91 + + + 13 + + + Acro Pitch + + + groupBox17 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 4 + + + label14 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 0 + + + THR_RATE_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 1 + + + THR_RATE_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 2 + + + label20 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 3 + + + THR_RATE_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 4 + + + label25 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 5 + + + 6, 221 + + + 170, 110 + + + 16 + + + Throttle Rate + + + groupBox5 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 5 + + + ACRO_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 0 + + + label40 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 1 + + + ACRO_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 2 + + + label44 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 3 + + + ACRO_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 4 + + + label48 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 5 + + + 6, 337 + + + 170, 91 + + + 14 + + + Acro Roll + + + groupBox18 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 6 + + + True + + + 3, 198 + + + 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 + + + WP_SPEED_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + NAV_LAT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + NAV_LAT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 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, 221 + + + 170, 43 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 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, 221 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 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 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 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 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 12 + + + STB_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + STB_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + STB_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 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 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 14 + + + RATE_YAW_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + RATE_YAW_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + RATE_YAW_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 15 + + + RATE_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + RATE_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + RATE_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 16 + + + RATE_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + RATE_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + RATE_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 17 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + ArduCopter + + + TabAC + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 1 + + + label26 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + CMB_videoresolutions + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + label12 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + CHK_GDIPlus + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 36 + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 37 + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 38 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 39 + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 40 + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 41 + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 42 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + + + TabPlanner + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 2 + + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + + + TabSetup + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 3 + + + 52, 18 + + + 278, 0 + + + 0, 0, 0, 0 + + + 0, 0 + + + 730, 460 + + + 62 + + + ConfigTabs + + + System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 2 + + + THR_FS_VALUE + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + THR_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + THR_MIN + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 0 + 111, 82 @@ -384,29 +2724,125 @@ 7 - - 405, 217 + + ARSPD_RATIO - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + 195, 108 - - 0 + + 1 - - Throttle 0-100% + + Airspeed m/s - - groupBox3 + + groupBox1 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 0 + + 1 111, 82 @@ -600,29 +3036,101 @@ 7 - - 406, 325 + + LIM_PITCH_MIN - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + LIM_PITCH_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + LIM_ROLL_CD + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + 195, 108 - - 1 + + 2 - - Airspeed m/s + + Navigation Angles - - groupBox1 + + groupBox2 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 1 + + 2 111, 59 @@ -768,29 +3276,77 @@ 5 - - 205, 325 + + XTRK_ANGLE_CD - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + XTRK_GAIN_SC + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + 195, 108 - - 2 + + 3 - - Navigation Angles + + Xtrack Pids - - groupBox2 + + groupBox15 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 2 + + 3 111, 36 @@ -888,29 +3444,101 @@ 3 - - 4, 325 + + KFF_PTCH2THR - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + KFF_RDDRMIX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + KFF_PTCHCOMP + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + 195, 108 - - 3 + + 4 - - Xtrack Pids + + Other Mix's - - groupBox15 + + groupBox16 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 3 + + 4 111, 13 @@ -1056,29 +3684,125 @@ 5 - - 205, 217 + + ENRGY2THR_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + ENRGY2THR_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + ENRGY2THR_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + 195, 108 - - 4 + + 5 - - Other Mix's + + Energy/Alt Pid - - groupBox16 + + groupBox14 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 4 + + 5 111, 82 @@ -1272,29 +3996,125 @@ 7 - - 4, 217 + + ALT2PTCH_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + ALT2PTCH_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + ALT2PTCH_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + ALT2PTCH_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + 195, 108 - - 5 + + 6 - - Energy/Alt Pid + + Nav Pitch Alt Pid - - groupBox14 + + groupBox13 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 5 + + 6 111, 82 @@ -1488,29 +4308,125 @@ 7 - - 406, 109 + + ARSP2PTCH_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + ARSP2PTCH_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + ARSP2PTCH_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + ARSP2PTCH_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + 195, 108 - - 6 + + 7 - - Nav Pitch Alt Pid + + Nav Pitch AS Pid - - groupBox13 + + groupBox12 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 6 + + 7 111, 82 @@ -1704,29 +4620,125 @@ 7 - - 205, 109 + + HDNG2RLL_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + HDNG2RLL_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + HDNG2RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + 195, 108 - - 7 + + 8 - - Nav Pitch AS Pid + + Nav Roll Pid - - groupBox12 + + groupBox11 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 7 + + 8 111, 82 @@ -1920,29 +4932,125 @@ 7 - - 4, 109 + + YW2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + YW2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + YW2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + 195, 108 - - 8 + + 9 - - Nav Roll Pid + + Servo Yaw Pid - - groupBox11 + + groupBox10 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 8 + + 9 111, 82 @@ -2136,29 +5244,125 @@ 7 - - 406, 1 + + PTCH2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + PTCH2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + PTCH2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + 195, 108 - - 9 + + 10 - - Servo Yaw Pid + + Servo Pitch Pid - - groupBox10 + + groupBox9 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 9 + + 10 111, 82 @@ -2352,29 +5556,125 @@ 7 - - 205, 1 + + RLL2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + RLL2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + RLL2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + 195, 108 - - 10 + + 11 - - Servo Pitch Pid + + Servo Roll Pid - - groupBox9 + + groupBox8 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 10 + + 11 111, 82 @@ -2568,126 +5868,6 @@ 7 - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAP - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - ArduPlane - - - TabAP - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 0 - - - 358, 270 - - - 53, 23 - - - 18 - - - Ch7 Opt - - - myLabel1 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - TabAC - - - 0 - - - Do Nothing - - - - - - - - - Simple Mode - - - RTL - - - - - - - - - Save WP - - - 417, 270 - - - 112, 21 - - - 17 - - - CH7_OPT - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 1 - 80, 63 @@ -2832,30 +6012,6 @@ 5 - - 182, 337 - - - 170, 91 - - - 13 - - - Acro Pitch - - - groupBox17 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 2 - NoControl @@ -3000,30 +6156,6 @@ 5 - - 6, 221 - - - 170, 110 - - - 16 - - - Throttle Rate - - - groupBox5 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 3 - 80, 63 @@ -3168,57 +6300,6 @@ 5 - - 6, 337 - - - 170, 91 - - - 14 - - - Acro Roll - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 4 - - - True - - - 3, 198 - - - 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 - - - 5 - 80, 86 @@ -3411,30 +6492,6 @@ 7 - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 6 - 80, 13 @@ -3483,30 +6540,6 @@ 1 - - 358, 221 - - - 170, 43 - - - 2 - - - Crosstrack Correction - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 7 - 80, 63 @@ -3651,30 +6684,6 @@ 5 - - 182, 221 - - - 170, 110 - - - 3 - - - Altitude Hold - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 8 - 80, 61 @@ -3819,30 +6828,6 @@ 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 - - - 9 - 80, 63 @@ -3987,30 +6972,6 @@ 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 - - - 10 - 80, 63 @@ -4155,30 +7116,6 @@ 5 - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 11 - 80, 63 @@ -4323,30 +7260,6 @@ 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 - - - 12 - 80, 63 @@ -4491,30 +7404,6 @@ 5 - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 13 - 80, 63 @@ -4659,30 +7548,6 @@ 5 - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 14 - 80, 63 @@ -4827,57 +7692,6 @@ 5 - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 15 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - ArduCopter - - - TabAC - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 1 - NoControl @@ -4953,6 +7767,9 @@ 2 + + 17, 17 + NoControl @@ -4968,9 +7785,6 @@ GDI+ (old type) - - 17, 17 - OpenGL = Disabled GDI+ = Enabled @@ -6035,87 +8849,6 @@ GDI+ = Enabled 42 - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - TabPlanner - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 2 - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - - - TabSetup - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 3 - - - 52, 18 - - - 278, 0 - - - 0, 0, 0, 0 - - - 0, 0 - - - 730, 460 - - - 62 - - - ConfigTabs - - - System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 2 - 0, 0 @@ -6269,6 +9002,9 @@ GDI+ = Enabled 5 + + 17, 17 + Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index 9064d01ee2..c5638dfb2d 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -134,7 +134,7 @@ Point Camera Here - 175, 48 + 175, 70 contextMenuStrip1 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 1e0723f1f9..01bbbac698 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -290,11 +290,13 @@ namespace ArdupilotMega.GCSViews } float ans; - if (float.TryParse(TXT_homealt.Text, out result) && float.TryParse(cell.Value.ToString(), out ans)) + if (float.TryParse(cell.Value.ToString(), out ans)) { ans = (int)ans; - if (alt != 0) + if (alt != 0) // use passed in value; cell.Value = alt.ToString(); + if (ans == 0) + cell.Value = 50; // online verify height if (isonline && CHK_geheight.Checked) { @@ -314,7 +316,7 @@ namespace ArdupilotMega.GCSViews { cell.Value = (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text)).ToString(); } // is relative and check height - else if (float.TryParse(TXT_homealt.Text, out result) && isonline && CHK_geheight.Checked) + else if (isonline && CHK_geheight.Checked) { alt = (int)getGEAlt(lat, lng); @@ -328,10 +330,6 @@ namespace ArdupilotMega.GCSViews cell.Style.BackColor = Color.LightGreen; } } - else - { - cell.Value = 100 ; - } } cell.DataGridView.EndEdit(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 97dd323d1e..ad649e5a85 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -348,7 +348,6 @@ namespace ArdupilotMega.GCSViews ArdupilotMega.MainV2.config["REV_pitch"] = CHKREV_pitch.Checked.ToString(); ArdupilotMega.MainV2.config["REV_rudder"] = CHKREV_rudder.Checked.ToString(); ArdupilotMega.MainV2.config["GPSrate"] = GPSrate.Text; - ArdupilotMega.MainV2.config["Xplanes"] = RAD_softXplanes.Checked.ToString(); ArdupilotMega.MainV2.config["MAVrollgain"] = TXT_rollgain.Text; ArdupilotMega.MainV2.config["MAVpitchgain"] = TXT_pitchgain.Text; @@ -389,9 +388,6 @@ namespace ArdupilotMega.GCSViews case "GPSrate": GPSrate.Text = ArdupilotMega.MainV2.config[key].ToString(); break; - case "Xplanes": - RAD_softXplanes.Checked = bool.Parse(ArdupilotMega.MainV2.config[key].ToString()); - break; case "MAVrollgain": TXT_rollgain.Text = ArdupilotMega.MainV2.config[key].ToString(); break; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 0f98ceb923..6581e2e9df 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -2006,7 +2006,7 @@ namespace ArdupilotMega } catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; } - if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync + if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M' || lastbad[1] == 'G') // out of sync { if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r') { diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 7ac34587f4..13bdc50258 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.29")] +[assembly: AssemblyFileVersion("1.1.30")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Script.cs b/Tools/ArdupilotMegaPlanner/Script.cs index 016bde55bd..df94c21309 100644 --- a/Tools/ArdupilotMegaPlanner/Script.cs +++ b/Tools/ArdupilotMegaPlanner/Script.cs @@ -10,8 +10,8 @@ namespace ArdupilotMega { DateTime timeout = DateTime.Now; List items = new List(); - Microsoft.Scripting.Hosting.ScriptEngine engine; - Microsoft.Scripting.Hosting.ScriptScope scope; + static Microsoft.Scripting.Hosting.ScriptEngine engine; + static Microsoft.Scripting.Hosting.ScriptScope scope; // keeps history MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t(); diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs b/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs new file mode 100644 index 0000000000..62d2900dd9 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs @@ -0,0 +1,135 @@ +namespace ArdupilotMega +{ + partial class SerialInput + { + /// + /// Required designer variable. + /// + private System.ComponentModel.IContainer components = null; + + /// + /// Clean up any resources being used. + /// + /// true if managed resources should be disposed; otherwise, false. + protected override void Dispose(bool disposing) + { + if (disposing && (components != null)) + { + components.Dispose(); + } + base.Dispose(disposing); + } + + #region Windows Form Designer generated code + + /// + /// Required method for Designer support - do not modify + /// the contents of this method with the code editor. + /// + private void InitializeComponent() + { + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(SerialInput)); + this.CMB_serialport = new System.Windows.Forms.ComboBox(); + this.BUT_connect = new ArdupilotMega.MyButton(); + this.CMB_baudrate = new System.Windows.Forms.ComboBox(); + this.label1 = new System.Windows.Forms.Label(); + this.LBL_location = new System.Windows.Forms.Label(); + this.textBox1 = new System.Windows.Forms.TextBox(); + this.SuspendLayout(); + // + // CMB_serialport + // + this.CMB_serialport.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.CMB_serialport.FormattingEnabled = true; + this.CMB_serialport.Location = new System.Drawing.Point(13, 13); + this.CMB_serialport.Name = "CMB_serialport"; + this.CMB_serialport.Size = new System.Drawing.Size(121, 21); + this.CMB_serialport.TabIndex = 0; + // + // BUT_connect + // + this.BUT_connect.Location = new System.Drawing.Point(279, 12); + this.BUT_connect.Name = "BUT_connect"; + this.BUT_connect.Size = new System.Drawing.Size(75, 23); + this.BUT_connect.TabIndex = 1; + this.BUT_connect.Text = "Connect"; + this.BUT_connect.UseVisualStyleBackColor = true; + this.BUT_connect.Click += new System.EventHandler(this.BUT_connect_Click); + // + // CMB_baudrate + // + this.CMB_baudrate.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.CMB_baudrate.FormattingEnabled = true; + this.CMB_baudrate.Items.AddRange(new object[] { + "4800", + "9600", + "14400", + "19200", + "28800", + "38400", + "57600", + "115200"}); + this.CMB_baudrate.Location = new System.Drawing.Point(140, 12); + this.CMB_baudrate.Name = "CMB_baudrate"; + this.CMB_baudrate.Size = new System.Drawing.Size(121, 21); + this.CMB_baudrate.TabIndex = 2; + // + // label1 + // + this.label1.AutoSize = true; + this.label1.Location = new System.Drawing.Point(90, 47); + this.label1.Name = "label1"; + this.label1.Size = new System.Drawing.Size(187, 13); + this.label1.TabIndex = 3; + this.label1.Text = "Pick the Nmea gps port and baud rate\r\n"; + // + // LBL_location + // + this.LBL_location.AutoSize = true; + this.LBL_location.Font = new System.Drawing.Font("Microsoft Sans Serif", 14.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.LBL_location.Location = new System.Drawing.Point(9, 78); + this.LBL_location.Name = "LBL_location"; + this.LBL_location.Size = new System.Drawing.Size(50, 24); + this.LBL_location.TabIndex = 4; + this.LBL_location.Text = "0,0,0"; + // + // textBox1 + // + this.textBox1.Enabled = false; + this.textBox1.Location = new System.Drawing.Point(19, 126); + this.textBox1.Multiline = true; + this.textBox1.Name = "textBox1"; + this.textBox1.Size = new System.Drawing.Size(335, 133); + this.textBox1.TabIndex = 5; + this.textBox1.Text = resources.GetString("textBox1.Text"); + // + // SerialInput + // + this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); + this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.ClientSize = new System.Drawing.Size(369, 300); + this.Controls.Add(this.textBox1); + this.Controls.Add(this.LBL_location); + this.Controls.Add(this.label1); + this.Controls.Add(this.CMB_baudrate); + this.Controls.Add(this.BUT_connect); + this.Controls.Add(this.CMB_serialport); + this.Icon = ((System.Drawing.Icon)(resources.GetObject("$this.Icon"))); + this.Name = "SerialInput"; + this.Text = "Follow Me"; + this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.SerialOutput_FormClosing); + this.ResumeLayout(false); + this.PerformLayout(); + + } + + #endregion + + private System.Windows.Forms.ComboBox CMB_serialport; + private MyButton BUT_connect; + private System.Windows.Forms.ComboBox CMB_baudrate; + private System.Windows.Forms.Label label1; + private System.Windows.Forms.Label LBL_location; + private System.Windows.Forms.TextBox textBox1; + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.cs b/Tools/ArdupilotMegaPlanner/SerialInput.cs new file mode 100644 index 0000000000..d7ebd2c98d --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.cs @@ -0,0 +1,224 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Data; +using System.Drawing; +using System.Linq; +using System.Text; +using System.Windows.Forms; +using System.IO.Ports; + +namespace ArdupilotMega +{ + public partial class SerialInput : Form + { + System.Threading.Thread t12; + static bool threadrun = false; + static internal SerialPort comPort = new SerialPort(); + static internal PointLatLngAlt lastgotolocation = new PointLatLngAlt(0, 0, 0, "Goto last"); + static internal PointLatLngAlt gotolocation = new PointLatLngAlt(0, 0, 0, "Goto"); + static internal int intalt = 100; + + public SerialInput() + { + InitializeComponent(); + + CMB_serialport.DataSource = SerialPort.GetPortNames(); + + if (threadrun) + { + BUT_connect.Text = "Stop"; + } + } + + private void BUT_connect_Click(object sender, EventArgs e) + { + if (comPort.IsOpen) + { + threadrun = false; + comPort.Close(); + BUT_connect.Text = "Connect"; + } + else + { + try + { + comPort.PortName = CMB_serialport.Text; + } + catch { MessageBox.Show("Invalid PortName"); return; } + try { + comPort.BaudRate = int.Parse(CMB_baudrate.Text); + } catch {MessageBox.Show("Invalid BaudRate"); return;} + try { + comPort.Open(); + } catch {MessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;} + + + string alt = "100"; + + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + { + alt = (10 * MainV2.cs.multiplierdist).ToString("0"); + } + else + { + alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + } + if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Alt", ref alt)) + return; + + intalt = (int)(100 * MainV2.cs.multiplierdist); + if (!int.TryParse(alt, out intalt)) + { + MessageBox.Show("Bad Alt"); + return; + } + + t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) + { + IsBackground = true, + Name = "Nmea Input" + }; + t12.Start(); + + BUT_connect.Text = "Stop"; + } + } + + void mainloop() + { + + + threadrun = true; + int counter = 0; + while (threadrun) + { + try + { + string line = comPort.ReadLine(); + + + //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.cs.lng < 0 ? "W" : "E", MainV2.cs.gpsstatus, MainV2.cs.satcount, MainV2.cs.gpshdop, MainV2.cs.alt, "M", 0, "M", ""); + if (line.StartsWith("$GPGGA")) // + { + int c1 = line.IndexOf(',',0)+ 1; + int c2 = line.IndexOf(',', c1) + 1; + int c3 = line.IndexOf(',', c2) + 1; + int c4 = line.IndexOf(',', c3 ) + 1; + int c5 = line.IndexOf(',', c4 ) + 1; + int c6 = line.IndexOf(',', c5 ) + 1; + int c7 = line.IndexOf(',', c6 ) + 1; + int c8 = line.IndexOf(',', c7 ) + 1; + int c9 = line.IndexOf(',', c8 ) + 1; + int c10 = line.IndexOf(',', c9 ) + 1; + int c11 = line.IndexOf(',', c10 ) + 1; + int c12 = line.IndexOf(',', c11) + 1; + + gotolocation.Lat = double.Parse(line.Substring(c2, c3 - c2 - 1)) / 100.0; + + gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60); + + if (line.Substring(c3, c4 - c3 - 1) == "S") + gotolocation.Lat *= -1; + + gotolocation.Lng = double.Parse(line.Substring(c4, c5 - c4 - 1)) / 100.0; + + gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60); + + if (line.Substring(c5, c6 - c5 - 1) == "W") + gotolocation.Lng *= -1; + + gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) + + + } + + + if (counter % 10 == 0 && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && + { + Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt); + lastgotolocation = new PointLatLngAlt(gotolocation); + + Locationwp gotohere = new Locationwp(); + + gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; + gotohere.alt = (float)(gotolocation.Alt); + gotohere.lat = (float)(gotolocation.Lat); + gotohere.lng = (float)(gotolocation.Lng); + + try + { + updateLocationLabel(gotohere); + } + catch { } + + if (MainV2.comPort.BaseStream.IsOpen && MainV2.givecomport == false) + { + try + { + MainV2.givecomport = true; + + MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2); + + MainV2.givecomport = false; + } + catch { MainV2.givecomport = false; } + } + } + + System.Threading.Thread.Sleep(200); + counter++; + } + catch { System.Threading.Thread.Sleep(2000); } + } + } + + private void updateLocationLabel(Locationwp plla) + { + this.BeginInvoke((MethodInvoker)delegate + { + LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt; + } + ); + + } + + private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e) + { + } + + // Calculates the checksum for a sentence + string GetChecksum(string sentence) + { + // Loop through all chars to get a checksum + int Checksum = 0; + foreach (char Character in sentence.ToCharArray()) + { + switch (Character) + { + case '$': + // Ignore the dollar sign + break; + case '*': + // Stop processing before the asterisk + continue; + default: + // Is this the first value for the checksum? + if (Checksum == 0) + { + // Yes. Set the checksum to the value + Checksum = Convert.ToByte(Character); + } + else + { + // No. XOR the checksum with this character's value + Checksum = Checksum ^ Convert.ToByte(Character); + } + break; + } + } + // Return the checksum formatted as a two-character hexadecimal + return Checksum.ToString("X2"); + } + + } +} diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.resx b/Tools/ArdupilotMegaPlanner/SerialInput.resx new file mode 100644 index 0000000000..62276c20c7 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.resx @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + What this does. +1. gets the current gps coords from a nmea gps. +2. sends a guided mode WP to the AP every 2 seconds. + +How to use it +1. connect to ap. +2. take off, test guided mode is working. +3. open this and pick your comport, and baud rate for your nmea gps. +4. it should now be following you. + + + + + AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAABILAAASCwAAAAAAAAAA + AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADOxkjAtnoOAKpJ4vyiK + c+8nh3D/J4Zv/yeHcP8oi3PvKpJ4vy6fg4AzsZIwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADjGo2AyspPfLZ+D/yiQ + d/8hlXj/G6F9/xeqg/8XqYL/GKqD/xuhfv8ilnn/KZB3/y2fhP8yspPfN8ajYAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADvRrDA1vpzfL6uN/yel + hP8XvJD/DMyY/wfQl/8FzJP/A8qS/wPJkf8EypL/BsyU/wnRmP8PzZn/Gb2R/yemhP8tqoz/Mb2a3zbQ + qkAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAA4y6ZgMbWV/yin + iP8WwZP/Btqf/wDPlf8AyI7/A8aP/yfNnv9T2LP/UNax/03XsP8506b/G8ya/wHKkf8F0Zf/CNuf/xLB + kv8fpYT/J7KQ/y7IomAAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAANcajny+w + kf8hqoj/CNSd/wDRlf8Axor/Hcyd/3Lhwf+p7Nj/o+vV/57m0/+X5dD/k+TN/4/jzf+K5Mz/fuHH/0PW + rf8HzJT/ANCT/wDRlv8OpX//HayI/yrFn58AAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAADDC + nmAtro7/H62J/wPWmv8Ay47/AMaO/3XhxP+e6tT/mObP/5Pjy/+Q4sr/jODJ/4ffx/+C3MT/f9vC/3nb + wf9y2r7/adq7/2DauP8ZzZv/Fdae/8T/9/9WxKj/HKuI/y7IomAAAAAAAAAAAAAAAAD///8AAAAAAAAA + AAAiuZMwKKyM/x6ohf8C1Zr/AMmL/wHGjv+49OL///////////9+3ML/f9zD/4Dcwv9+28L/e9rA/3bZ + vv9w1rr/Z9S4/17Rs/9Qz63/Qcyn/3LewP////////////n///8MpH7/JbKP/zXQqUAAAAAAAAAAAP// + /wAAAAAAAAAAABymhN8dnn//BNGa/wDKjP8AxY3/sfHf/////////////////2nXt/9w1rv/c9e8/3TX + vP9x17z/a9W5/2TTtf9Y0K//SMyp/zXFoP9i07X/////////////////f/LR/wDQlf8epYT/Mb2a3wAA + AAAAAAAA////AAAAAAADlnJgFZR1/wq4iv8AzpH/AMCD/4rmzf//////////////////////WdGv/2PU + tf9p1rf/atS4/2nUtv9i0rT/Vc+u/0fKpv8zxZz/Ws+w//////////////////////8GyJL/ANCS/xLB + kv8tq4z/OMajYAAAAAD///8AAAAAAACHZt8NkW//ANKV/wDChP9i27r//////////////////////9Dx + 6P9MzKn/Vc+v/17Rsv9g0rP/XNCx/1XNrv9Fyaf/McSd/1fPr///////////////////////QM2m/ynK + oP8JzJX/C9yh/ymmhf80spPfAAAAAP///wAAcUwwAHtc/wCrfP8AyIv/AMKK//////////////////// + /////////////5Dgyv9Gyqb/TMyq/07Nq/9MzKn/Qcmj/y/Fnf9Wzq3//////////////////////57k + 0v8av5T/Lceg/yzOo/8M05v/Hr6T/zCghf80spIw////AABoRYAAclT/AL2H/wDBhf9R1rL///////// + ////////4vfw//////////////////H8+P9KzKn/Ocah/zTFnv8qwpj/Us2t//////////////////// + ////////DLqM/yDBlv8wxp//OM6m/xPPm/8Xz53/LZF5/y+fg4////8AAGNAvwB7Wf8Aw4j/ALyC/4bj + yP+g5tL/g93E/2HSsv9Pzqz/Us6s//////////////////////9Yzq//Gr2S/0jLp/////////////// + /////////////yrDm/8SvI//JMGY/zDHn/81zKT/Is2e/xTUnf8nl3v/LJJ5v////wAAXz3vAIlg/wDA + hf8AuoD/quzZ/5Hjyv9628D/ada2/1jRsP9Jy6f/a9a4//////////////////////+Y4s7///////// + //////////////////+c4tD/AbaH/xW8kf8jwZj/LcWd/y/Jn/8kzJ3/E9Ca/yGjgf8ri3Tv////AABd + PP8Ak2b/AL6D/w/Ekv+m6tf/j+HJ/3vawP9p1rf/W9Gx/0rNqf85yJ//Nsaf//////////////////// + /////////////////////////////wCwe/8AtoT/ELqP/xu+k/8jwZj/KMeb/yHKm/8QzZf/HqyG/ymI + cf////8AAF07/wCSZP8AvYL/GMWU/6Dn1P+K38f/ddi+/27Wuf+E3MX/leHN/6fm1f+l5tX/neLQ//// + ////////////////////////////////////////j9/J/27Vuv9Tzq7/JsKY/xa/kv8aw5T/FcaW/wvL + lf8aqoT/J4dw/////wAAXTv/AJFk/wC9gP8GwY3/mObQ/5rkz/+26dv/y/Hl/8Dt3/+06tz/pebV/5bg + zP+g5NL//////////////v///f7+//7+/v//////7fn2////////////tOnb/6Ll0v+v6Nj/jeDI/zXK + o/8IxJD/BMqS/xaqgv8lh2//////AABeO+8AgVf/AL1//wDBif/R9uv/1PPq/8Tv5P+36t3/rujY/6Lk + 0v+U4cv/jt7J//j8+///////+/38//f8+//2+/r/+Pz7//3+/v/m9/P/9Pv6//D6+P9/28L/jd7J/5jj + z/+h5dL/qOvX/4Hmyf8f1J//E596/yOJcO////8AAGA8vwB3U/8p06P/hufM/8Ty5f/D7+T/s+vb/6bm + 1P+c4c//j9/K/4vcyP/t+fb///7///j8/P/0+/r/8vr5//P7+f/1+/r/+/39///////i9fL/ZNO1/3HW + vP992sH/htzG/4vhyv+S5dD/mO7W/6X74v80noT/Io90v////wAAZkCAAHla/33ny/945cb/nunV/7Xr + 3v+l5tT/luDN/4ndxv992cL/1vLq//v9/P/1+/n/8vv4//L69//z+/j/9Pv5/7Xo2//x+vn///////// + //+y59n/aNS3/3LWvP932r//fNzD/4Ljyf+J7ND/l/bd/yORdf8knH6A////AABuRzAAdlT/Xc6x/23o + xv9s4MH/qurZ/5jiz/+I3cb/edjA/8ju5f/3/Pv/8vv4//H6+P/y+/j/6/f0/7np3v/7/fz//v7+/6fk + 1f+56tz///////////9h0bT/aNW4/23Wu/9v3L//dOLG/37w0f9m1rn/Hpt8/ymujTD///8AAAAAAACD + X98po4X/Z+7K/1vgvP+A4sf/jOHK/3rZwv+r59f/9Pv6/+/69//v+vf/8vr4/9fy6/9n0rf/VM6t/6Di + 0v/N7+f/adO4/1PMrf9t1Lr/i9zI/1/Rs/9h0rX/ZNe4/2bbvf9s5sb/ePfV/z2ylf8lrozfAAAAAP// + /wAAAAAAAJNsYAWQbf9U1rP/Vee//0rYsf993sb/pebV//P7+v/s+Pb/6/f1/+749v+s5tj/Vc2u/1jP + r/9ZzrD/btW5/1bOr/9Wza//Vs6v/1fOr/9Z0LD/WdCy/1vTtP9d1rX/Xt+8/2btyP9k4L//IaaF/y7D + nmAAAAAA////AAAAAAAAAAAAD6J9zyCjgv9S68L/P9+0/2Pevv/5////7/v6/+v59//j9/L/gtvF/1PN + r/9Wz7D/Wc+x/1nQsf9Zz7H/WM6w/1fPsP9UzrD/VM+w/1TPrv9U0a//U9Oy/1Tatv9Z5sD/Y/LL/zSx + lP8qupbPAAAAAAAAAAD///8AAAAAAAAAAAAYto4wGaeE/y23lP8+5rn/6/////j////w//3/ve/i/2bV + uP9Tzq7/Vc+v/1jPsP9Z0LL/WM+w/1fOsf9Wz7D/Us2w/1HOrf9Qzq3/T9Cu/0zSr/9M2LP/TeC5/1bt + xP9HxaX/KLKQ/zTPqDAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAkvpdgG6iF/y++m//e/////P///3rl + yf9G0K3/VdKy/1bPsf9Wz7H/Vs6w/1bPsP9Sza//Ucyu/0/Nrf9NzKz/S82s/0fOrP9G0a7/QdWv/0Le + tP9I6L7/Q8Ok/yitjP8yyKJgAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAmwJlgG6iF/yK3 + kP8k3q7/H9el/x7Pn/8tzKT/Q9Cs/1HQsP9Q0K7/TM6u/0nMrf9Hzaz/RMyp/0LNqf8+zqn/ONGo/zTV + qf833rD/O+S4/zvCof8orIv/MMSfYAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAA + AAAkvpdgG6iE/xukgv8gy53/HNql/xzRn/8czJz/HcmZ/yXJnP8qyp7/Lcqg/yzLn/8nypz/JMqc/yTO + n/8l1KT/KN2r/y3Tpv8nq4n/JaqJ/yzAm2AAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAato8wFKN/zxCScv8RnHn/DbqM/wjIlP8GyZT/BsaS/wbFkf8GxZH/B8WR/wfH + k/8IypX/DMmV/xG3jP8WoX3/Fph2/xqkgs8ft5EwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA////AAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAJVvYACGZM8Aelr/AHlZ/wCFX/8AiWL/AJlr/wCb + bP8AlGf/AI5k/wB/W/8AeFj/AHtb/wCHZd8ClXBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAABwSzAAaESAAGI/vwBf + Pd8AXTz/AF08/wBdPP8AXz3fAGJAvwBoRIAAcUswAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// + /wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// + /wD///8A////AP///wD///8A/+AD//+AAP/+AAA//AAAH/gAAA/wAAAH4AAAA+AAAAPAAAABwAAAAYAA + AACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAwAAAAcAAAAHgAAAD4AAAA/AA + AAf4AAAP/AAAH/4AAD//gAD//+AD//////8= + + + \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/temp.Designer.cs b/Tools/ArdupilotMegaPlanner/temp.Designer.cs index 734a6833ad..d09cc97373 100644 --- a/Tools/ArdupilotMegaPlanner/temp.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/temp.Designer.cs @@ -45,6 +45,7 @@ this.BUT_clearcustommaps = new ArdupilotMega.MyButton(); this.BUT_lang_edit = new ArdupilotMega.MyButton(); this.BUT_georefimage = new ArdupilotMega.MyButton(); + this.BUT_follow_me = new ArdupilotMega.MyButton(); this.SuspendLayout(); // // button1 @@ -212,11 +213,22 @@ this.BUT_georefimage.Text = "Geo ref images"; this.BUT_georefimage.Click += new System.EventHandler(this.BUT_georefimage_Click); // + // BUT_follow_me + // + this.BUT_follow_me.Location = new System.Drawing.Point(525, 164); + this.BUT_follow_me.Name = "BUT_follow_me"; + this.BUT_follow_me.Size = new System.Drawing.Size(75, 23); + this.BUT_follow_me.TabIndex = 17; + this.BUT_follow_me.Text = "Follow Me"; + this.BUT_follow_me.UseVisualStyleBackColor = true; + this.BUT_follow_me.Click += new System.EventHandler(this.BUT_follow_me_Click); + // // temp // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(731, 281); + this.Controls.Add(this.BUT_follow_me); this.Controls.Add(this.BUT_georefimage); this.Controls.Add(this.BUT_lang_edit); this.Controls.Add(this.BUT_clearcustommaps); @@ -261,6 +273,7 @@ private MyButton BUT_clearcustommaps; private MyButton BUT_lang_edit; private MyButton BUT_georefimage; + private MyButton BUT_follow_me; //private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1; } diff --git a/Tools/ArdupilotMegaPlanner/temp.cs b/Tools/ArdupilotMegaPlanner/temp.cs index c59d018d59..394ab8b1d5 100644 --- a/Tools/ArdupilotMegaPlanner/temp.cs +++ b/Tools/ArdupilotMegaPlanner/temp.cs @@ -871,5 +871,12 @@ namespace ArdupilotMega { new georefimage().Show(); } + + private void BUT_follow_me_Click(object sender, EventArgs e) + { + SerialInput si = new SerialInput(); + MainV2.fixtheme((Form)si); + si.Show(); + } } } diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 21f0b705c5..bfe1930485 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -519,10 +519,10 @@ def fly_ArduCopter(viewerip=None): print("land failed") failed = True - print("# disarm motors") - if not disarm_motors(mavproxy, mav): - print("disarm_motors failed") - failed = True + #print("# disarm motors") + #if not disarm_motors(mavproxy, mav): + # print("disarm_motors failed") + # failed = True except pexpect.TIMEOUT, e: failed = True diff --git a/Tools/autotest/mission2.txt b/Tools/autotest/mission2.txt index e921ac757c..7fc51ab508 100644 --- a/Tools/autotest/mission2.txt +++ b/Tools/autotest/mission2.txt @@ -23,7 +23,7 @@ QGC WPL 110 # MAV_CMD_NAV_LOITER_TURNS # Turns lat lon alt continue -6 0 3 18 2 0 0 0 0 0 20 1 +#6 0 3 18 2 0 0 0 0 0 20 1 # MAV_CMD_DO_SET_ROI, MAV_ROI_WPNEXT = 1 # MAV_ROI WP index ROI index lat lon alt continue diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp new file mode 100755 index 0000000000..a50f10d6a0 --- /dev/null +++ b/libraries/AC_PID/AC_PID.cpp @@ -0,0 +1,118 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.cpp +/// @brief Generic PID algorithm + +#include +#include "AC_PID.h" + + +int32_t AC_PID::get_p(int32_t error) +{ + return (float)error * _kp; +} + +int32_t AC_PID::get_i(int32_t error, float dt) +{ + if((_ki != 0) && (dt != 0)){ + _integrator += ((float)error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + } + return _integrator; +} + +int32_t AC_PID::get_d(int32_t input, float dt) +{ + if ((_kd != 0) && (dt != 0)) { + _derivative = (input - _last_input) / dt; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + _derivative = _last_derivative + + (dt / ( _filter + dt)) * (_derivative - _last_derivative); + + // update state + _last_input = input; + _last_derivative = _derivative; + + // add in derivative component + return _kd * _derivative; + } +} + +int32_t AC_PID::get_pi(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt); +} + + +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt) + get_d(error, dt); +} + + + +/* +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + // Compute proportional component + _output = error * _kp; + + // Compute derivative component if time has elapsed + if ((fabs(_kd) > 0) && (dt > 0)) { + + _derivative = (error - _last_input) / dt; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + _derivative = _last_derivative + + (dt / ( _filter + dt)) * (_derivative - _last_derivative); + + // update state + _last_input = error; + _last_derivative = _derivative; + + // add in derivative component + _output += _kd * _derivative; + } + + // Compute integral component if time has elapsed + if ((fabs(_ki) > 0) && (dt > 0)) { + _integrator += (error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + _output += _integrator; + } + + return _output; +} +*/ + + +void +AC_PID::reset_I() +{ + _integrator = 0; + _last_input = 0; + _last_derivative = 0; +} + +void +AC_PID::load_gains() +{ + _group.load(); +} + +void +AC_PID::save_gains() +{ + _group.save(); +} diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h new file mode 100755 index 0000000000..f9b4e359de --- /dev/null +++ b/libraries/AC_PID/AC_PID.h @@ -0,0 +1,152 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef AC_PID_h +#define AC_PID_h + +#include +#include // for fabs() + +/// @class AC_PID +/// @brief Object managing one PID control +class AC_PID { +public: + + /// Constructor for PID that saves its settings to EEPROM + /// + /// @note PIDs must be named to avoid either multiple parameters with the + /// same name, or an overly complex constructor. + /// + /// @param key Storage key assigned to this PID. Should be unique. + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(AP_Var::Key key, + const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(key, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + // no need for explicit load, assuming that the main code uses AP_Var::load_all. + } + + /// Constructor for PID that does not save its settings. + /// + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(AP_Var::k_key_none, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + } + + /// Iterate the PID, return the new control value + /// + /// Positive error produces positive output. + /// + /// @param error The measured error value + /// @param dt The time delta in milliseconds (note + /// that update interval cannot be more + /// than 65.535 seconds due to limited range + /// of the data type). + /// @param scaler An arbitrary scale factor + /// + /// @returns The updated control output. + /// + int32_t get_pid(int32_t error, float dt); + int32_t get_pi(int32_t error, float dt); + int32_t get_p(int32_t error); + int32_t get_i(int32_t error, float dt); + int32_t get_d(int32_t error, float dt); + + + /// Reset the PID integrator + /// + void reset_I(); + + /// Load gain properties + /// + void load_gains(); + + /// Save gain properties + /// + void save_gains(); + + /// @name parameter accessors + //@{ + + /// Overload the function call operator to permit relatively easy initialisation + void operator() (const float p, + const float i, + const float d, + const int16_t imaxval) { + _kp = p; _ki = i; _kd = d; _imax = imaxval; + } + + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + float kD() const { return _kd.get(); } + int16_t imax() const { return _imax.get(); } + + void kP(const float v) { _kp.set(v); } + void kI(const float v) { _ki.set(v); } + void kD(const float v) { _kd.set(v); } + void imax(const int16_t v) { _imax.set(abs(v)); } + + float get_integrator() const { return _integrator; } + +private: + AP_Var_group _group; + AP_Float16 _kp; + AP_Float16 _ki; + AP_Float16 _kd; + AP_Int16 _imax; + + float _integrator; ///< integrator value + int32_t _last_input; ///< last input for derivative + float _last_derivative; ///< last derivative for low-pass filter + float _output; + float _derivative; + + /// Low pass filter cut frequency for derivative calculation. + /// + static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )"; + // Examples for _filter: + // f_cut = 10 Hz -> _filter = 15.9155e-3 + // f_cut = 15 Hz -> _filter = 10.6103e-3 + // f_cut = 20 Hz -> _filter = 7.9577e-3 + // f_cut = 25 Hz -> _filter = 6.3662e-3 + // f_cut = 30 Hz -> _filter = 5.3052e-3 +}; + +#endif diff --git a/libraries/AC_PID/keywords.txt b/libraries/AC_PID/keywords.txt new file mode 100755 index 0000000000..68e861fd00 --- /dev/null +++ b/libraries/AC_PID/keywords.txt @@ -0,0 +1,9 @@ +PID KEYWORD1 +get_pid KEYWORD2 +reset_I KEYWORD2 +kP KEYWORD2 +kD KEYWORD2 +kI KEYWORD2 +imax KEYWORD2 +load_gains KEYWORD2 +save_gains KEYWORD2 diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index cf1b051f25..89f71bd3d6 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -12,7 +12,7 @@ FastSerialPort0(Serial); // FTDI/console Arduino_Mega_ISR_Registry isr_registry; -AP_PeriodicProcess adc_scheduler; +AP_TimerProcess adc_scheduler; unsigned long timer; diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde index a1ff95ac38..4b963bc2df 100644 --- a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde @@ -1,8 +1,8 @@ #include +#include #include #include // ArduPilot Mega Vector/Matrix math Library -#include #include #include #include diff --git a/libraries/AP_PID/examples/AP_pid/AP_pid.pde b/libraries/AP_PID/examples/AP_pid/AP_pid.pde index ca46f3d6db..5d7cf59df4 100644 --- a/libraries/AP_PID/examples/AP_pid/AP_pid.pde +++ b/libraries/AP_PID/examples/AP_pid/AP_pid.pde @@ -3,21 +3,26 @@ 2010 Code by Jason Short. DIYDrones.com */ +#include #include #include #include // ArduPilot RC Library -#include // ArduPilot Mega RC Library +#include // ArduPilot Mega RC Library long radio_in; long radio_trim; -PID pid(); +Arduino_Mega_ISR_Registry isr_registry; +AP_PID pid; +APM_RC_APM1 APM_RC; void setup() { - Serial.begin(38400); - Serial.println("ArduPilot Mega PID library test"); - APM_RC.Init(); // APM Radio initialization + Serial.begin(115200); + Serial.println("ArduPilot Mega AP_PID library test"); + + isr_registry.init(); + APM_RC.Init(&isr_registry); // APM Radio initialization delay(1000); //rc.trim(); @@ -27,12 +32,10 @@ void setup() pid.kI(0); pid.kD(0.5); pid.imax(50); - pid.save_gains(); pid.kP(0); pid.kI(0); pid.kD(0); pid.imax(0); - pid.load_gains(); Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); } diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp index eea2e91b33..af9365dd84 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.cpp +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -88,11 +88,14 @@ AP_RC_Channel::set_pwm(int pwm) control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; + /* + // coming soon ?? if(expo) { long temp = control_in; temp = (temp * temp) / (long)_high; control_in = (int)((control_in >= 0) ? temp : -temp); } + */ } } diff --git a/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde index f636c37aa1..a7de6e0fd4 100644 --- a/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde +++ b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde @@ -29,7 +29,6 @@ AP_RC rc; void setup() { Serial.begin(115200); - //Serial.begin(38400); Serial.println("ArduPilot RC Channel test"); rc.init(); // APM Radio initialization diff --git a/libraries/FastSerial/examples/FastSerial/FastSerial.pde b/libraries/FastSerial/examples/FastSerial/FastSerial.pde index 4a9ae94b51..a1af765233 100644 --- a/libraries/FastSerial/examples/FastSerial/FastSerial.pde +++ b/libraries/FastSerial/examples/FastSerial/FastSerial.pde @@ -17,8 +17,9 @@ #undef PROGMEM #define PROGMEM __attribute__(( section(".progmem.data") )) -#undef PSTR -#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) +# undef PSTR +# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \ + (prog_char_t *)&__c[0];})) // // Create a FastSerial driver that looks just like the stock Arduino @@ -38,7 +39,7 @@ void setup(void) // // Set the speed for our replacement serial port. // - Serial.begin(38400); + Serial.begin(115200); // // Test printing things diff --git a/libraries/GPS_IMU/GPS_IMU.cpp b/libraries/GPS_IMU/GPS_IMU.cpp index e6975ccdc2..62ef39b968 100755 --- a/libraries/GPS_IMU/GPS_IMU.cpp +++ b/libraries/GPS_IMU/GPS_IMU.cpp @@ -4,7 +4,11 @@ #include "GPS_IMU.h" #include -#include "WProgram.h" +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif // Constructors ////////////////////////////////////////////////////////////////