From 233b033e8cc59d2e5d9e6feaa9f25d4b9cb4704a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 22 Jul 2013 13:28:11 +1000 Subject: [PATCH] Plane: removed old speed/altitude control algorithms Use TECS only. This makes the code a lot simpler and easier to properly document --- ArduPlane/ArduPlane.pde | 23 ++++---------------- ArduPlane/Attitude.pde | 47 ++-------------------------------------- ArduPlane/Parameters.h | 16 +++++--------- ArduPlane/Parameters.pde | 11 ---------- ArduPlane/navigation.pde | 1 - ArduPlane/radio.pde | 10 +-------- 6 files changed, 12 insertions(+), 96 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index f5cd8ff507..e6bdd7e859 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -428,13 +428,6 @@ static int32_t target_airspeed_cm; // The difference between current and desired airspeed. Used in the pitch controller. Centimeters per second. static float airspeed_error_cm; -// The calculated total energy error (kinetic (altitude) plus potential (airspeed)). -// Used by the throttle controller -static int32_t energy_error; - -// kinetic portion of energy error (m^2/s^2) -static int32_t airspeed_energy_error; - // An amount that the airspeed should be increased in auto modes based on the user positioning the // throttle stick in the top half of the range. Centimeters per second. static int16_t airspeed_nudge_cm; @@ -827,10 +820,7 @@ static void fast_loop() */ static void update_speed_height(void) { - if ((g.alt_control_algorithm == ALT_CONTROL_TECS || - g.alt_control_algorithm == ALT_CONTROL_DEFAULT) && - auto_throttle_mode && !throttle_suppressed) - { + if (auto_throttle_mode && !throttle_suppressed) { // Call TECS 50Hz update SpdHgt_Controller->update_50hz(relative_altitude()); } @@ -1068,7 +1058,7 @@ static void update_flight_mode(void) nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL); } - if (alt_control_airspeed()) { + if (airspeed.use()) { calc_nav_pitch(); if (nav_pitch_cd < takeoff_pitch_cd) nav_pitch_cd = takeoff_pitch_cd; @@ -1094,7 +1084,7 @@ static void update_flight_mode(void) nav_pitch_cd = g.land_pitch_cd; } else { calc_nav_pitch(); - if (!alt_control_airspeed()) { + if (!airspeed.use()) { // when not under airspeed control, don't allow // down pitch in landing nav_pitch_cd = constrain_int32(nav_pitch_cd, 0, nav_pitch_cd); @@ -1305,13 +1295,8 @@ static void update_alt() geofence_check(true); - // Calculate new climb rate - //if(medium_loopCounter == 0 && slow_loopCounter == 0) - // add_altitude_data(millis() / 100, g_gps->altitude / 10); - // Update the speed & height controller states - if ((g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) && - auto_throttle_mode && !throttle_suppressed) { + if (auto_throttle_mode && !throttle_suppressed) { SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt + (int32_t(g.alt_offset)*100), target_airspeed_cm, (control_mode==AUTO && takeoff_complete == false), diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 54fa6397cd..acc239ebd7 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -354,39 +354,7 @@ static void calc_throttle() return; } - if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) { - channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand(); - } else if (!alt_control_airspeed()) { - int16_t throttle_target = aparm.throttle_cruise + throttle_nudge; - - // TODO: think up an elegant way to bump throttle when - // groundspeed_undershoot > 0 in the no airspeed sensor case; PID - // control? - - // no airspeed sensor, we use nav pitch to determine the proper throttle output - // AUTO, RTL, etc - // --------------------------------------------------------------------------- - if (nav_pitch_cd >= 0) { - channel_throttle->servo_out = throttle_target + (aparm.throttle_max - throttle_target) * nav_pitch_cd / aparm.pitch_limit_max_cd; - } else { - channel_throttle->servo_out = throttle_target - (throttle_target - aparm.throttle_min) * nav_pitch_cd / aparm.pitch_limit_min_cd; - } - - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, aparm.throttle_min.get(), aparm.throttle_max.get()); - } else { - // throttle control with airspeed compensation - // ------------------------------------------- - energy_error = airspeed_energy_error + altitude_error_cm * 0.098f; - - // positive energy errors make the throttle go higher - channel_throttle->servo_out = aparm.throttle_cruise + g.pidTeThrottle.get_pid(energy_error); - channel_throttle->servo_out += (channel_pitch->servo_out * g.kff_pitch_to_throttle); - - channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, - aparm.throttle_min.get(), aparm.throttle_max.get()); - } - - + channel_throttle->servo_out = SpdHgt_Controller->get_throttle_demand(); } /***************************************** @@ -419,13 +387,7 @@ static void calc_nav_pitch() { // Calculate the Pitch of the plane // -------------------------------- - if (g.alt_control_algorithm == ALT_CONTROL_TECS || g.alt_control_algorithm == ALT_CONTROL_DEFAULT) { - nav_pitch_cd = SpdHgt_Controller->get_pitch_demand(); - } else if (alt_control_airspeed()) { - nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm); - } else { - nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm); - } + nav_pitch_cd = SpdHgt_Controller->get_pitch_demand(); nav_pitch_cd = constrain_int32(nav_pitch_cd, aparm.pitch_limit_min_cd.get(), aparm.pitch_limit_max_cd.get()); } @@ -864,8 +826,3 @@ static void demo_servos(uint8_t i) } } -// return true if we should use airspeed for altitude/throttle control -static bool alt_control_airspeed(void) -{ - return airspeed.use() && g.alt_control_algorithm == ALT_CONTROL_AIRSPEED; -} diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index d5b460f1ad..3aef41ab4e 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -200,7 +200,7 @@ public: // k_param_kff_pitch_compensation = 200, // unused k_param_kff_rudder_mix, - k_param_kff_pitch_to_throttle, + k_param_kff_pitch_to_throttle, // unused k_param_kff_throttle_to_pitch, k_param_scaling_speed, @@ -244,10 +244,10 @@ public: k_param_pidNavRoll = 240, // unused k_param_pidServoRoll, // unused k_param_pidServoPitch, // unused - k_param_pidNavPitchAirspeed, - k_param_pidServoRudder, - k_param_pidTeThrottle, - k_param_pidNavPitchAltitude, + k_param_pidNavPitchAirspeed, // unused + k_param_pidServoRudder, // unused + k_param_pidTeThrottle, // unused + k_param_pidNavPitchAltitude, // unused k_param_pidWheelSteer, // 254,255: reserved @@ -415,9 +415,6 @@ public: AP_YawController yawController; // PID controllers - PID pidNavPitchAirspeed; - PID pidTeThrottle; - PID pidNavPitchAltitude; PID pidWheelSteer; Parameters() : @@ -449,9 +446,6 @@ public: // PID controller initial P initial I initial D initial imax //----------------------------------------------------------------------------------- - pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC), - pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX), - pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM), pidWheelSteer (0, 0, 0, 0) {} diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index b9e43832cf..d4fc9df313 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -59,14 +59,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(kff_rudder_mix, "KFF_RDDRMIX", RUDDER_MIX), - // @Param: KFF_PTCH2THR - // @DisplayName: Pitch to Throttle Mix - // @Description: Pitch to throttle feed-forward gain. - // @Range: 0 5 - // @Increment: 0.01 - // @User: Advanced - GSCALAR(kff_pitch_to_throttle, "KFF_PTCH2THR", 0), - // @Param: KFF_THR2PTCH // @DisplayName: Throttle to Pitch Mix // @Description: Throttle to pitch feed-forward gain. @@ -812,9 +804,6 @@ const AP_Param::Info var_info[] PROGMEM = { GGROUP(rc_12, "RC12_", RC_Channel_aux), #endif - GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID), - GGROUP(pidTeThrottle, "ENRGY2THR_", PID), - GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID), GGROUP(pidWheelSteer, "WHEELSTEER_",PID), // @Group: RLL2SRV_ diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index d405c1538a..5e9bbec91f 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -111,7 +111,6 @@ static void calc_airspeed_errors() target_airspeed_cm = (aparm.airspeed_max * 100); airspeed_error_cm = target_airspeed_cm - aspeed_cm; - airspeed_energy_error = ((target_airspeed_cm * target_airspeed_cm) - (aspeed_cm*aspeed_cm))*0.00005; } static void calc_gndspeed_undershoot() diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index 26a3583f07..907e52664d 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -101,7 +101,7 @@ static void read_radio() if (g.throttle_nudge && channel_throttle->servo_out > 50) { float nudge = (channel_throttle->servo_out - 50) * 0.02; - if (alt_control_airspeed()) { + if (airspeed.use()) { airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge; } else { throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge; @@ -110,14 +110,6 @@ static void read_radio() airspeed_nudge_cm = 0; throttle_nudge = 0; } - - /* - * cliSerial->printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), - * (int)g.rc_1.control_in, - * (int)g.rc_2.control_in, - * (int)g.rc_3.control_in, - * (int)g.rc_4.control_in); - */ } static void control_failsafe(uint16_t pwm)