All Squashed

ArduCopter/mode_auto.cpp

@@ -1416,7 +1416,7 @@ void PayloadPlace::run()
         // vel_threshold_fraction * max velocity
         const float vel_threshold_fraction = 0.1;
         const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
-        bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= descent_start_altitude_cm - stop_distance;
+        bool reached_altitude = copter.pos_control->get_pos_desired_z_cm() >= descent_start_altitude_cm - stop_distance;
         if (reached_altitude) {
             state = State::Done;
         }

ArduCopter/mode_guided.cpp

@@ -383,10 +383,10 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
         // convert origin to alt-above-terrain if necessary
         if (!guided_pos_terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
-            pos_control->set_pos_offset_z_cm(origin_terr_offset);
+            pos_control->init_pos_terrain_cm(origin_terr_offset);
         }
     } else {
-        pos_control->set_pos_offset_z_cm(0.0);
+        pos_control->init_pos_terrain_cm(0.0);
     }
 
     // set yaw state
@@ -496,10 +496,10 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
         // convert origin to alt-above-terrain if necessary
         if (!guided_pos_terrain_alt) {
             // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin
-            pos_control->set_pos_offset_z_cm(origin_terr_offset);
+            pos_control->init_pos_terrain_cm(origin_terr_offset);
         }
     } else {
-        pos_control->set_pos_offset_z_cm(0.0);
+        pos_control->init_pos_terrain_cm(0.0);
     }
 
     guided_pos_target_cm = pos_target_f.topostype();
@@ -899,11 +899,11 @@ void ModeGuided::posvelaccel_control_run()
     // send position and velocity targets to position controller
     if (!stabilizing_vel_xy()) {
         // set the current commanded xy pos to the target pos and xy vel to the desired vel
-        guided_pos_target_cm.xy() = pos_control->get_pos_target_cm().xy();
+        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
         guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy();
     } else if (!stabilizing_pos_xy()) {
         // set the current commanded xy pos to the target pos
-        guided_pos_target_cm.xy() = pos_control->get_pos_target_cm().xy();
+        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
     }
     pos_control->input_pos_vel_accel_xy(guided_pos_target_cm.xy(), guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
     if (!stabilizing_vel_xy()) {

ArduCopter/mode_throw.cpp

@@ -72,9 +72,9 @@ void ModeThrow::run()
         // initialise the demanded height to 3m above the throw height
         // we want to rapidly clear surrounding obstacles
         if (g2.throw_type == ThrowType::Drop) {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100);
+            pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() - 100);
         } else {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300);
+            pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() + 300);
         }
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum

ArduCopter/mode_zigzag.cpp

@@ -464,7 +464,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
             }
 #endif
         } else {
-            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
+            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_desired_z_cm() : inertial_nav.get_position_z_up_cm();
         }
     }
 
@@ -518,7 +518,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
         }
 #endif
     } else {
-        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
+        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_desired_z_cm() : inertial_nav.get_position_z_up_cm();
     }
 
     return true;

ArduCopter/surface_tracking.cpp

@@ -18,7 +18,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         AP_SurfaceDistance &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state;
 
         // update position controller target offset to the surface's alt above the EKF origin
-        copter.pos_control->set_pos_offset_target_z_cm(rf_state.terrain_offset_cm);
+        copter.pos_control->set_pos_terrain_target_cm(rf_state.terrain_offset_cm);
         last_update_ms = now_ms;
         valid_for_logging = true;
 
@@ -28,7 +28,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         if (timeout ||
             reset_target ||
             (last_glitch_cleared_ms != rf_state.glitch_cleared_ms)) {
-            copter.pos_control->set_pos_offset_z_cm(rf_state.terrain_offset_cm);
+            copter.pos_control->init_pos_terrain_cm(rf_state.terrain_offset_cm);
             reset_target = false;
             last_glitch_cleared_ms = rf_state.glitch_cleared_ms;
         }
@@ -37,8 +37,7 @@ void Copter::SurfaceTracking::update_surface_offset()
         // reset position controller offsets if surface tracking is inactive
         // flag target should be reset when/if it next becomes active
         if (timeout) {
-            copter.pos_control->set_pos_offset_z_cm(0);
-            copter.pos_control->set_pos_offset_target_z_cm(0);
+            copter.pos_control->init_pos_terrain_cm(0);
             reset_target = true;
         }
     }
