From 7d7a2aced7901fedaaf4bb8b454caf7aa64e0da2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Apr 2014 11:17:59 +0900
Subject: [PATCH] Copter: init vert speed and accel for each flight mode
This resolves issue #1021 in which LAND mode could descend at the
PILOT_VELZ rate instead of the WPNAV_SPEED_DN
Pilot defined acceleration is used for AltHold, AutoTune , Circle,
Hybrid, Loiter, OF_Loiter and Sport flight modes
Waypoint Nav (ie. AutoPilot) acceleration is used for Auto, Land, RTL
---
ArduCopter/control_althold.pde | 1 +
ArduCopter/control_autotune.pde | 5 +++++
ArduCopter/control_circle.pde | 5 +++++
ArduCopter/control_hybrid.pde | 1 +
ArduCopter/control_land.pde | 4 ++++
ArduCopter/control_loiter.pde | 3 ++-
ArduCopter/control_ofloiter.pde | 7 +++++++
ArduCopter/control_sport.pde | 7 +++++++
8 files changed, 32 insertions(+), 1 deletion(-)
diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde
index 1fc9b77c3..f15818056 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 cf2cbf818..d57f40f53 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 3a201e748..deb905873 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 4bd77c65e..a0b309328 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 893577667..97f7dee16 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 77fba105b..9829479ba 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 2cda161b3..32ddd011d 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 61b46deb8..275bdf7e0 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;
}
--
GitLab