From 985a4e7f61f274ae0d0191159532121618739622 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 10 Apr 2011 20:31:33 +0000 Subject: [PATCH] =?UTF-8?q?removed=20the=20throttle=20min=20max=20settings?= =?UTF-8?q?=20for=20user=20config.=20turned=20off=20altitude=20interpolati?= =?UTF-8?q?on=20for=20now.=20removed=20the=20"Dampener"=20setting=20in=20c?= =?UTF-8?q?onfig.=20Using=20less=20confusing=20kD=20instead.=20removed=20t?= =?UTF-8?q?hrottle=5Fcruise=20reset=20in=20events.pde=20for=20RTL=20-=20wo?= =?UTF-8?q?uld=20cause=20a=20crash.=20added=20I=20term=20for=20pitch/roll?= =?UTF-8?q?=20for=20when=20flying=20greater=20than=2020=C2=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit git-svn-id: https://arducopter.googlecode.com/svn/trunk@1868 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h.reference | 27 ----------- ArduCopterMega/ArduCopterMega.pde | 16 +++---- ArduCopterMega/Attitude.pde | 18 ++++++-- ArduCopterMega/Parameters.h | 18 ++++---- ArduCopterMega/config.h | 66 ++++++--------------------- ArduCopterMega/events.pde | 2 - ArduCopterMega/setup.pde | 10 ++-- 7 files changed, 48 insertions(+), 109 deletions(-) diff --git a/ArduCopterMega/APM_Config.h.reference b/ArduCopterMega/APM_Config.h.reference index cecd7722bd..60c26760a1 100644 --- a/ArduCopterMega/APM_Config.h.reference +++ b/ArduCopterMega/APM_Config.h.reference @@ -367,33 +367,6 @@ //#define ENABLE_HIL ENABLED // -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -// THROTTLE_MIN OPTIONAL -// -// The minimum throttle setting to which the autopilot will reduce the -// throttle while descending. The default is zero, which is -// suitable for aircraft with a steady power-off glide. Increase this -// value if your aircraft needs throttle to maintain a stable descent in -// level flight. -// -// THROTTLE_CRUISE OPTIONAL -// -// The approximate throttle setting to achieve AIRSPEED_CRUISE in level flight. -// The default is 45%, which is reasonable for a modestly powered aircraft. -// -// THROTTLE_MAX OPTIONAL -// -// The maximum throttle setting the autopilot will apply. The default is 75%. -// Reduce this value if your aicraft is overpowered, or has complex flight -// characteristics at high throttle settings. -// -//#define THROTTLE_MIN 0 -//#define THROTTLE_CRUISE 45 -//#define THROTTLE_MAX 75 -// - ////////////////////////////////////////////////////////////////////////////// // Autopilot control limits // diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index c2046fbf7c..9e1a411fe4 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -966,7 +966,7 @@ void update_current_flight_mode(void) output_manual_throttle(); // apply nav_pitch and nav_roll to output - fbw_nav_mixer(); + simple_mixer(); // perform stabilzation output_stabilize_roll(); @@ -988,8 +988,8 @@ void update_current_flight_mode(void) if(flight_timer >= 2){ flight_timer = 0; - if(g.rc_3.control_in <= 0){ - next_WP.alt -= 1; + if(g.rc_3.control_in <= 200){ + next_WP.alt -= 1; // 1 meter per second next_WP.alt = max(next_WP.alt, 100); }else if (g.rc_3.control_in > 700){ next_WP.alt += 1; @@ -1102,10 +1102,6 @@ void read_AHRS(void) //----------------------------------------------- dcm.update_DCM(G_Dt); omega = dcm.get_gyro(); - - // Testing remove !!! - //dcm.pitch_sensor = 0; - //dcm.roll_sensor = 0; } void update_trig(void){ @@ -1140,7 +1136,7 @@ void update_alt() // decide which sensor we're usings sonar_alt = sonar.read(); - if(baro_alt < 500 && sonar_alt < 600){ + if(baro_alt < 500 && sonar_alt < 600){ // less than 5m or 15 feet altitude_sensor = SONAR; }else{ altitude_sensor = BARO; @@ -1165,10 +1161,10 @@ void update_alt() // altitude smoothing // ------------------ - calc_altitude_smoothing_error(); + //calc_altitude_smoothing_error(); - //calc_altitude_error(); + calc_altitude_error(); // Amount of throttle to apply for hovering // ---------------------------------------- diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index b382b54928..1e741cdeea 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -20,7 +20,7 @@ control_nav_mixer() } void -fbw_nav_mixer() +simple_mixer() { // control +- 45° is mixed with the navigation request by the Autopilot // output is in degrees = target pitch and roll of copter @@ -37,7 +37,12 @@ output_stabilize_roll() error = g.rc_1.servo_out - dcm.roll_sensor; // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); + error = constrain(error, -2500, 2500); + + // only buildup I if we are trying to roll hard + if(abs(g.rc_1.servo_out) < 1500){ + g.pid_stabilize_roll.reset_I(); + } // write out angles back to servo out - this will be converted to PWM by RC_Channel g.rc_1.servo_out = g.pid_stabilize_roll.get_pid(error, delta_ms_fast_loop, 1.0); @@ -58,10 +63,15 @@ output_stabilize_pitch() float error, rate; int dampener; - error = g.rc_2.servo_out - dcm.pitch_sensor; + error = g.rc_2.servo_out - dcm.pitch_sensor; // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); + error = constrain(error, -2500, 2500); + + // only buildup I if we are trying to roll hard + if(abs(g.rc_2.servo_out) < 1500){ + g.pid_stabilize_pitch.reset_I(); + } // write out angles back to servo out - this will be converted to PWM by RC_Channel g.rc_2.servo_out = g.pid_stabilize_pitch.get_pid(error, delta_ms_fast_loop, 1.0); diff --git a/ArduCopterMega/Parameters.h b/ArduCopterMega/Parameters.h index b8891b7f6f..436f05cde4 100644 --- a/ArduCopterMega/Parameters.h +++ b/ArduCopterMega/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 = 3; + static const uint16_t k_format_version = 4; // // Parameter identities. @@ -232,12 +232,12 @@ public: waypoint_radius (WP_RADIUS_DEFAULT, k_param_waypoint_radius, PSTR("WP_RADIUS")), loiter_radius (LOITER_RADIUS_DEFAULT, k_param_loiter_radius, PSTR("LOITER_RADIUS")), - throttle_min (THROTTLE_MIN, k_param_throttle_min, PSTR("THR_MIN")), - throttle_max (THROTTLE_MAX, k_param_throttle_max, PSTR("THR_MAX")), + throttle_min (0, k_param_throttle_min, PSTR("THR_MIN")), + throttle_max (1000, k_param_throttle_max, PSTR("THR_MAX")), throttle_fs_enabled (THROTTLE_FAILSAFE, k_param_throttle_fs_enabled, PSTR("THR_FAILSAFE")), throttle_fs_action (THROTTLE_FAILSAFE_ACTION, k_param_throttle_fs_action, PSTR("THR_FS_ACTION")), throttle_fs_value (THROTTLE_FS_VALUE, k_param_throttle_fs_value, PSTR("THR_FS_VALUE")), - throttle_cruise (THROTTLE_CRUISE, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), + throttle_cruise (100, k_param_throttle_cruise, PSTR("TRIM_THROTTLE")), flight_mode_channel (FLIGHT_MODE_CHANNEL+1, k_param_flight_mode_channel, PSTR("FLT_MODE_CH")), flight_modes (k_param_flight_modes, PSTR("FLIGHT_MODES")), @@ -268,9 +268,9 @@ public: pid_acro_rate_pitch (k_param_pid_acro_rate_pitch, PSTR("ACR_PIT_"), ACRO_RATE_PITCH_P, ACRO_RATE_PITCH_I, ACRO_RATE_PITCH_D, ACRO_RATE_PITCH_IMAX * 100), pid_acro_rate_yaw (k_param_pid_acro_rate_yaw, PSTR("ACR_YAW_"), ACRO_RATE_YAW_P, ACRO_RATE_YAW_I, ACRO_RATE_YAW_D, ACRO_RATE_YAW_IMAX * 100), - pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_D, STABILIZE_ROLL_IMAX * 100), - pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_D, STABILIZE_PITCH_IMAX * 100), - pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, YAW_D, YAW_IMAX * 100), + pid_stabilize_roll (k_param_pid_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, 0, STABILIZE_ROLL_IMAX * 100), + pid_stabilize_pitch (k_param_pid_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, 0, STABILIZE_PITCH_IMAX * 100), + pid_yaw (k_param_pid_yaw, PSTR("STB_YAW_"), YAW_P, YAW_I, 0, 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), @@ -278,8 +278,8 @@ public: pid_baro_throttle (k_param_pid_baro_throttle, PSTR("THR_BAR_"), THROTTLE_BARO_P, THROTTLE_BARO_I, THROTTLE_BARO_D, THROTTLE_BARO_IMAX * 100), pid_sonar_throttle (k_param_pid_sonar_throttle, PSTR("THR_SON_"), THROTTLE_SONAR_P, THROTTLE_SONAR_I, THROTTLE_SONAR_D, THROTTLE_SONAR_IMAX * 100), - stabilize_dampener (STABILIZE_DAMPENER, k_param_stabilize_dampener, PSTR("STB_DAMP")), - hold_yaw_dampener (HOLD_YAW_DAMPENER, k_param_hold_yaw_dampener, PSTR("YAW_DAMP")), + stabilize_dampener (STABILIZE_ROLL_D, k_param_stabilize_dampener, PSTR("STB_DAMP")), + hold_yaw_dampener (YAW_D, k_param_hold_yaw_dampener, PSTR("YAW_DAMP")), junk(0) // XXX just so that we can add things without worrying about the trailing comma { diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index 29efe11b5c..3ee8826546 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -287,7 +287,7 @@ #define ACRO_RATE_PITCH_IMAX_CENTIDEGREE ACRO_RATE_PITCH_IMAX * 100 #ifndef ACRO_RATE_YAW_P -# define ACRO_RATE_YAW_P .1 +# define ACRO_RATE_YAW_P .2 // used to control response in turning #endif #ifndef ACRO_RATE_YAW_I # define ACRO_RATE_YAW_I 0.0 @@ -305,7 +305,6 @@ #endif - ////////////////////////////////////////////////////////////////////////////// // STABILZE Angle Control // @@ -313,32 +312,26 @@ # define STABILIZE_ROLL_P 0.6 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.0 +# define STABILIZE_ROLL_I 0.1 // #endif #ifndef STABILIZE_ROLL_D -# define STABILIZE_ROLL_D 0.0 +# define STABILIZE_ROLL_D 0.135 #endif #ifndef STABILIZE_ROLL_IMAX -# define STABILIZE_ROLL_IMAX 3 +# define STABILIZE_ROLL_IMAX 10 // 10 degrees #endif #ifndef STABILIZE_PITCH_P # define STABILIZE_PITCH_P 0.6 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.0 +# define STABILIZE_PITCH_I 0.1 #endif #ifndef STABILIZE_PITCH_D -# define STABILIZE_PITCH_D 0.0 +# define STABILIZE_PITCH_D 0.135 #endif #ifndef STABILIZE_PITCH_IMAX -# define STABILIZE_PITCH_IMAX 3 -#endif - -// STABILZE RATE Control -// -#ifndef STABILIZE_DAMPENER -# define STABILIZE_DAMPENER 0.135 +# define STABILIZE_PITCH_IMAX 10 #endif @@ -346,44 +339,24 @@ // YAW Control // #ifndef YAW_P -# define YAW_P 0.25 +# define YAW_P 0.35 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif #ifndef YAW_I -# define YAW_I 0.0 +# define YAW_I 0.0 // Always 0 #endif #ifndef YAW_D -# define YAW_D 0.0 +# define YAW_D 0.175 // #endif #ifndef YAW_IMAX -# define YAW_IMAX 1 +# define YAW_IMAX 0 // Always 0 #endif -// STABILZE YAW Control -// -#ifndef HOLD_YAW_DAMPENER -# define HOLD_YAW_DAMPENER 0.175 -#endif - -////////////////////////////////////////////////////////////////////////////// -// Throttle control -// -#ifndef THROTTLE_MIN -# define THROTTLE_MIN 0 -#endif -#ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 250 -#endif -#ifndef THROTTLE_MAX -# define THROTTLE_MAX 1000 -#endif - - ////////////////////////////////////////////////////////////////////////////// // Autopilot control limits // // how much to we pitch towards the target #ifndef PITCH_MAX -# define PITCH_MAX 36 +# define PITCH_MAX 25 // degrees #endif @@ -397,12 +370,11 @@ # define NAV_I 0.1 #endif #ifndef NAV_D -# define NAV_D 0.00 +# define NAV_D 0.00 // should always be 0 #endif #ifndef NAV_IMAX -# define NAV_IMAX 250 +# define NAV_IMAX 250 // 250 degrees #endif -# define NAV_IMAX_CENTIDEGREE NAV_IMAX * 100 ////////////////////////////////////////////////////////////////////////////// @@ -541,16 +513,6 @@ # define USE_CURRENT_ALT FALSE #endif -////////////////////////////////////////////////////////////////////////////// -// Developer Items -// - -#ifndef STANDARD_SPEED -# define STANDARD_SPEED 15.0 -#define STANDARD_SPEED_SQUARED (STANDARD_SPEED * STANDARD_SPEED) -#endif -#define STANDARD_THROTTLE_SQUARED (THROTTLE_CRUISE * THROTTLE_CRUISE) - ////////////////////////////////////////////////////////////////////////////// // RC override diff --git a/ArduCopterMega/events.pde b/ArduCopterMega/events.pde index df9ba84780..3ce33d0406 100644 --- a/ArduCopterMega/events.pde +++ b/ArduCopterMega/events.pde @@ -35,7 +35,6 @@ void failsafe_off_event() // we should already be in RTL and throttle set to cruise // ------------------------------------------------------ set_mode(RTL); - g.throttle_cruise = THROTTLE_CRUISE; } */ } @@ -44,7 +43,6 @@ void low_battery_event(void) { gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); set_mode(RTL); - g.throttle_cruise.set(THROTTLE_CRUISE); } diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 8c846a265f..f02caea240 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -667,9 +667,9 @@ default_flight_modes() void default_throttle() { - g.throttle_min = THROTTLE_MIN; - g.throttle_max = THROTTLE_MAX; - g.throttle_cruise = THROTTLE_CRUISE; + g.throttle_min = 0; + g.throttle_max = 1000; + g.throttle_cruise = 100; g.throttle_fs_enabled = THROTTLE_FAILSAFE; g.throttle_fs_action = THROTTLE_FAILSAFE_ACTION; g.throttle_fs_value = THROTTLE_FS_VALUE; @@ -739,10 +739,10 @@ default_gains() // custom dampeners // roll pitch - g.stabilize_dampener = STABILIZE_DAMPENER; + g.stabilize_dampener = STABILIZE_ROLL_D; //yaw - g.hold_yaw_dampener = HOLD_YAW_DAMPENER; + g.hold_yaw_dampener = YAW_D; // navigation g.pid_nav_lat.kP(NAV_P);