Copter: guided supports using wpnav to reach position targets

ArduCopter/GCS_Mavlink.cpp

@@ -129,6 +129,7 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned()
         return;
     case ModeGuided::SubMode::TakeOff:
     case ModeGuided::SubMode::WP:
+    case ModeGuided::SubMode::Pos:
         type_mask = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
                     POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
                     POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position

ArduCopter/Parameters.cpp

@@ -1017,7 +1017,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: GUID_OPTIONS
     // @DisplayName: Guided mode options
     // @Description: Options that can be applied to change guided mode behaviour
-    // @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY
+    // @Bitmask: 0:Allow Arming from Transmitter,2:Ignore pilot yaw,3:SetAttitudeTarget interprets Thrust As Thrust,4:Do not stabilize PositionXY,5:Do not stabilize VelocityXY,6:Waypoint navigation used for position targets
     // @User: Advanced
     AP_GROUPINFO("GUID_OPTIONS", 41, ParametersG2, guided_options, 0),
 #endif

ArduCopter/mode.h

@@ -895,6 +895,7 @@ public:
     bool set_attitude_target_provides_thrust() const;
     bool stabilizing_pos_xy() const;
     bool stabilizing_vel_xy() const;
+    bool use_wpnav_for_position_control() const;
 
     void limit_clear();
     void limit_init_time_and_pos();
@@ -908,6 +909,7 @@ public:
     enum class SubMode {
         TakeOff,
         WP,
+        Pos,
         PosVelAccel,
         VelAccel,
         Accel,
@@ -943,8 +945,13 @@ private:
         SetAttitudeTarget_ThrustAsThrust = (1U << 3),
         DoNotStabilizePositionXY = (1U << 4),
         DoNotStabilizeVelocityXY = (1U << 5),
+        WPNavUsedForPosControl = (1U << 6),
     };
 
+    // wp controller
+    void wp_control_start();
+    void wp_control_run();
+
     void pva_control_start();
     void pos_control_start();
     void accel_control_start();

ArduCopter/mode_guided.cpp

@@ -61,14 +61,19 @@ void ModeGuided::run()
         break;
 
     case SubMode::WP:
-        // run position controller
-        pos_control_run();
+        // run waypoint controller
+        wp_control_run();
         if (send_notification && wp_nav->reached_wp_destination()) {
             send_notification = false;
             gcs().send_mission_item_reached_message(0);
         }
         break;
 
+    case SubMode::Pos:
+        // run position controller
+        pos_control_run();
+        break;
+
     case SubMode::Accel:
         accel_control_run();
         break;
@@ -136,6 +141,68 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     return true;
 }
 
