From defdeaed95a1cf80afdb680207f46eb17c8b3114 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 28 Mar 2018 13:54:24 +0900 Subject: [PATCH] Sub: integrate AC_Loiter --- ArduSub/Parameters.cpp | 4 ++++ ArduSub/Parameters.h | 1 + ArduSub/Sub.cpp | 1 + ArduSub/Sub.h | 2 ++ ArduSub/control_auto.cpp | 6 +++--- ArduSub/control_poshold.cpp | 8 ++++---- ArduSub/system.cpp | 1 + 7 files changed, 16 insertions(+), 7 deletions(-) diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 9388ad5b48..7c8037c900 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -445,6 +445,10 @@ const AP_Param::Info Sub::var_info[] = { // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp GOBJECT(wp_nav, "WPNAV_", AC_WPNav), + // @Group: LOIT_ + // @Path: ../libraries/AC_WPNav/AC_Loiter.cpp + GOBJECT(loiter_nav, "LOITER_", AC_Loiter), + #if CIRCLE_NAV_ENABLED == ENABLED // @Group: CIRCLE_ // @Path: ../libraries/AC_WPNav/AC_Circle.cpp diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index a3030dd828..5226f471dc 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -96,6 +96,7 @@ public: k_param_circle_nav, // Disabled k_param_avoid, // Relies on proximity and fence k_param_NavEKF3, + k_param_loiter_nav, // Other external hardware interfaces diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 4416b75209..3b658922cd 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -50,6 +50,7 @@ Sub::Sub(void) attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS), pos_control(ahrs_view, inertial_nav, motors, attitude_control), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), + loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), in_mavlink_delay(false), param_loader(var_info), diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index b8d90c4ec1..45e60494b2 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -64,6 +64,7 @@ #include // needed for AHRS build #include // ArduPilot Mega inertial navigation library #include // Waypoint navigation library +#include #include // circle navigation library #include // Fence library #include // main loop scheduler @@ -396,6 +397,7 @@ private: AC_PosControl_Sub pos_control; AC_WPNav wp_nav; + AC_Loiter loiter_nav; AC_Circle circle_nav; // Reference to the relay object diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index b9fa2df8d6..44e0b59145 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -602,7 +602,7 @@ float Sub::get_auto_heading(void) float track_bearing = get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); // Bearing from current position towards intermediate position target (centidegrees) - float desired_angle = wp_nav.get_loiter_bearing_to_target(); + float desired_angle = pos_control.get_bearing_to_target(); float angle_error = wrap_180_cd(desired_angle - track_bearing); float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); @@ -646,7 +646,7 @@ bool Sub::auto_terrain_recover_start() mission.stop(); // Reset xy target - wp_nav.init_loiter_target(); + loiter_nav.init_target(); // Reset z axis controller pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); @@ -733,7 +733,7 @@ void Sub::auto_terrain_recover_run() } // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler); /////////////////////// // update xy targets // diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 2413e0091f..b856e5b740 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -24,7 +24,7 @@ bool Sub::poshold_init() // set target to current position // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I - wp_nav.init_loiter_target(); + loiter_nav.init_target(); last_pilot_heading = ahrs.yaw_sensor; @@ -40,7 +40,7 @@ void Sub::poshold_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - wp_nav.init_loiter_target(); + loiter_nav.init_target(); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); return; @@ -50,7 +50,7 @@ void Sub::poshold_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run loiter controller - wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); + loiter_nav.update(ekfGndSpdLimit, ekfNavVelGainScaler); /////////////////////// // update xy outputs // @@ -64,7 +64,7 @@ void Sub::poshold_run() if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { lateral_out = pilot_lateral; forward_out = pilot_forward; - wp_nav.init_loiter_target(); // initialize target to current position after repositioning + loiter_nav.init_target(); // initialize target to current position after repositioning } else { translate_wpnav_rp(lateral_out, forward_out); } diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp index b8654843c8..8f6f2c8b2a 100644 --- a/ArduSub/system.cpp +++ b/ArduSub/system.cpp @@ -124,6 +124,7 @@ void Sub::init_ardupilot() #if AVOIDANCE_ENABLED == ENABLED wp_nav.set_avoidance(&avoid); + loiter_nav.set_avoidance(&avoid); #endif pos_control.set_dt(MAIN_LOOP_SECONDS);