Copter: feed forward only used for AltHold, Loiter, PosHold

land modes use non-feedforward alt hold

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

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

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

ArduCopter/control_guided.cpp

@@ -139,8 +139,9 @@ void Copter::guided_angle_control_start()
     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());
 
-    // 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 targets
     guided_angle_state.update_time_ms = millis();
@@ -493,7 +494,7 @@ void Copter::guided_angle_control_run()
     attitude_control.angle_ef_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);
 
     // call position controller
-    pos_control.set_alt_target_from_climb_rate(climb_rate_cms, G_Dt, false);
+    pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
     pos_control.update_z_controller();
 }
 

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

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

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