aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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
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);
}
}

2
ArduCopter/Copter.h
View File

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

17
ArduCopter/Parameters.cpp
View File

@ -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
};

7
ArduCopter/Parameters.h
View File

@ -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[];

6
ArduCopter/control_althold.cpp
View File

@ -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

4
ArduCopter/control_autotune.cpp
View File

@ -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

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_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

2
ArduCopter/control_guided.cpp
View File

@ -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

6
ArduCopter/control_loiter.cpp
View File

@ -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();

6
ArduCopter/control_poshold.cpp
View File

@ -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);

6
ArduCopter/control_sport.cpp
View File

@ -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

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)
{
// 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) {