From 2018af319f4fbaa25ab21a546125f272a6f5f000 Mon Sep 17 00:00:00 2001 From: nagezeng Date: Tue, 28 Jun 2022 16:40:36 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B5=B7=E9=99=8D=E7=8A=B6=E6=80=81=E8=B0=83?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_auto.cpp | 6 +++--- ArduCopter/version.h | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index af79443e66..32f760ed64 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -228,7 +228,7 @@ void ModeAuto::wp_start(const Location& dest_loc) if (auto_yaw.mode() != AUTO_YAW_ROI) { auto_yaw.set_mode_to_default(false); } - if (copter.updownStatus < UpDown_InMission) + if (copter.updownStatus != UpDown_InMission) { copter.updownStatus = UpDown_InMission; copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus); @@ -769,7 +769,7 @@ void ModeAuto::takeoff_run() if (motors->armed() || copter.ap.auto_armed) { if(g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 && \ - copter.updownStatus < UpDown_RequestClimb && \ + copter.updownStatus != UpDown_RequestClimb && \ updown_state == 2) { updown_state = 3; // 到达悬停高度,状态更新到等待 @@ -805,7 +805,7 @@ void ModeAuto::takeoff_run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0); return; - }else if(copter.updownStatus > UpDown_RequestClimb){ + }else if(copter.updownStatus == UpDown_ContinueClimb){ // 地面站弹框确认,发送状态3 if(updown_state < 4){ // 地面站弹框确定,发送新的值,状态位同步更新 updown_state = 4; takeoff_start(last_loc); diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 48fa39b0e8..6dd83fc01f 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -5,7 +5,7 @@ #endif #include "ap_version.h" -#define GIT_VERSION "3" +#define GIT_VERSION "4" #define THISFIRMWARE "ZRUAV v4.1.19" //"ArduCopter V4.0.0" // the following line is parsed by the autotest scripts