Copter: support set_posvelaccel_offset in auto

Co-authored-by: Leonard Hall <leonardthall@gmail.com>

ArduCopter/Copter.h

@@ -266,9 +266,9 @@ private:
         //   measured ground or ceiling level measured using the range finder.
         void update_surface_offset();
 
-        // get/set target altitude (in cm) above ground
+        // target has already been set by terrain following so do not initalise again
-        bool get_target_alt_cm(float &target_alt_cm) const;
+        // this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
-        void set_target_alt_cm(float target_alt_cm);
+        void external_init();
 
         // get target and actual distances (in m) for logging purposes
         bool get_target_dist_for_logging(float &target_dist) const;

ArduCopter/mode.h

@@ -182,12 +182,6 @@ public:
     // altitude fence
     float get_avoidance_adjusted_climbrate(float target_rate);
 
-    const Vector3f& get_vel_desired_cms() {
-        // note that position control isn't used in every mode, so
-        // this may return bogus data:
-        return pos_control->get_vel_desired_cms();
-    }
-
     // send output to the motors, can be overridden by subclasses
     virtual void output_to_motors();
 
@@ -651,6 +645,10 @@ private:
 
     bool shift_alt_to_current_alt(Location& target_loc) const;
 
+    // subtract position controller offsets from target location
+    // should be used when the location will be used as a target for the position controller
+    void subtract_pos_offsets(Location& target_loc) const;
+
     void do_takeoff(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool set_next_wp(const AP_Mission::Mission_Command& current_cmd, const Location &default_loc);

ArduCopter/mode_auto.cpp

@@ -1419,7 +1419,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;
         }
@@ -1472,6 +1472,8 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
         (wp_nav->get_terrain_source() == AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER)) {
         int32_t curr_rngfnd_alt_cm;
         if (copter.get_rangefinder_height_interpolated_cm(curr_rngfnd_alt_cm)) {
+            // subtract position offset (if any)
+            curr_rngfnd_alt_cm -= pos_control->get_pos_offset_z_cm();
             // wp_nav is using rangefinder so use current rangefinder alt
             target_loc.set_alt_cm(MAX(curr_rngfnd_alt_cm, 200), Location::AltFrame::ABOVE_TERRAIN);
             return true;
@@ -1486,11 +1488,21 @@ bool ModeAuto::shift_alt_to_current_alt(Location& target_loc) const
         return false;
     }
 
-    // set target_loc's alt
+    // set target_loc's alt minus position offset (if any)
-    target_loc.set_alt_cm(currloc.alt, currloc.get_alt_frame());
+    target_loc.set_alt_cm(currloc.alt - pos_control->get_pos_offset_z_cm(), currloc.get_alt_frame());
     return true;
 }
 
+// subtract position controller offsets from target location
+// should be used when the location will be used as a target for the position controller
+void ModeAuto::subtract_pos_offsets(Location& target_loc) const
+{
+    // subtract position controller offsets from target location
+    const Vector3p& pos_ofs_neu_cm = pos_control->get_pos_offset_cm();
+    Vector3p pos_ofs_ned_m = Vector3p{pos_ofs_neu_cm.x * 0.01, pos_ofs_neu_cm.y * 0.01, -pos_ofs_neu_cm.z * 0.01};
+    target_loc.offset(-pos_ofs_ned_m);
+}
+
 /********************************************************************************/
 // Nav (Must) commands
 /********************************************************************************/
@@ -1531,6 +1543,10 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
     // calculate default location used when lat, lon or alt is zero
     Location default_loc = copter.current_loc;
