From 3b622dc43cb6b3dde3d5465c1ad69c29dbebd1a5 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Wed, 7 Sep 2011 16:43:06 +0000 Subject: [PATCH] Reset_hold_I no longer clears throttle_I value for return to alt hold Removed Baro Init in Auto Mode. increased Rate Error limit on Alt Hold Slightly decreased alt P to reduce pulsing motors Added rate limit for nav so we can reduce initial pitch to target. Using MINIMUM_THROTTLE value to test for Reseting Alt Hold now. Fixed extra Log formatting statement in CTUN. git-svn-id: https://arducopter.googlecode.com/svn/trunk@3294 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/Attitude.pde | 7 ++----- ArduCopterMega/Log.pde | 2 +- ArduCopterMega/commands.pde | 7 ------- ArduCopterMega/config.h | 4 ++-- ArduCopterMega/navigation.pde | 2 ++ ArduCopterMega/system.pde | 3 ++- 6 files changed, 9 insertions(+), 16 deletions(-) diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index ef835f8bc1..d122d2a8aa 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -99,7 +99,7 @@ get_nav_throttle(long z_error, int target_speed) target_speed = z_error * scaler; rate_error = target_speed - altitude_rate; - rate_error = constrain(rate_error, -90, 90); + rate_error = constrain(rate_error, -110, 110); throttle = g.pi_throttle.get_pi(rate_error, delta_ms_medium_loop); return g.throttle_cruise + rate_error; @@ -151,9 +151,7 @@ static void reset_hold_I(void) { g.pi_loiter_lat.reset_I(); g.pi_loiter_lat.reset_I(); - g.pi_crosstrack.reset_I(); - g.pi_throttle.reset_I(); } // Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc. @@ -174,7 +172,6 @@ throttle control static int get_throttle(int throttle_input) { throttle_input = (float)throttle_input * angle_boost(); - //throttle_input = max(throttle_slew, throttle_input); return max(throttle_input, 0); } @@ -216,7 +213,7 @@ static int alt_hold_velocity() static float angle_boost() { float temp = cos_pitch_x * cos_roll_x; - temp = 2.0 - constrain(temp, .5, 1.0); + temp = 2.0 - constrain(temp, .6, 1.0); return temp; } diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index d1c43af261..d67fabf2a8 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -656,7 +656,6 @@ static void Log_Read_Control_Tuning() DataFlash.ReadInt(), DataFlash.ReadInt(), - DataFlash.ReadInt(), DataFlash.ReadInt(), DataFlash.ReadInt()); } @@ -706,6 +705,7 @@ static void Log_Read_Performance() DataFlash.ReadByte(), DataFlash.ReadByte(), + DataFlash.ReadByte(), DataFlash.ReadByte(), DataFlash.ReadByte(), diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 1d031792fd..10d5fb31f3 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -22,13 +22,6 @@ static void clear_command_queue(){ static void init_auto() { - #if HIL_MODE != HIL_MODE_ATTITUDE - // read Baro pressure at ground - - // this resets Baro for more accuracy - //----------------------------------- - init_barometer(); - #endif - //if (g.waypoint_index == g.waypoint_total) { // Serial.println("ia_f"); // do_RTL(); diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 6ef9f412b5..664ab801c9 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -513,10 +513,10 @@ // Throttle control gains // #ifndef THROTTLE_P -# define THROTTLE_P 0.4 // trying a lower val +# define THROTTLE_P 0.35 // #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.10 //with 4m error, 12.5s windup +# define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif //#ifndef THROTTLE_D //# define THROTTLE_D 0.6 // upped with filter diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index b2678cdb69..116f311d6b 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -104,6 +104,7 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed // calc the cos of the error to tell how fast we are moving towards the target in cm y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); y_rate_error = y_target_speed - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); @@ -111,6 +112,7 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed // calc the sin of the error to tell how fast we are moving laterally to the target in cm x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); x_rate_error = x_target_speed - x_actual_speed; + x_rate_error = constrain(x_rate_error, -250, 250); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 44fde00483..d9ce317d9d 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -392,6 +392,7 @@ static void set_mode(byte mode) yaw_mode = ALT_HOLD_YAW; roll_pitch_mode = ALT_HOLD_RP; throttle_mode = ALT_HOLD_THR; + reset_hold_I(); init_throttle_cruise(); next_WP.alt = current_loc.alt; @@ -538,7 +539,7 @@ static void init_throttle_cruise() { // are we moving from manual throttle to auto_throttle? - if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > 150)){ + if((old_control_mode <= SIMPLE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); }