From 9ee28542da635d0efb38f91b85e5f64a7a65c1f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Sep 2021 21:30:25 +0900 Subject: [PATCH] Copter: guided supports using wpnav to reach position targets --- ArduCopter/GCS_Mavlink.cpp | 1 + ArduCopter/Parameters.cpp | 2 +- ArduCopter/mode.h | 7 ++ ArduCopter/mode_guided.cpp | 147 ++++++++++++++++++++++++++++++++++--- 4 files changed, 146 insertions(+), 11 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index f151e0a8e4..322b09be6f 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -138,6 +138,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_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 607498f073..d30dab7d4b 100644 --- a/ArduCopter/Parameters.cpp +++ b/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 diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 69f5ff4479..4e900bbb38 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -866,6 +866,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(); @@ -879,6 +880,7 @@ public: enum class SubMode { TakeOff, WP, + Pos, PosVelAccel, VelAccel, Accel, @@ -912,8 +914,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(); diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 672728aa72..5e514e24c9 100644 --- a/ArduCopter/mode_guided.cpp +++ b/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) { - 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: