From f19922fec5686e33b70ad05597ee5af609add5e9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 30 Mar 2016 12:03:41 +1100 Subject: [PATCH] Copter: added LAND_SPEED_HIGH parameter this is the descent rate for the first part of a landing. Separating this from WPNAV_SPEED_DN allows for independent tuning of fast fwd flight from landings --- ArduCopter/Parameters.cpp | 9 +++++++++ ArduCopter/Parameters.h | 2 ++ ArduCopter/control_land.cpp | 4 ++++ 3 files changed, 15 insertions(+) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index f920eefca6..f5d914a866 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -251,6 +251,15 @@ const AP_Param::Info Copter::var_info[] = { // @User: Standard GSCALAR(land_speed, "LAND_SPEED", LAND_SPEED), + // @Param: LAND_SPEED_HIGH + // @DisplayName: Land speed high + // @Description: The descent speed for the first stage of landing in cm/s. If this is zero then WPNAV_SPEED_DN is used + // @Units: cm/s + // @Range: 0 500 + // @Increment: 10 + // @User: Standard + GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0), + // @Param: PILOT_VELZ_MAX // @DisplayName: Pilot maximum vertical speed // @Description: The maximum vertical velocity the pilot may request in cm/s diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e1b70fbd8e..2d44b160b5 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -321,6 +321,7 @@ public: k_param_land_speed, k_param_auto_velocity_z_min, // remove k_param_auto_velocity_z_max, // remove - 219 + k_param_land_speed_high, // // 220: PI/D Controllers @@ -405,6 +406,7 @@ public: // AP_Int32 rtl_loiter_time; AP_Int16 land_speed; + AP_Int16 land_speed_high; AP_Int16 pilot_velocity_z_max; // maximum vertical velocity the pilot may request AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 91253bada0..d3d3359737 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -230,6 +230,10 @@ float Copter::get_land_descent_speed() #endif // if we are above 10m and the sonar does not sense anything perform regular alt hold descent if (pos_control.get_pos_target().z >= pv_alt_above_origin(LAND_START_ALT) && !(sonar_ok && sonar_alt_health >= SONAR_ALT_HEALTH_MAX)) { + if (g.land_speed_high > 0) { + // user has asked for a different landing speed than normal descent rate + return -abs(g.land_speed_high); + } return pos_control.get_speed_down(); }else{ return -abs(g.land_speed);