+
+    // subtract position offsets
+    subtract_pos_offsets(default_loc);
+
     if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
         if (!wp_nav->get_wp_destination_loc(default_loc)) {
             // this should never happen
@@ -1651,33 +1667,23 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
 // note: caller should set yaw_mode
 void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 {
-    // convert back to location
+    // calculate default location used when lat, lon or alt is zero
-    Location target_loc(cmd.content.location);
+    Location default_loc = copter.current_loc;
 
-    // use current location if not provided
+    // subtract position offsets
-    if (target_loc.lat == 0 && target_loc.lng == 0) {
+    subtract_pos_offsets(default_loc);
-        // To-Do: make this simpler
-        Vector3f temp_pos;
-        copter.wp_nav->get_wp_stopping_point_xy(temp_pos.xy());
-        const Location temp_loc(temp_pos, Location::AltFrame::ABOVE_ORIGIN);
-        target_loc.lat = temp_loc.lat;
-        target_loc.lng = temp_loc.lng;
-    }
 
-    // use current altitude if not provided
+    // use previous waypoint destination as default if available
-    // To-Do: use z-axis stopping point instead of current alt
+    if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
-    if (target_loc.alt == 0) {
+        if (!wp_nav->get_wp_destination_loc(default_loc)) {
-        // set to current altitude but in command's alt frame
+            // this should never happen
-        int32_t curr_alt;
+            INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
-        if (copter.current_loc.get_alt_cm(target_loc.get_alt_frame(),curr_alt)) {
-            target_loc.set_alt_cm(curr_alt, target_loc.get_alt_frame());
-        } else {
-            // default to current altitude as alt-above-home
-            target_loc.set_alt_cm(copter.current_loc.alt,
-                                  copter.current_loc.get_alt_frame());
         }
     }
 
+    // get waypoint's location from command and send to wp_nav
+    const Location target_loc = loc_from_cmd(cmd, default_loc);
+
     // start way point navigator and provide it the desired location
     if (!wp_start(target_loc)) {
         // failure to set next destination can only be because of missing terrain data
@@ -1689,7 +1695,13 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
 // do_circle - initiate moving in a circle
 void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 {
-    const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
+    // calculate default location used when lat, lon or alt is zero
+    Location default_loc = copter.current_loc;
+
+    // subtract position offsets
+    subtract_pos_offsets(default_loc);
+
+    const Location circle_center = loc_from_cmd(cmd, default_loc);
 
     // calculate radius
     uint16_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
@@ -1757,6 +1769,10 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
 {
     // calculate default location used when lat, lon or alt is zero
     Location default_loc = copter.current_loc;
+
+    // subtract position offsets
+    subtract_pos_offsets(default_loc);
+
     if (wp_nav->is_active() && wp_nav->reached_wp_destination()) {
         if (!wp_nav->get_wp_destination_loc(default_loc)) {
             // this should never happen

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();
@@ -825,8 +825,7 @@ void ModeGuided::velaccel_control_run()
 
     if (!stabilizing_vel_xy() && !do_avoid) {
         // set the current commanded xy vel to the desired vel
-        guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
+        guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy();
-        guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
     }
     pos_control->input_vel_accel_xy(guided_vel_target_cms.xy(), guided_accel_target_cmss.xy(), false);
     if (!stabilizing_vel_xy() && !do_avoid) {
@@ -903,14 +902,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.x = pos_control->get_pos_target_cm().x;
+        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
-        guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
+        guided_vel_target_cms.xy() = pos_control->get_vel_desired_cms().xy();
-        guided_vel_target_cms.x = pos_control->get_vel_desired_cms().x;
-        guided_vel_target_cms.y = pos_control->get_vel_desired_cms().y;
     } else if (!stabilizing_pos_xy()) {
         // set the current commanded xy pos to the target pos
-        guided_pos_target_cm.x = pos_control->get_pos_target_cm().x;
+        guided_pos_target_cm.xy() = pos_control->get_pos_desired_cm().xy();
-        guided_pos_target_cm.y = pos_control->get_pos_target_cm().y;
     }
     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

@@ -245,8 +245,8 @@ void ModeZigZag::return_to_manual_control(bool maintain_target)
             const Vector3f& wp_dest = wp_nav->get_wp_destination();
             loiter_nav->init_target(wp_dest.xy());
 #if AP_RANGEFINDER_ENABLED
-            if (wp_nav->origin_and_destination_are_terrain_alt()) {
+            if (copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy()) {
-                copter.surface_tracking.set_target_alt_cm(wp_dest.z);
+                copter.surface_tracking.external_init();
             }
 #endif
         } else {
@@ -455,16 +455,10 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
         terrain_alt = wp_nav->origin_and_destination_are_terrain_alt();
         next_dest.z = wp_nav->get_wp_destination().z;
     } else {
-        // if we have a downward facing range finder then use terrain altitude targets
         terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
-        if (terrain_alt) {
+        next_dest.z = pos_control->get_pos_desired_z_cm();
-#if AP_RANGEFINDER_ENABLED
+        if (!terrain_alt) {
-            if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
+            next_dest.z += pos_control->get_pos_terrain_cm();
-                next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
-            }
-#endif
-        } else {
-            next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : inertial_nav.get_position_z_up_cm();
         }
     }
 
@@ -506,20 +500,11 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
 
     // get distance from vehicle to start_pos
     const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()};
-    next_dest.x = curr_pos2d.x + (AB_side.x * scalar);
+    next_dest.xy() = curr_pos2d + (AB_side * scalar);
-    next_dest.y = curr_pos2d.y + (AB_side.y * scalar);
 
     // if we have a downward facing range finder then use terrain altitude targets
     terrain_alt = copter.rangefinder_alt_ok() && wp_nav->rangefinder_used_and_healthy();
-    if (terrain_alt) {
+    next_dest.z = pos_control->get_pos_desired_z_cm();
-#if AP_RANGEFINDER_ENABLED
-        if (!copter.surface_tracking.get_target_alt_cm(next_dest.z)) {
-            next_dest.z = copter.rangefinder_state.alt_cm_filt.get();
-        }
-#endif
-    } else {
-        next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_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;
         }
@@ -36,40 +36,22 @@ void Copter::SurfaceTracking::update_surface_offset()
     } else {
         // reset position controller offsets if surface tracking is inactive
         // flag target should be reset when/if it next becomes active
-        if (timeout) {
+        if (timeout && !reset_target) {
-            copter.pos_control->set_pos_offset_z_cm(0);
+            copter.pos_control->init_pos_terrain_cm(0);
-            copter.pos_control->set_pos_offset_target_z_cm(0);
+            valid_for_logging = false;
             reset_target = true;
         }
     }
 }
 
-
+// target has already been set by terrain following so do not initalise again
-// get target altitude (in cm) above ground
+// this should be called by flight modes when switching from terrain following to surface tracking (e.g. ZigZag)
-// returns true if there is a valid target
+void Copter::SurfaceTracking::external_init()
-bool Copter::SurfaceTracking::get_target_alt_cm(float &target_alt_cm) const
 {
-    // fail if we are not tracking downwards
+    if ((surface == Surface::GROUND) && copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0)) {
-    if (surface != Surface::GROUND) {
+        last_update_ms = millis();
-        return false;
+        reset_target = false;
     }
-    // check target has been updated recently
-    if (AP_HAL::millis() - last_update_ms > SURFACE_TRACKING_TIMEOUT_MS) {
-        return false;
-    }
-    target_alt_cm = (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm());
-    return true;
-}
-
-// set target altitude (in cm) above ground
-void Copter::SurfaceTracking::set_target_alt_cm(float _target_alt_cm)
-{
-    // fail if we are not tracking downwards
-    if (surface != Surface::GROUND) {
-        return;
-    }
-    copter.pos_control->set_pos_offset_z_cm(copter.inertial_nav.get_position_z_up_cm() - _target_alt_cm);
-    last_update_ms = AP_HAL::millis();
 }
 
 bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) const
@@ -79,7 +61,7 @@ bool Copter::SurfaceTracking::get_target_dist_for_logging(float &target_dist) co
     }
 
     const float dir = (surface == Surface::GROUND) ? 1.0f : -1.0f;
-    target_dist = dir * (copter.pos_control->get_pos_target_z_cm() - copter.pos_control->get_pos_offset_z_cm()) * 0.01f;
+    target_dist = dir * copter.pos_control->get_pos_desired_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};
     }
 }