@@ -68,7 +67,7 @@ void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
     if (surface != Surface::GROUND) {
         return;
     }
-    copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm);
+    copter.pos_control->set_pos_terrain_target_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm);
     last_update_ms = AP_HAL::millis();
 }
 
@@ -79,6 +78,7 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co
     }
 
     const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
+    // fix me
     target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f;
     return true;
 }

ArduCopter/takeoff.cpp

@@ -52,7 +52,7 @@ void Mode::_TakeOff::start(float alt_cm)
 {
     // initialise takeoff state
     _running = true;
-    take_off_start_alt = copter.pos_control->get_pos_target_z_cm();
+    take_off_start_alt = copter.pos_control->get_pos_desired_z_cm();
     take_off_complete_alt  = take_off_start_alt + alt_cm;
 }
 
@@ -87,7 +87,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
         if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || 
             (copter.pos_control->get_z_accel_cmss() >= 0.5 * copter.pos_control->get_max_accel_z_cmss()) ||
             (copter.pos_control->get_vel_desired_cms().z >= constrain_float(pilot_climb_rate_cm, copter.pos_control->get_max_speed_up_cms() * 0.1, copter.pos_control->get_max_speed_up_cms() * 0.5)) || 
-            (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_target_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) {
+            (is_positive(take_off_complete_alt - take_off_start_alt) && copter.pos_control->get_pos_desired_z_cm() - take_off_start_alt > 0.5 * (take_off_complete_alt - take_off_start_alt))) {
             // throttle > 90%
             // acceleration > 50% maximum acceleration
             // velocity > 10% maximum velocity && commanded climb rate
@@ -104,7 +104,7 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm)
 
         // stop take off early and return if negative climb rate is commanded or we are within 0.1% of our take off altitude
         if (is_negative(pilot_climb_rate_cm) ||
-            (take_off_complete_alt  - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_target_z_cm() - take_off_start_alt) {
+            (take_off_complete_alt  - take_off_start_alt) * 0.999f < copter.pos_control->get_pos_desired_z_cm() - take_off_start_alt) {
             stop();
         }
     }
@@ -210,13 +210,13 @@ void _AutoTakeoff::run()
     const float vel_threshold_fraction = 0.1;
     // stopping distance from vel_threshold_fraction * max velocity
     const float stop_distance = 0.5 * sq(vel_threshold_fraction * copter.pos_control->get_max_speed_up_cms()) / copter.pos_control->get_max_accel_z_cmss();
-    bool reached_altitude = copter.pos_control->get_pos_target_z_cm() >= pos_z - stop_distance;
+    bool reached_altitude = copter.pos_control->get_pos_desired_z_cm() >= pos_z - stop_distance;
     bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * vel_threshold_fraction;
     complete = reached_altitude && reached_climb_rate;
 
     // calculate completion for location in case it is needed for a smooth transition to wp_nav
     if (complete) {
-        const Vector3p _complete_pos = copter.pos_control->get_pos_target_cm();
+        const Vector3p _complete_pos = copter.pos_control->get_pos_desired_cm();
         complete_pos = Vector3p{_complete_pos.x, _complete_pos.y, pos_z};
     }
 }

ArduPlane/mode_qloiter.cpp

@@ -48,7 +48,7 @@ void ModeQLoiter::run()
         // we have an active landing target override
         Vector2f rel_origin;
         if (plane.next_WP_loc.get_vector_xy_from_origin_NE(rel_origin)) {
-            quadplane.pos_control->set_pos_target_xy_cm(rel_origin.x, rel_origin.y);
+            quadplane.pos_control->set_pos_desired_xy_cm(rel_origin);
             last_target_loc_set_ms = 0;
         }
     }

ArduPlane/quadplane.cpp

@@ -3606,7 +3606,7 @@ void QuadPlane::Log_Write_QControl_Tuning()
     float des_alt_m = 0.0f;
     int16_t target_climb_rate_cms = 0;
     if (plane.control_mode != &plane.mode_qstabilize) {
-        des_alt_m = pos_control->get_pos_target_z_cm() * 0.01f;
+        des_alt_m = pos_control->get_pos_desired_z_cm() * 0.01f;
         target_climb_rate_cms = pos_control->get_vel_target_z_cms();
     }
 
