From 193bfe839ef921658c7a38fb0d7d1277f1fdb489 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 27 Oct 2015 22:06:50 +0900 Subject: [PATCH] Copter: feed forward only used for AltHold, Loiter, PosHold land modes use non-feedforward alt hold --- ArduCopter/control_althold.cpp | 9 +++++---- ArduCopter/control_autotune.cpp | 7 ++++--- ArduCopter/control_brake.cpp | 7 ++++--- ArduCopter/control_loiter.cpp | 9 +++++---- ArduCopter/control_poshold.cpp | 7 ++++--- ArduCopter/control_sport.cpp | 7 ++++--- 6 files changed, 26 insertions(+), 20 deletions(-) diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index f7b2bd9554..646fbf9865 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -13,8 +13,9 @@ bool Copter::althold_init(bool ignore_checks) 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(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // stop takeoff if running takeoff_stop(); @@ -95,7 +96,7 @@ void Copter::althold_run() attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); break; @@ -124,7 +125,7 @@ void Copter::althold_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); break; } diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index 7fe8555d99..6aa7277f23 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -247,8 +247,9 @@ bool Copter::autotune_start(bool ignore_checks) 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(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); return true; } @@ -325,7 +326,7 @@ void Copter::autotune_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); } } diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index 9ad1b355ea..424f7f139d 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -21,8 +21,9 @@ bool Copter::brake_init(bool ignore_checks) pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE); - // initialise altitude target to stopping point - pos_control.set_target_to_stopping_point_z(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); return true; }else{ @@ -67,6 +68,6 @@ void Copter::brake_run() // body-frame rate controller is run directly from 100hz loop // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(0.0f, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index d39768744f..5db4de138d 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -18,8 +18,9 @@ bool Copter::loiter_init(bool ignore_checks) 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(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); return true; }else{ @@ -107,7 +108,7 @@ void Copter::loiter_run() attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); break; @@ -141,7 +142,7 @@ void Copter::loiter_run() } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.update_z_controller(); break; } diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index b8c02b4f24..fde3ad8f66 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -86,8 +86,9 @@ bool Copter::poshold_init(bool ignore_checks) 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(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialise lean angles to current attitude poshold.pilot_roll = 0; @@ -510,7 +511,7 @@ void Copter::poshold_run() target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt); } // update altitude target and call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); } diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 54d993f8ae..76c759638f 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -13,8 +13,9 @@ bool Copter::sport_init(bool ignore_checks) 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(); + // initialise position and desired velocity + pos_control.set_alt_target(inertial_nav.get_altitude()); + pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z()); return true; } @@ -101,7 +102,7 @@ void Copter::sport_run() } // call position controller - pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); + pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control.update_z_controller(); }