From e8db2225dd3e50b18a310a4e809ff4654a00f5e1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 31 Dec 2011 10:05:13 -0800 Subject: [PATCH 1/5] upped to r5 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7b26e8d9bf..97980416bd 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.1.1r4 alpha" +#define THISFIRMWARE "ArduCopter V2.1.1r5 alpha" /* ArduCopter Version 2.0 Beta Authors: Jason Short From bfd7608f4d00a05ec49443873c2eff25c4fd3f4c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 31 Dec 2011 10:10:45 -0800 Subject: [PATCH 2/5] Lowered Yaw to prevent overshoot based on feedback --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 6f15346fd4..6b73aa4219 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -582,7 +582,7 @@ // YAW Control // #ifndef STABILIZE_YAW_P -# define STABILIZE_YAW_P 9.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy +# 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 From 86e01fce17a99f45a3b6517f3e91dc900a092baa Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 31 Dec 2011 10:54:31 -0800 Subject: [PATCH 3/5] Cosmetic --- ArduCopter/ArduCopter.pde | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 97980416bd..36eea67d0e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -744,7 +744,6 @@ static void medium_loop() // ------------------- g_gps->new_data = false; - // calculate the copter's desired bearing and WP distance // ------------------------------------------------------ if(navigate()){ @@ -1060,11 +1059,13 @@ static void update_GPS(void) // --------------- gps_fix_count++; - // we are not tracking I term on navigation, so this isn't needed + // used to calculate speed in X and Y, iterms + // ------------------------------------------ dTnav = (float)(millis() - nav_loopTimer)/ 1000.0; nav_loopTimer = millis(); // prevent runup from bad GPS + // -------------------------- dTnav = min(dTnav, 1.0); if(ground_start_count > 1){ @@ -1420,6 +1421,12 @@ static void update_navigation() update_nav_wp(); break; + case STABILIZE: + //wp_control = NO_NAV_MODE; + //update_nav_wp(); + + break; + } // are we in SIMPLE mode? @@ -1767,6 +1774,10 @@ static void update_nav_wp() }else if(wp_control == NO_NAV_MODE){ nav_roll = 0; nav_pitch = 0; + + // calc the Iterms for Loiter based on velicity + //calc_position_hold(); + } } From 24bddb1c476bf7de1c8b2f495b4fdd647702e0a0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 31 Dec 2011 10:59:59 -0800 Subject: [PATCH 4/5] Added more constraints around velocity --- ArduCopter/navigation.pde | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 4065aac72b..93f6f9d9c7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -94,6 +94,17 @@ 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_position_hold() +{ + // only if we are not moving + //if (x_actual_speed == 0){ + // // what is the error? + // int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); + //} +} + + #define NAV_ERR_MAX 800 static void calc_loiter(int x_error, int y_error) { @@ -102,20 +113,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; - x_rate_error = constrain(x_rate_error, -1000, 1000); nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); - nav_lon_p = constrain(nav_lon_p, -3500, 3500); + 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; // 413 - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + y_rate_error = y_target_speed - y_actual_speed; nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); - nav_lat_p = constrain(nav_lat_p, -3500, 3500); + nav_lat_p = constrain(nav_lat_p, -1200, 1200); nav_lat = nav_lat_p + y_iterm; + nav_lat = constrain(nav_lat, -2500, 2500); /* int8_t ttt = 1.0/dTnav; @@ -232,23 +243,23 @@ static void calc_nav_rate(int max_speed) //x_actual_speed = -sin(temp) * (float)g_gps->ground_speed; x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 - x_rate_error = constrain(x_rate_error, -1400, 1400); + 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_p = constrain(nav_lon_p, -3500, 3500); nav_lon = nav_lon_p + x_iterm; + nav_lon = constrain(nav_lon, -3000, 3000); // heading towards target //y_actual_speed = cos(temp) * (float)g_gps->ground_speed; y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1400, 1400); // added a rate error limit to keep pitching down to a minimum + 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_p = constrain(nav_lat_p, -3500, 3500); 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", From 35fba5d4a3743b01ffa9dcf0df32597ce1c1d47e Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sat, 31 Dec 2011 15:44:40 -0700 Subject: [PATCH 5/5] Tweak to FBW min altitude Patch from Yury --- ArduPlane/ArduPlane.pde | 2 +- ArduPlane/Parameters.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f81c1efe4e..6b03a94f3e 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -905,7 +905,7 @@ static void update_current_flight_mode(void) nav_roll = g.channel_roll.norm_input() * g.roll_limit; altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; - if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) { + if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == 0)) { altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min; } else { if (g.channel_pitch.norm_input()<0) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index cdb3ea1217..9277351bf9 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -85,7 +85,7 @@ public: // k_param_flybywire_airspeed_min = 120, k_param_flybywire_airspeed_max, - k_param_FBWB_min_altitude, // -1=disabled, minimum value for altitude in cm (default 30m) + k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm) // // 130: Sensor parameters