From b64ab07ca451ea3d5292caaf8bd2b09da8caf1ae Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 3 Aug 2014 17:16:51 +1000 Subject: [PATCH] Plane: added STAB_PITCH_DN_CD parameter this adds some down trim when at throttle levels below the trim throttle in FBWA mode. defaults to 200 centi-degrees. I may adjust based on flight tests --- ArduPlane/ArduPlane.pde | 1 + ArduPlane/Attitude.pde | 14 ++++++++++++++ ArduPlane/GCS_Mavlink.pde | 5 +---- ArduPlane/Parameters.h | 2 ++ ArduPlane/Parameters.pde | 9 +++++++++ ArduPlane/system.pde | 11 +++++++++++ 6 files changed, 38 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index a75c2b7c38..bf5e58eb0d 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1280,6 +1280,7 @@ static void update_flight_mode(void) } else { nav_pitch_cd = -(pitch_input * pitch_limit_min_cd); } + adjust_nav_pitch_throttle(); nav_pitch_cd = constrain_int32(nav_pitch_cd, pitch_limit_min_cd, aparm.pitch_limit_max_cd.get()); if (fly_inverted()) { nav_pitch_cd = -nav_pitch_cd; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 1e6b0a0fec..b0a9dde3e0 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -913,3 +913,17 @@ static void demo_servos(uint8_t i) } } +/* + adjust nav_pitch_cd for STAB_PITCH_DOWN_CD. This is used to make + keeping up good airspeed in FBWA mode easier, as the plane will + automatically pitch down a little when at low throttle. It makes + FBWA landings without stalling much easier. + */ +static void adjust_nav_pitch_throttle(void) +{ + uint8_t throttle = throttle_percentage(); + if (throttle < aparm.throttle_cruise) { + float p = (aparm.throttle_cruise - throttle) / (float)aparm.throttle_cruise; + nav_pitch_cd -= g.stab_pitch_down_cd * p; + } +} diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 97079eabab..2183b4fa90 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -375,15 +375,12 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) } else if (!ahrs.airspeed_estimate(&aspeed)) { aspeed = 0; } - float throttle_norm = channel_throttle->norm_output() * 100; - throttle_norm = constrain_int16(throttle_norm, -100, 100); - uint16_t throttle = ((uint16_t)(throttle_norm + 100)) / 2; mavlink_msg_vfr_hud_send( chan, aspeed, gps.ground_speed(), (ahrs.yaw_sensor / 100) % 360, - throttle, + throttle_percentage(), current_loc.alt / 100.0, barometer.get_climb_rate()); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index f86fc84e9d..1781816435 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -118,6 +118,7 @@ public: k_param_sonar, k_param_terrain, k_param_terrain_follow, + k_param_stab_pitch_down_cd, // 100: Arming parameters k_param_arming = 100, @@ -324,6 +325,7 @@ public: AP_Float kff_throttle_to_pitch; AP_Float ground_steer_alt; AP_Int16 ground_steer_dps; + AP_Int16 stab_pitch_down_cd; // speed used for speed scaling AP_Float scaling_speed; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 3a70859944..df295a5921 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -104,6 +104,15 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(kff_throttle_to_pitch, "KFF_THR2PTCH", 0), + // @Param: STAB_PITCH_DN_CD + // @DisplayName: Low throttle pitch down trim + // @Description: This controls the amount of downpitch to add in FBWA and AUTOTUNE modes when at low throttle. No down trim is added when throttle is above TRIM_THROTTLE. Below TRIM_THROTTLE downtrim is added in proportion to the amount the throttle is below TRIM_THROTTLE. At zero throttle the full downpitch specified in this parameter is added. This parameter is meant to help keep airspeed up when flying in FBWA mode with low throttle, such as when on a landing approach. A value of 200 (2 degrees) is good for many planes, although a higher value may be needed for high drag aircraft. + // @Range: 0 1000 + // @Increment: 1 + // @Units: centi-Degrees + // @User: Advanced + GSCALAR(stab_pitch_down_cd, "STAB_PITCH_DN_CD", 200), + // @Param: STICK_MIXING // @DisplayName: Stick Mixing // @Description: When enabled, this adds user stick input to the control surfaces in auto modes, allowing the user to have some degree of flight control without changing modes. There are two types of stick mixing available. If you set STICK_MIXING to 1 then it will use "fly by wire" mixing, which controls the roll and pitch in the same way that the FBWA mode does. This is the safest option if you usually fly ArduPlane in FBWA or FBWB mode. If you set STICK_MIXING to 2 then it will enable direct mixing mode, which is what the STABILIZE mode uses. That will allow for much more extreme maneuvers while in AUTO mode. diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index dbd79c66d5..ef9eb83ac3 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -634,3 +634,14 @@ static void telemetry_send(void) (AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get()); #endif } + + +/* + return throttle percentage from 0 to 100 + */ +static uint8_t throttle_percentage(void) +{ + // to get the real throttle we need to use norm_output() which + // returns a number from -1 to 1. + return constrain_int16(50*(channel_throttle->norm_output()+1), 0, 100); +}