From f0185cb79baccdc1f500bc484d74f0e59110ba49 Mon Sep 17 00:00:00 2001 From: Raouf Date: Tue, 7 Aug 2018 13:22:51 +0900 Subject: [PATCH] Rover: Add loiter behaviour parameter --- APMrover2/Parameters.cpp | 7 +++++++ APMrover2/Parameters.h | 3 +++ APMrover2/mode_loiter.cpp | 2 +- 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index f2847a5d16..b15bdb7c31 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -564,6 +564,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @RebootRequired: True AP_GROUPINFO("FRAME_TYPE", 24, ParametersG2, frame_type, 0), + // @Param: LOIT_TYPE + // @DisplayName: Loiter type + // @Description: Loiter behaviour when around next to a taget point + // @Values: 0:Reverse to target point,1:Always face bow to target point + // @User: Standard + AP_GROUPINFO("LOIT_TYPE", 25, ParametersG2, loit_type, 0), + AP_GROUPEND }; diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 6413415b56..75f92667d7 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -354,6 +354,9 @@ public: // frame type for vehicle (used for vectored motor vehicles and custom motor configs) AP_Int8 frame_type; + + // loiter type + AP_Int8 loit_type; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index b885781524..138d779eab 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -36,7 +36,7 @@ void ModeLoiter::update() _desired_yaw_cd = get_bearing_cd(rover.current_loc, _destination); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); // if destination is behind vehicle, reverse towards it - if (fabsf(_yaw_error_cd) > 9000) { + if (fabsf(_yaw_error_cd) > 9000 && g2.loit_type == 0) { _desired_yaw_cd = wrap_180_cd(_desired_yaw_cd + 18000); _yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor); _desired_speed = -_desired_speed;