+// initialise guided mode's waypoint navigation controller
+void ModeGuided::wp_control_start()
+{
+    // set to position control mode
+    guided_mode = SubMode::WP;
+
+    // initialise waypoint and spline controller
+    wp_nav->wp_and_spline_init();
+
+    // initialise wpnav to stopping point
+    Vector3f stopping_point;
+    wp_nav->get_wp_stopping_point(stopping_point);
+
+    // no need to check return status because terrain data is not used
+    wp_nav->set_wp_destination(stopping_point, false);
+
+    // initialise yaw
+    auto_yaw.set_mode_to_default(false);
+}
+
+// run guided mode's waypoint navigation controller
+void ModeGuided::wp_control_run()
+{
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!copter.failsafe.radio && use_pilot_yaw()) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
+        if (!is_zero(target_yaw_rate)) {
+            auto_yaw.set_mode(AUTO_YAW_HOLD);
+        }
+    }
+
+    // if not armed set throttle to zero and exit immediately
+    if (is_disarmed_or_landed()) {
+        // do not spool down tradheli when on the ground with motor interlock enabled
+        make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
+        return;
+    }
+
+    // set motors to full range
+    motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
+
+    // run waypoint controller
+    copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
+
+    // call z-axis position controller (wpnav should have already updated it's alt target)
+    pos_control->update_z_controller();
+
+    // call attitude controller
+    if (auto_yaw.mode() == AUTO_YAW_HOLD) {
+        // roll & pitch from waypoint controller, yaw rate from pilot
+        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate);
+    } else if (auto_yaw.mode() == AUTO_YAW_RATE) {
+        // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item
+        attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds());
+    } else {
+        // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading()
+        attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw());
+    }
+}
+
 // initialise position controller
 void ModeGuided::pva_control_start()
 {
@@ -162,7 +229,7 @@ void ModeGuided::pva_control_start()
 void ModeGuided::pos_control_start()
 {
     // set to position control mode
-    guided_mode = SubMode::WP;
+    guided_mode = SubMode::Pos;
 
     // initialise position controller
     pva_control_start();
@@ -246,8 +313,28 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
     }
 #endif
 
+    // if configured to use wpnav for position control
+    if (use_wpnav_for_position_control()) {
+        // ensure we are in position control mode
+        if (guided_mode != SubMode::WP) {
+            wp_control_start();
+        }
+
+        // set yaw state
+        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+
+        // no need to check return status because terrain data is not used
+        wp_nav->set_wp_destination(destination, terrain_alt);
+
+        // log target
+        copter.Log_Write_GuidedTarget(guided_mode, destination, terrain_alt, Vector3f(), Vector3f());
+        send_notification = true;
+        return true;
+    }
+
+    // if configured to use position controller for position control
     // ensure we are in position control mode
-    if (guided_mode != SubMode::WP) {
+    if (guided_mode != SubMode::Pos) {
         pos_control_start();
     }
 
@@ -289,11 +376,18 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
 
 bool ModeGuided::get_wp(Location& destination) const
 {
-    if (guided_mode != SubMode::WP) {
+    switch (guided_mode) {
+    case SubMode::WP:
+        return wp_nav->get_oa_wp_destination(destination);
+    case SubMode::Pos:
+        destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
+        return true;
+    default:
         return false;
     }
-    destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
-    return true;
+
+    // should never get here but just in case
+    return false;
 }
 
 // sets guided mode's target from a Location object
@@ -311,8 +405,31 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
     }
 #endif
 
+    // if using wpnav for position control
+    if (use_wpnav_for_position_control()) {
+        if (guided_mode != SubMode::WP) {
+            wp_control_start();
+        }
+
+        if (!wp_nav->set_wp_destination_loc(dest_loc)) {
+            // failure to set destination can only be because of missing terrain data
+            AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
+            // failure is propagated to GCS with NAK
+            return false;
+        }
+
+        // set yaw state
+        set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
+
+        // log target
+        copter.Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt), (dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN), Vector3f(), Vector3f());
+        send_notification = true;
+        return true;
+    }
+
+    // if configured to use position controller for position control
     // ensure we are in position control mode
-    if (guided_mode != SubMode::WP) {
+    if (guided_mode != SubMode::Pos) {
         pos_control_start();
     }
 
@@ -467,6 +584,12 @@ bool ModeGuided::stabilizing_vel_xy() const
     return !((copter.g2.guided_options.get() & uint32_t(Options::DoNotStabilizeVelocityXY)) != 0);
 }
 
+// returns true if GUIDED_OPTIONS param specifies waypoint navigation should be used for position control (allow path planning to be used but updates must be slower)
+bool ModeGuided::use_wpnav_for_position_control() const
+{
+    return ((copter.g2.guided_options.get() & uint32_t(Options::WPNavUsedForPosControl)) != 0);
+}
+
 // set guided mode angle target and climbrate
 void ModeGuided::set_angle(const Quaternion &q, float climb_rate_cms_or_thrust, bool use_yaw_rate, float yaw_rate_rads, bool use_thrust)
 {
@@ -988,8 +1111,9 @@ uint32_t ModeGuided::wp_distance() const
 {
     switch(guided_mode) {
     case SubMode::WP:
+        return wp_nav->get_wp_distance_to_destination();
+    case SubMode::Pos:
         return norm(guided_pos_target_cm.x - inertial_nav.get_position().x, guided_pos_target_cm.y - inertial_nav.get_position().y);
-        break;
     case SubMode::PosVelAccel:
         return pos_control->get_pos_error_xy_cm();
         break;
@@ -1002,8 +1126,9 @@ int32_t ModeGuided::wp_bearing() const
 {
     switch(guided_mode) {
     case SubMode::WP:
+        return wp_nav->get_wp_bearing_to_destination();
+    case SubMode::Pos:
         return get_bearing_cd(inertial_nav.get_position(), guided_pos_target_cm.tofloat());
-        break;
     case SubMode::PosVelAccel:
         return pos_control->get_bearing_to_target_cd();
         break;
@@ -1022,6 +1147,8 @@ float ModeGuided::crosstrack_error() const
 {
     switch (guided_mode) {
     case SubMode::WP:
+        return wp_nav->crosstrack_error();
+    case SubMode::Pos:
     case SubMode::TakeOff:
     case SubMode::Accel:
     case SubMode::VelAccel: