Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 07:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: Separate max ascent and descent speeds

Browse Source 
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.
This commit is contained in:
ChrisBird 2017-11-08 05:25:53 -08:00 committed by Randy Mackay
parent 767018b1d4
commit caaeae3d0a
12 changed files with 50 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduCopter/Attitude.cpp
View File

@ -200,10 +200,10 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
// check throttle is above, below or in the deadband // check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) { if (throttle_control < deadband_bottom) {
// below the deadband // 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) { }else if (throttle_control > deadband_top) {
// above the deadband // 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{ }else{
// must be in the deadband // must be in the deadband
desired_rate = 0.0f; desired_rate = 0.0f;
@ -307,3 +307,13 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
x = ne_x; x = ne_x;
y = ne_y; 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);
}
}

2
ArduCopter/Copter.h
View File

@ -1162,6 +1162,8 @@ private:
void dataflash_periodic(void); void dataflash_periodic(void);
void ins_periodic(); void ins_periodic();
void accel_cal_update(void); void accel_cal_update(void);
uint16_t get_pilot_speed_dn();
public: public:
void mavlink_delay_cb(); void mavlink_delay_cb();

17
ArduCopter/Parameters.cpp
View File

@ -251,14 +251,14 @@ const AP_Param::Info Copter::var_info[] = {
// @User: Standard // @User: Standard
GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0), GSCALAR(land_speed_high, "LAND_SPEED_HIGH", 0),
// @Param: PILOT_VELZ_MAX // @Param: PILOT_SPEED_UP
// @DisplayName: Pilot maximum vertical speed // @DisplayName: Pilot maximum vertical speed ascending
// @Description: The maximum vertical velocity the pilot may request in cm/s // @Description: The maximum vertical ascending velocity the pilot may request in cm/s
// @Units: cm/s // @Units: cm/s
// @Range: 50 500 // @Range: 50 500
// @Increment: 10 // @Increment: 10
// @User: Standard // @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 // @Param: PILOT_ACCEL_Z
// @DisplayName: Pilot vertical acceleration // @DisplayName: Pilot vertical acceleration
@ -1018,6 +1018,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Path: ../libraries/AP_Winch/AP_Winch.cpp // @Path: ../libraries/AP_Winch/AP_Winch.cpp
AP_SUBGROUPINFO(winch, "WINCH", 23, ParametersG2, AP_Winch), 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 AP_GROUPEND
}; };

7
ArduCopter/Parameters.h
View File

@ -99,7 +99,7 @@ public:
k_param_throttle_accel_enabled, // deprecated - remove k_param_throttle_accel_enabled, // deprecated - remove
k_param_wp_yaw_behavior, k_param_wp_yaw_behavior,
k_param_acro_trainer, 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_circle_rate, // deprecated - remove
k_param_rangefinder_gain, k_param_rangefinder_gain,
k_param_ch8_option, k_param_ch8_option,
@ -414,7 +414,7 @@ public:
AP_Int32 rtl_loiter_time; AP_Int32 rtl_loiter_time;
AP_Int16 land_speed; AP_Int16 land_speed;
AP_Int16 land_speed_high; 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 AP_Int16 pilot_accel_z; // vertical acceleration the pilot may request
// Throttle // Throttle
@ -574,6 +574,9 @@ public:
// wheel encoder and winch // wheel encoder and winch
AP_WheelEncoder wheel_encoder; AP_WheelEncoder wheel_encoder;
AP_Winch winch; AP_Winch winch;
// Additional pilot velocity items
AP_Int16 pilot_speed_dn;
}; };
extern const AP_Param::Info var_info[]; extern const AP_Param::Info var_info[];

6
ArduCopter/control_althold.cpp
View File

