From caaeae3d0ac4056716f43710c66534461a66a580 Mon Sep 17 00:00:00 2001 From: ChrisBird Date: Wed, 8 Nov 2017 05:25:53 -0800 Subject: [PATCH] Copter: Separate max ascent and descent speeds Added equivalent parameters to WPNAV_SPEED_UP and WPNAV_SPEED_DN New parameters named: PILOT_SPEED_UP (technically renamed PILOT_VELZ_MAX) PILOT_SPEED_DN Removed parameter PILOT_VELZ_MAX (technically renamed to PILOT_SPEED_UP). Flight Modes impacted: ALTHOLD AUTOTUNE CIRCLE LOITER POSHOLD SPORT TAKEOFF Update a section in GUIDED mode but I don't think it is ever used but update just in case. It will use the PILOT_SPEED_UP for ascending max velocity. For down it will check if it is 0, if so then it will PILOT_SPEED_UP instead, if non zero it will use PILOT_SPEED_DN. This retains current behavior and gives the flexibility to change it if desired. --- ArduCopter/Attitude.cpp | 14 ++++++++++++-- ArduCopter/Copter.h | 2 ++ ArduCopter/Parameters.cpp | 17 +++++++++++++---- ArduCopter/Parameters.h | 7 +++++-- ArduCopter/control_althold.cpp | 6 +++--- ArduCopter/control_autotune.cpp | 4 ++-- ArduCopter/control_circle.cpp | 4 ++-- ArduCopter/control_guided.cpp | 2 +- ArduCopter/control_loiter.cpp | 6 +++--- ArduCopter/control_poshold.cpp | 6 +++--- ArduCopter/control_sport.cpp | 6 +++--- ArduCopter/takeoff.cpp | 2 +- 12 files changed, 50 insertions(+), 26 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index ea735a6f56..8efc01171a 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -200,10 +200,10 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control) // check throttle is above, below or in the deadband if (throttle_control < deadband_bottom) { // below the deadband - desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / deadband_bottom; + desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom; }else if (throttle_control > deadband_top) { // above the deadband - desired_rate = g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000.0f-deadband_top); + desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top); }else{ // must be in the deadband desired_rate = 0.0f; @@ -307,3 +307,13 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y) x = ne_x; y = ne_y; } + +// It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value. +uint16_t Copter::get_pilot_speed_dn() +{ + if (g2.pilot_speed_dn == 0) { + return abs(g.pilot_speed_up); + } else { + return abs(g2.pilot_speed_dn); + } +} diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a359445c99..1784b9d2a8 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1162,6 +1162,8 @@ private: void dataflash_periodic(void); void ins_periodic(); void accel_cal_update(void); + + uint16_t get_pilot_speed_dn(); public: void mavlink_delay_cb(); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 3cc8a2aaf2..c92acab9a8 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -251,14 +251,14 @@ const AP_Param::Info Copter::var_info[] = { // @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 + // @Param: PILOT_SPEED_UP + // @DisplayName: Pilot maximum vertical speed ascending + // @Description: The maximum vertical ascending velocity the pilot may request in cm/s // @Units: cm/s // @Range: 50 500 // @Increment: 10 // @User: Standard - GSCALAR(pilot_velocity_z_max, "PILOT_VELZ_MAX", PILOT_VELZ_MAX), + GSCALAR(pilot_speed_up, "PILOT_SPEED_UP", PILOT_VELZ_MAX), // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration @@ -1018,6 +1018,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Path: ../libraries/AP_Winch/AP_Winch.cpp AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), + // @Param: PILOT_SPEED_DN + // @DisplayName: Pilot maximum vertical speed descending + // @Description: The maximum vertical descending velocity the pilot may request in cm/s + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("PILOT_SPEED_DN", 24, ParametersG2, pilot_speed_dn, 0), + AP_GROUPEND }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 45467682d3..f1398ce98b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -99,7 +99,7 @@ public: k_param_throttle_accel_enabled, // deprecated - remove k_param_wp_yaw_behavior, k_param_acro_trainer, - k_param_pilot_velocity_z_max, + k_param_pilot_speed_up, // renamed from k_param_pilot_velocity_z_max k_param_circle_rate, // deprecated - remove k_param_rangefinder_gain, k_param_ch8_option, @@ -414,7 +414,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_speed_up; // maximum vertical ascending velocity the pilot may request AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request // Throttle @@ -574,6 +574,9 @@ public: // wheel encoder and winch AP_WheelEncoder wheel_encoder; AP_Winch winch; + + // Additional pilot velocity items + AP_Int16 pilot_speed_dn; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 3ddd12c013..bf8bb0a427 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -9,7 +9,7 @@ bool Copter::althold_init(bool ignore_checks) { // initialize vertical speeds and leash lengths - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -32,7 +32,7 @@ void Copter::althold_run() float takeoff_climb_rate = 0.0f; // initialize vertical speeds and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs @@ -47,7 +47,7 @@ void Copter::althold_run() // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 3b6f697a7a..3307d6b6ac 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -263,7 +263,7 @@ bool Copter::autotune_start(bool ignore_checks) } // initialize vertical speeds and leash lengths - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -404,7 +404,7 @@ void Copter::autotune_run() autotune_do_gcs_announcements(); // initialize vertical speeds and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index e1e06fcbf0..05fa2c0957 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -14,7 +14,7 @@ bool Copter::circle_init(bool ignore_checks) pos_control->set_speed_xy(wp_nav->get_speed_xy()); pos_control->set_accel_xy(wp_nav->get_wp_acceleration()); pos_control->set_jerk_xy_to_default(); - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed @@ -36,7 +36,7 @@ void Copter::circle_run() // initialize speeds and accelerations pos_control->set_speed_xy(wp_nav->get_speed_xy()); pos_control->set_accel_xy(wp_nav->get_wp_acceleration()); - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 8eb929f3a1..675cc2bb77 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -110,7 +110,7 @@ void Copter::guided_vel_control_start() pos_control->set_jerk_xy_to_default(); // initialize vertical speeds and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise velocity controller diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index f8fc21f47c..3cbf6a436b 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -13,7 +13,7 @@ bool Copter::loiter_init(bool ignore_checks) wp_nav->init_loiter_target(); // initialize vertical speed and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -74,7 +74,7 @@ void Copter::loiter_run() float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe @@ -90,7 +90,7 @@ void Copter::loiter_run() // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason wp_nav->clear_pilot_desired_acceleration(); diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 4ef6a43591..67025eeb2c 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -81,7 +81,7 @@ bool Copter::poshold_init(bool ignore_checks) } // initialize vertical speeds and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -137,7 +137,7 @@ void Copter::poshold_run() const Vector3f& vel = inertial_nav.get_velocity(); // initialize vertical speeds and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately @@ -159,7 +159,7 @@ void Copter::poshold_run() // get pilot desired climb rate (for alt-hold mode and take-off) target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index f56cf5ab08..16b83fdbee 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -8,7 +8,7 @@ bool Copter::sport_init(bool ignore_checks) { // initialize vertical speed and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // initialise position and desired velocity @@ -28,7 +28,7 @@ void Copter::sport_run() float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration - pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // apply SIMPLE mode transform @@ -68,7 +68,7 @@ void Copter::sport_run() // get pilot desired climb rate float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); - target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max); + target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); #if FRAME_CONFIG == HELI_FRAME // helicopters are held on the ground until rotor speed runup has finished diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 0834a4292c..aaf8247d22 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -59,7 +59,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) void Copter::takeoff_timer_start(float alt_cm) { // calculate climb rate - float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f)); + float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_speed_up*2.0f/3.0f, g.pilot_speed_up-50.0f)); // sanity check speed and target if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {