diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index 1fc9b77c3865b67641c276a0133775b1289e244b..f15818056fa49c8689ea209a4d97c85a22656197 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -12,6 +12,7 @@ static bool 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_accel_z(g.pilot_accel_z); return true; } diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index cf2cbf81838a6316715c96a93e6a41956f63ab0b..d57f40f53f19e0fc05be460c9474995e3985d353 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -194,6 +194,11 @@ static bool autotune_init(bool ignore_checks) // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); + + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + return true; } diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 3a201e748280897f6b38cbb4e7f5238cca623c15..deb905873adf1839c5b3ab10d747e671282bd742 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -10,6 +10,11 @@ static bool circle_init(bool ignore_checks) if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) { circle_pilot_yaw_override = false; circle_nav.init(); + + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + return true; }else{ return false; diff --git a/ArduCopter/control_hybrid.pde b/ArduCopter/control_hybrid.pde index 4bd77c65e6e3dc6645ccc2d3b744e86b8806bfb5..a0b309328bd5c3d8bdfad497dfc5950bd87ccd70 100644 --- a/ArduCopter/control_hybrid.pde +++ b/ArduCopter/control_hybrid.pde @@ -106,6 +106,7 @@ static bool hybrid_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_accel_z(g.pilot_accel_z); // initialise lean angles to current attitude hybrid.pilot_roll = 0; diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 8935776675e859630caf915b2bb37781f22c0f08..97f7dee1612a92d7fc29ee657663d25b526846d7 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -18,6 +18,10 @@ static bool land_init(bool ignore_checks) // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); + + // initialize vertical speeds and leash lengths + pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up()); + pos_control.set_accel_z(wp_nav.get_accel_z()); return true; } diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 77fba105b151ba063445a8a7d674e04a01d0dacc..9829479ba353419935a2af0a9fcaa30f165d2086 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -12,8 +12,9 @@ static bool loiter_init(bool ignore_checks) // set target to current position wp_nav.init_loiter_target(); - // initialize vertical speeds + // initialize vertical speed and accelerationj pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); // initialise altitude target to stopping point pos_control.set_target_to_stopping_point_z(); diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde index 2cda161b3961436d3389b32af6d2129ef8894df2..32ddd011dbdc02ddb828d905dc81081618acc77e 100644 --- a/ArduCopter/control_ofloiter.pde +++ b/ArduCopter/control_ofloiter.pde @@ -9,6 +9,13 @@ static bool ofloiter_init(bool ignore_checks) { #if OPTFLOW == ENABLED if (g.optflow_enabled || ignore_checks) { + // initialise altitude target to stopping point + pos_control.set_target_to_stopping_point_z(); + + // initialize vertical speed and acceleration + pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + return true; }else{ return false; diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 61b46deb88d78beb3ef597ab9b241d37730caf5e..275bdf7e0d2fc39bcc60256ed2a60060d7aa4622 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -7,6 +7,13 @@ // sport_init - initialise sport controller static bool sport_init(bool ignore_checks) { + // initialize vertical speed and accelerationj + pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max); + pos_control.set_accel_z(g.pilot_accel_z); + + // initialise altitude target to stopping point + pos_control.set_target_to_stopping_point_z(); + return true; }