@ -9,7 +9,7 @@
bool Copter::althold_init(bool ignore_checks) bool Copter::althold_init(bool ignore_checks)
{ {
// initialize vertical speeds and leash lengths // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
@ -32,7 +32,7 @@ void Copter::althold_run()
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;
// initialize vertical speeds and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// apply SIMPLE mode transform to pilot inputs // apply SIMPLE mode transform to pilot inputs
@ -47,7 +47,7 @@ void Copter::althold_run()
// get pilot desired climb rate // get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); 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 #if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished // helicopters are held on the ground until rotor speed runup has finished

4
ArduCopter/control_autotune.cpp
View File

@ -263,7 +263,7 @@ bool Copter::autotune_start(bool ignore_checks)
} }
// initialize vertical speeds and leash lengths // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
@ -404,7 +404,7 @@ void Copter::autotune_run()
autotune_do_gcs_announcements(); autotune_do_gcs_announcements();
// initialize vertical speeds and acceleration // 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); 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 // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately

4
ArduCopter/control_circle.cpp
View File

@ -14,7 +14,7 @@ bool Copter::circle_init(bool ignore_checks)
pos_control->set_speed_xy(wp_nav->get_speed_xy()); pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration()); pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_jerk_xy_to_default(); 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise circle controller including setting the circle center based on vehicle speed // initialise circle controller including setting the circle center based on vehicle speed
@ -36,7 +36,7 @@ void Copter::circle_run()
// initialize speeds and accelerations // initialize speeds and accelerations
pos_control->set_speed_xy(wp_nav->get_speed_xy()); pos_control->set_speed_xy(wp_nav->get_speed_xy());
pos_control->set_accel_xy(wp_nav->get_wp_acceleration()); 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); 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 // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately

2
ArduCopter/control_guided.cpp
View File

@ -110,7 +110,7 @@ void Copter::guided_vel_control_start()
pos_control->set_jerk_xy_to_default(); pos_control->set_jerk_xy_to_default();
// initialize vertical speeds and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise velocity controller // initialise velocity controller

6
ArduCopter/control_loiter.cpp
View File

@ -13,7 +13,7 @@ bool Copter::loiter_init(bool ignore_checks)
wp_nav->init_loiter_target(); wp_nav->init_loiter_target();
// initialize vertical speed and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
@ -74,7 +74,7 @@ void Copter::loiter_run()
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// process pilot inputs unless we are in radio failsafe // process pilot inputs unless we are in radio failsafe
@ -90,7 +90,7 @@ void Copter::loiter_run()
// get pilot desired climb rate // get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); 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 { } else {
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason // 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(); wp_nav->clear_pilot_desired_acceleration();

6
ArduCopter/control_poshold.cpp
View File

@ -81,7 +81,7 @@ bool Copter::poshold_init(bool ignore_checks)
} }
// initialize vertical speeds and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
@ -137,7 +137,7 @@ void Copter::poshold_run()
const Vector3f& vel = inertial_nav.get_velocity(); const Vector3f& vel = inertial_nav.get_velocity();
// initialize vertical speeds and acceleration // 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); 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 // 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) // 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 = 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 // get takeoff adjusted pilot and takeoff climb rates
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);

6
ArduCopter/control_sport.cpp
View File

@ -8,7 +8,7 @@
bool Copter::sport_init(bool ignore_checks) bool Copter::sport_init(bool ignore_checks)
{ {
// initialize vertical speed and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// initialise position and desired velocity // initialise position and desired velocity
@ -28,7 +28,7 @@ void Copter::sport_run()
float takeoff_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f;
// initialize vertical speed and acceleration // 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); pos_control->set_accel_z(g.pilot_accel_z);
// apply SIMPLE mode transform // apply SIMPLE mode transform
@ -68,7 +68,7 @@ void Copter::sport_run()
// get pilot desired climb rate // get pilot desired climb rate
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); 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 #if FRAME_CONFIG == HELI_FRAME
// helicopters are held on the ground until rotor speed runup has finished // helicopters are held on the ground until rotor speed runup has finished

2
ArduCopter/takeoff.cpp
View File

@ -59,7 +59,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
void Copter::takeoff_timer_start(float alt_cm) void Copter::takeoff_timer_start(float alt_cm)
{ {
// calculate climb rate // 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 // sanity check speed and target
if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) { if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {