From 683122b72be44aaacbc5471cf9c64f498648a16d Mon Sep 17 00:00:00 2001 From: zbr3550 Date: Thu, 15 Oct 2020 09:13:59 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=95=E5=9C=88=E8=88=AA=E7=BA=BF=E6=9C=BA?= =?UTF-8?q?=E5=A4=B4=E6=96=B9=E5=90=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/UserCode.cpp | 1 - ArduCopter/autoyaw.cpp | 2 +- ArduCopter/mode_auto.cpp | 1 + 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 017952cb6c..56f9a6d024 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -51,7 +51,6 @@ void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here zr_SuperSlowLoop(); - } #endif diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index 2c0a527b82..a2deff62e1 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -8,7 +8,7 @@ float Mode::AutoYaw::roi_yaw() roi_yaw_counter++; if (roi_yaw_counter >= 4) { roi_yaw_counter = 0; - _roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi); + _roi_yaw = get_bearing_cd(copter.inertial_nav.get_position(), roi)+27000; } return _roi_yaw; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f344962bdd..a763c740c2 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -448,6 +448,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) break; case MAV_CMD_NAV_LOITER_TURNS: //18 Loiter N Times + do_roi(cmd); do_circle(cmd); break;