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
This commit is contained in:
Randy Mackay 2014-04-30 11:17:59 +09:00
parent 9f63de9b59
commit 7d7a2aced7
8 changed files with 32 additions and 1 deletions

View File

@ -12,6 +12,7 @@ static bool 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(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
return true; return true;
} }

View File

@ -194,6 +194,11 @@ static bool autotune_init(bool ignore_checks)
// initialise altitude target to stopping point // initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z(); 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; return true;
} }

View File

@ -10,6 +10,11 @@ static bool circle_init(bool ignore_checks)
if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) { if ((GPS_ok() && inertial_nav.position_ok()) || ignore_checks) {
circle_pilot_yaw_override = false; circle_pilot_yaw_override = false;
circle_nav.init(); 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; return true;
}else{ }else{
return false; return false;

View File

@ -106,6 +106,7 @@ static bool hybrid_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(-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 // initialise lean angles to current attitude
hybrid.pilot_roll = 0; hybrid.pilot_roll = 0;

View File

@ -18,6 +18,10 @@ static bool land_init(bool ignore_checks)
// initialise altitude target to stopping point // initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z(); 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; return true;
} }

View File

@ -12,8 +12,9 @@ static bool loiter_init(bool ignore_checks)
// set target to current position // set target to current position
wp_nav.init_loiter_target(); 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_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 // initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z(); pos_control.set_target_to_stopping_point_z();

View File

@ -9,6 +9,13 @@ static bool ofloiter_init(bool ignore_checks)
{ {
#if OPTFLOW == ENABLED #if OPTFLOW == ENABLED
if (g.optflow_enabled || ignore_checks) { 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; return true;
}else{ }else{
return false; return false;

View File

@ -7,6 +7,13 @@
// sport_init - initialise sport controller // sport_init - initialise sport controller
static bool sport_init(bool ignore_checks) 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; return true;
} }