From e7c25286543b0623a41b5cb1e6077e2334d0fe61 Mon Sep 17 00:00:00 2001 From: sahith22 <47216915+sahith22@users.noreply.github.com> Date: Mon, 28 Oct 2019 19:07:13 +0530 Subject: [PATCH] Rover: Added reversing loiter_type parameter --- APMrover2/Parameters.cpp | 2 +- APMrover2/mode_loiter.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index c25167db94..ddc077c385 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -511,7 +511,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: LOIT_TYPE // @DisplayName: Loiter type // @Description: Loiter behaviour when moving to the target point - // @Values: 0:Forward or reverse to target point,1:Always face bow towards target point + // @Values: 0:Forward or reverse to target point,1:Always face bow towards target point,2:Always face stern towards target point // @User: Standard AP_GROUPINFO("LOIT_TYPE", 25, ParametersG2, loit_type, 0), diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp index f6918ab641..a30b0f1ea5 100644 --- a/APMrover2/mode_loiter.cpp +++ b/APMrover2/mode_loiter.cpp @@ -44,7 +44,7 @@ void ModeLoiter::update() _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination); float 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 && g2.loit_type == 0) { + if ((fabsf(yaw_error_cd) > 9000 && g2.loit_type == 0) || g2.loit_type == 2){ _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;