Skip to content

Commit

Permalink
Copter: Separate max ascent and descent speeds
Browse files Browse the repository at this point in the history
Added equivalent parameters to WPNAV_SPEED_UP and WPNAV_SPEED_DN

New parameters named:
PILOT_SPEED_UP
PILOT_SPEED_DN

Removed parameter PILOT_VELZ_MAX.

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.

Copter: Separate max ascent and descent speeds

Applied changes for pull request 6951 as per review comments

Copter: Switched to using G2 parameters, retained old param and convert from it

Copter: Fixed random character in a comment that I introduced
  • Loading branch information
ChrisBird committed Sep 16, 2017
1 parent d475b91 commit 8e25730
Show file tree
Hide file tree
Showing 11 changed files with 47 additions and 22 deletions.
4 changes: 2 additions & 2 deletions ArduCopter/Attitude.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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 = g2.pilot_velocity_z_max_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 = g2.pilot_velocity_z_max_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else{
// must be in the deadband
desired_rate = 0.0f;
Expand Down
23 changes: 22 additions & 1 deletion ArduCopter/Parameters.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -251,7 +251,7 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard
GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0),

// @Param: PILOT_VELZ_MAX
// @Param: PILOT_SPEED_UP
// @DisplayName: Pilot maximum vertical speed
// @Description: The maximum vertical velocity the pilot may request in cm/s
// @Units: cm/s
Expand Down Expand Up @@ -1010,6 +1010,25 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp
AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL),

// @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
AP_GROUPINFO("PILOT_SPEED_UP", 22, ParametersG2, pilot_velocity_z_max_up, PILOT_VELZ_MAX),

// @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", 23, ParametersG2, pilot_velocity_z_max_dn, PILOT_VELZ_MAX),


AP_GROUPEND
};

Expand Down Expand Up @@ -1055,6 +1074,8 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" },
{ Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" },
{ Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" },
{ Parameters::k_param_pilot_velocity_z_max, 0, AP_PARAM_INT16, "PILOT_SPEED_DN" },
{ Parameters::k_param_pilot_velocity_z_max, 0, AP_PARAM_INT16, "PILOT_SPEED_UP" },
};

void Copter::load_parameters(void)
Expand Down
6 changes: 5 additions & 1 deletion ArduCopter/Parameters.h
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class Parameters {
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_velocity_z_max, // deprecated - remove
k_param_circle_rate, // deprecated - remove
k_param_rangefinder_gain,
k_param_ch8_option,
Expand Down Expand Up @@ -570,6 +570,10 @@ class ParametersG2 {

// Safe RTL library
AP_SmartRTL smart_rtl;

// Additional pilot velocity items
AP_Int16 pilot_velocity_z_max_up; // maximum vertical ascending velocity the pilot may request
AP_Int16 pilot_velocity_z_max_dn; // maximum vertical descending velocity the pilot may request
};

extern const AP_Param::Info var_info[];
6 changes: 3 additions & 3 deletions ArduCopter/control_althold.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ bool Copter::althold_init(bool ignore_checks)
#endif

// 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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
Expand All @@ -39,7 +39,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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// apply SIMPLE mode transform to pilot inputs
Expand All @@ -54,7 +54,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, -g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);

#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/control_autotune.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
Expand Down Expand Up @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_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
Expand Down
4 changes: 2 additions & 2 deletions ArduCopter/control_circle.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise circle controller including setting the circle center based on vehicle speed
Expand All @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_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
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/control_guided.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise velocity controller
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/control_loiter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
Expand Down Expand Up @@ -81,7 +81,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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// process pilot inputs unless we are in radio failsafe
Expand All @@ -97,7 +97,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, -g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_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();
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/control_poshold.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
Expand Down Expand Up @@ -144,7 +144,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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_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
Expand All @@ -166,7 +166,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, -g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);

// get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
Expand Down
6 changes: 3 additions & 3 deletions ArduCopter/control_sport.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// initialise position and desired velocity
Expand All @@ -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(-g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);
pos_control->set_accel_z(g.pilot_accel_z);

// apply SIMPLE mode transform
Expand Down Expand Up @@ -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, -g2.pilot_velocity_z_max_dn, g2.pilot_velocity_z_max_up);

#if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished
Expand Down
2 changes: 1 addition & 1 deletion ArduCopter/takeoff.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(g2.pilot_velocity_z_max_up*2.0f/3.0f, g2.pilot_velocity_z_max_up-50.0f));

// sanity check speed and target
if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {
Expand Down

0 comments on commit 8e25730

Please sign in to comment.