@@ -3899,7 +3899,7 @@ bool QuadPlane::guided_mode_enabled(void)
  */
 void QuadPlane::set_alt_target_current(void)
 {
-    pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm());
+    pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm());
 }
 
 // user initiated takeoff for guided mode

ArduSub/GCS_Mavlink.cpp

@@ -815,7 +815,7 @@ void GCS_MAVLINK_Sub::handle_message(const mavlink_message_t &msg)
          */
 
         if (!z_ignore && sub.control_mode == Mode::Number::ALT_HOLD) { // Control only target depth when in ALT_HOLD
-            sub.pos_control.set_pos_target_z_cm(packet.alt*100);
+            sub.pos_control.set_pos_desired_z_cm(packet.alt*100);
             break;
         }
 

ArduSub/mode_acro.cpp

@@ -7,7 +7,7 @@
 
 bool ModeAcro::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in acro mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

ArduSub/mode_althold.cpp

@@ -111,9 +111,9 @@ void ModeAlthold::control_depth() {
     //we allow full control to the pilot, but as soon as there's no input, we handle being at surface/bottom
     if (fabsf(target_climb_rate_cm_s) < 0.05f)  {
         if (sub.ap.at_surface) {
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f)); // set target to 5 cm below surface level
         } else if (sub.ap.at_bottom) {
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm())); // set target to 10 cm above bottom
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm())); // set target to 10 cm above bottom
         }
     }
 

ArduSub/mode_manual.cpp

@@ -3,7 +3,7 @@
 
 bool ModeManual::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
 
     // attitude hold inputs become thrust inputs in manual mode
     // set to neutral to prevent chaotic behavior (esp. roll/pitch)

ArduSub/mode_stabilize.cpp

@@ -3,7 +3,7 @@
 
 bool ModeStabilize::init(bool ignore_checks) {
     // set target altitude to zero for reporting
-    position_control->set_pos_target_z_cm(0);
+    position_control->set_pos_desired_z_cm(0);
     sub.last_pilot_heading = ahrs.yaw_sensor;
 
     return true;

ArduSub/mode_surftrak.cpp

@@ -81,8 +81,7 @@ bool ModeSurftrak::set_rangefinder_target_cm(float target_cm)
 
         // Initialize the terrain offset
         auto terrain_offset_cm = sub.inertial_nav.get_position_z_up_cm() - rangefinder_target_cm;
-        sub.pos_control.set_pos_offset_z_cm(terrain_offset_cm);
-        sub.pos_control.set_pos_offset_target_z_cm(terrain_offset_cm);
+        sub.pos_control.init_pos_terrain_cm(terrain_offset_cm);
 
     } else {
         reset();
@@ -97,8 +96,7 @@ void ModeSurftrak::reset()
     rangefinder_target_cm = INVALID_TARGET;
 
     // Reset the terrain offset
-    sub.pos_control.set_pos_offset_z_cm(0);
-    sub.pos_control.set_pos_offset_target_z_cm(0);
+    sub.pos_control.init_pos_terrain_cm(0);
 }
 
 /*
@@ -117,11 +115,11 @@ void ModeSurftrak::control_range() {
         }
         if (sub.ap.at_surface) {
             // Set target depth to 5 cm below SURFACE_DEPTH and reset
-            position_control->set_pos_target_z_cm(MIN(position_control->get_pos_target_z_cm(), g.surface_depth - 5.0f));
+            position_control->set_pos_desired_z_cm(MIN(position_control->get_pos_desired_z_cm(), g.surface_depth - 5.0f));
             reset();
         } else if (sub.ap.at_bottom) {
             // Set target depth to 10 cm above bottom and reset
-            position_control->set_pos_target_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_target_z_cm()));
+            position_control->set_pos_desired_z_cm(MAX(inertial_nav.get_position_z_up_cm() + 10.0f, position_control->get_pos_desired_z_cm()));
             reset();
         } else {
             // Typical operation
@@ -164,7 +162,7 @@ void ModeSurftrak::update_surface_offset()
             }
 
             // Set the offset target, AC_PosControl will do the rest
-            sub.pos_control.set_pos_offset_target_z_cm(rangefinder_terrain_offset_cm);
+            sub.pos_control.set_pos_terrain_target_cm(rangefinder_terrain_offset_cm);
         }
     }
 #endif  // AP_RANGEFINDER_ENABLED