From c209d6e6dd8a785289ed69178cee21498e1b271d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 27 Sep 2011 09:35:05 -0700 Subject: [PATCH] 2.0.46 -Not flight tested! restored 42 alt hold code removed throttle timer, replaced with safer constant increased alt control range reformatted nav_rate calls removed unused simple mode defines set alt hold home to 10m tuned down rateP to .13 from .14 for broader application. --- ArduCopter/ArduCopter.pde | 44 ++++++++++++------------------------ ArduCopter/Attitude.pde | 20 +++++++--------- ArduCopter/config.h | 33 ++++++++------------------- ArduCopter/control_modes.pde | 3 --- ArduCopter/navigation.pde | 29 ++++-------------------- 5 files changed, 37 insertions(+), 92 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f574099119..ea37521b70 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.0.45 Beta" +#define THISFIRMWARE "ArduCopter V2.0.46 Beta" /* ArduCopter Version 2.0 Beta Authors: Jason Short @@ -129,6 +129,9 @@ static AP_Int8 *flight_modes = &g.flight_mode1; APM_BMP085_Class barometer; AP_Compass_HMC5843 compass(Parameters::k_param_compass); + #ifdef OPTFLOW_ENABLED + AP_OpticalFlow_ADNS3080 optflow; + #endif // real GPS selection #if GPS_PROTOCOL == GPS_PROTOCOL_AUTO @@ -200,10 +203,6 @@ static AP_Int8 *flight_modes = &g.flight_mode1; #endif // normal dcm AP_DCM dcm(&imu, g_gps); - - #ifdef OPTFLOW_ENABLED - AP_OpticalFlow_ADNS3080 optflow; - #endif #endif //////////////////////////////////////////////////////////////////////////////// @@ -483,7 +482,6 @@ static byte gps_watchdog; // -------------- static unsigned long fast_loopTimer; // Time in miliseconds of main control loop static byte medium_loopCounter; // Counters for branching from main control loop to slower loops -static unsigned long throttle_timer; static unsigned long fiftyhz_loopTimer; @@ -585,6 +583,9 @@ static void fast_loop() // write out the servo PWM values // ------------------------------ set_servos_4(); + + //if(motor_armed) + // Log_Write_Attitude(); } static void medium_loop() @@ -950,19 +951,10 @@ static void update_GPS(void) // so that the altitude is more accurate // ------------------------------------- if (current_loc.lat == 0) { - //SendDebugln("!! bad loc"); ground_start_count = 5; }else{ - //Serial.printf("init Home!"); - - // reset our nav loop timer - //nav_loopTimer = millis(); init_home(); - - // init altitude - // commented out because we aren't using absolute altitude - // current_loc.alt = home.alt; ground_start_count = 0; } } @@ -1048,10 +1040,7 @@ void update_roll_pitch_mode(void) switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: - // Roll control g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); - - // Pitch control g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); break; @@ -1070,7 +1059,6 @@ void update_roll_pitch_mode(void) } } - // 50 hz update rate, not 250 void update_throttle_mode(void) { @@ -1084,8 +1072,6 @@ void update_throttle_mode(void) g.pi_rate_pitch.reset_I(); g.rc_3.servo_out = 0; } - // reset the timer to throttle so that we never get fast I term run-ups - throttle_timer = 0; break; case THROTTLE_HOLD: @@ -1276,11 +1262,11 @@ adjust_altitude() { if(g.rc_3.control_in <= 200){ next_WP.alt -= 1; // 1 meter per second - next_WP.alt = max(next_WP.alt, (current_loc.alt - 400)); // don't go less than 4 meters below current location + next_WP.alt = max(next_WP.alt, (current_loc.alt - 500)); // don't go less than 4 meters below current location next_WP.alt = max(next_WP.alt, 100); // don't go less than 1 meter }else if (g.rc_3.control_in > 700){ next_WP.alt += 1; // 1 meter per second - next_WP.alt = min(next_WP.alt, (current_loc.alt + 400)); // don't go more than 4 meters below current location + next_WP.alt = min(next_WP.alt, (current_loc.alt + 500)); // don't go more than 4 meters below current location } } @@ -1360,10 +1346,10 @@ static void update_nav_wp() calc_location_error(&next_WP); // use error as the desired rate towards the target - calc_nav_rate(long_error, lat_error, g.waypoint_speed_max, 0); + calc_loiter(long_error, lat_error); // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); + calc_loiter_pitch_roll(); }else if(wp_control == CIRCLE_MODE){ @@ -1393,18 +1379,18 @@ static void update_nav_wp() // use error as the desired rate towards the target // nav_lon, nav_lat is calculated - calc_nav_rate(long_error, lat_error, 200, 0); + calc_loiter(long_error, lat_error); // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll(); + calc_loiter_pitch_roll(); } else { // for long journey's reset the wind resopnse // it assumes we are standing still. // use error as the desired rate towards the target - calc_nav_rate2(g.waypoint_speed_max); + calc_nav_rate(g.waypoint_speed_max); // rotate pitch and roll to the copter frame of reference - calc_nav_pitch_roll2(); + calc_nav_pitch_roll(); } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index c834a487d4..02b3ab7205 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -85,7 +85,7 @@ get_stabilize_yaw(long target_angle) return (int)constrain(rate, -2500, 2500); } -#define ALT_ERROR_MAX 350 +#define ALT_ERROR_MAX 300 static int get_nav_throttle(long z_error, int target_speed) { @@ -95,9 +95,8 @@ get_nav_throttle(long z_error, int target_speed) // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); target_speed = z_error * scaler; - rate_error = target_speed - altitude_rate; - rate_error = constrain(rate_error, -120, 140); + rate_error = constrain(rate_error, -110, 110); return (int)g.pi_throttle.get_pi(rate_error, .1); } @@ -106,10 +105,9 @@ static int get_rate_roll(long target_rate) { long error; - target_rate = constrain(target_rate, -2500, 2500); - - error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); - target_rate = g.pi_rate_roll.get_pi(error, G_Dt); + target_rate = constrain(target_rate, -2500, 2500); + error = (target_rate * 4.5) - (long)(degrees(omega.x) * 100.0); + target_rate = g.pi_rate_roll.get_pi(error, G_Dt); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -119,10 +117,9 @@ static int get_rate_pitch(long target_rate) { long error; - target_rate = constrain(target_rate, -2500, 2500); - - error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); - target_rate = g.pi_rate_pitch.get_pi(error, G_Dt); + target_rate = constrain(target_rate, -2500, 2500); + error = (target_rate * 4.5) - (long)(degrees(omega.y) * 100.0); + target_rate = g.pi_rate_pitch.get_pi(error, G_Dt); // output control: return (int)constrain(target_rate, -2500, 2500); @@ -132,7 +129,6 @@ static int get_rate_yaw(long target_rate) { long error; - error = (target_rate * 4.5) - (long)(degrees(omega.z) * 100.0); target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e550721548..59e5aa7815 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -221,8 +221,6 @@ //#define OPTFLOW_ENABLED #endif -//#define OPTFLOW_ENABLED - #ifndef OPTFLOW // sets global enabled/disabled flag for optflow (as seen in CLI) # define OPTFLOW DISABLED #endif @@ -317,19 +315,6 @@ // Attitude Control // -// SIMPLE Mode -#ifndef SIMPLE_YAW -# define SIMPLE_YAW YAW_HOLD -#endif - -#ifndef SIMPLE_RP -# define SIMPLE_RP ROLL_PITCH_STABLE -#endif - -#ifndef SIMPLE_THR -# define SIMPLE_THR THROTTLE_MANUAL -#endif - // Alt Hold Mode #ifndef ALT_HOLD_YAW # define ALT_HOLD_YAW YAW_HOLD @@ -403,40 +388,40 @@ // Attitude Control // #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.2 +# define STABILIZE_ROLL_P 4.0 #endif #ifndef STABILIZE_ROLL_I # define STABILIZE_ROLL_I 0.001 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX 0 // degrees +# define STABILIZE_ROLL_IMAX 1.5 // degrees #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.2 +# define STABILIZE_PITCH_P 4.0 #endif #ifndef STABILIZE_PITCH_I # define STABILIZE_PITCH_I 0.001 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX 0 // degrees +# define STABILIZE_PITCH_IMAX 1.5 // degrees #endif ////////////////////////////////////////////////////////////////////////////// // Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.14 +# define RATE_ROLL_P .13 #endif #ifndef RATE_ROLL_I -# define RATE_ROLL_I 0 //0.18 +# define RATE_ROLL_I 0.0 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 15 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.14 +# define RATE_PITCH_P 0.13 #endif #ifndef RATE_PITCH_I # define RATE_PITCH_I 0 //0.18 @@ -519,7 +504,7 @@ # define THROTTLE_P 0.6 // #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.02 // with 4m error, 8 PWM gain/s +# define THROTTLE_I 0.10 // with 4m error, 12.5s windup #endif #ifndef THROTTLE_IMAX # define THROTTLE_IMAX 300 @@ -671,7 +656,7 @@ #endif #ifndef ALT_HOLD_HOME -# define ALT_HOLD_HOME -1 +# define ALT_HOLD_HOME 10 #endif #ifndef USE_CURRENT_ALT diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 8d958b476e..af83f046a0 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -18,10 +18,7 @@ static void read_control_switch() // setup Simple mode // do we enable simple mode? do_simple = (g.simple_modes & (1 << switchPosition)); - //Serial.printf("do simple: %d \n", (int)do_simple); #endif - - }else{ switch_debouncer = true; } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index bc5c35e2e5..766a23a7d5 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -59,35 +59,16 @@ static void calc_location_error(struct Location *next_loc) // nav_roll = g.pid_of_roll.get_pid(-optflow.x_cm * 10, dTnav, 1.0); #define NAV_ERR_MAX 400 -static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed) +static void calc_loiter(int x_error, int y_error) { // moved to globals for logging - //int x_actual_speed, y_actual_speed; - //int x_rate_error, y_rate_error; + x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - float scaler = (float)max_speed/(float)NAV_ERR_MAX; - g.pi_loiter_lat.kP(scaler); - g.pi_loiter_lon.kP(scaler); - int x_target_speed = g.pi_loiter_lon.get_pi(x_error, dTnav); int y_target_speed = g.pi_loiter_lat.get_pi(y_error, dTnav); - //Serial.printf("scaler: %1.3f, y_target_speed %d",scaler,y_target_speed); - - if(x_target_speed > 0){ - x_target_speed = max(x_target_speed, min_speed); - }else{ - x_target_speed = min(x_target_speed, -min_speed); - } - - if(y_target_speed > 0){ - y_target_speed = max(y_target_speed, min_speed); - }else{ - y_target_speed = min(y_target_speed, -min_speed); - } - // find the rates: float temp = radians((float)g_gps->ground_course/100.0); @@ -116,7 +97,7 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } -static void calc_nav_rate2(int max_speed) +static void calc_nav_rate(int max_speed) { /* 0 1 2 3 4 5 6 7 8 @@ -151,7 +132,7 @@ static void calc_nav_rate2(int max_speed) } // nav_roll, nav_pitch -static void calc_nav_pitch_roll2() +static void calc_nav_pitch_roll() { float temp = radians((float)(9000 - (dcm.yaw_sensor - original_target_bearing))/100.0); float _cos_yaw_x = cos(temp); @@ -173,7 +154,7 @@ static void calc_nav_pitch_roll2() // nav_roll, nav_pitch -static void calc_nav_pitch_roll() +static void calc_loiter_pitch_roll() { // rotate the vector nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x;