From bd615a6892a701db91dd108ee2136b56f9fd02fb Mon Sep 17 00:00:00 2001 From: yaozb Date: Wed, 29 Jul 2020 10:04:10 +0800 Subject: [PATCH 1/3] =?UTF-8?q?=E8=B5=B7=E9=A3=9E=E7=9D=80=E9=99=86?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=80=92=E8=AE=A1=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 1 + ArduCopter/Parameters.h | 8 ++--- ArduCopter/config.h | 4 +++ ArduCopter/mode.cpp | 5 +++- ArduCopter/mode.h | 14 ++++----- ArduCopter/mode_auto.cpp | 28 ++++++++++++++---- ArduCopter/mode_brake.cpp | 32 +++++++++++++++++++- ArduCopter/mode_rtl.cpp | 44 ++++++++++++++++++++++++---- libraries/GCS_MAVLink/GCS.h | 4 +-- libraries/GCS_MAVLink/GCS_Common.cpp | 14 ++++----- modules/mavlink | 2 +- 11 files changed, 122 insertions(+), 34 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4a86d2bed3..f5220fe0ae 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -131,6 +131,7 @@ const AP_Param::Info Copter::var_info[] = { // @User: Standard GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ), + GSCALAR(updown_countdown, "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN), #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT // @DisplayName: RTL Altitude diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9c70540d16..c03fa36ee2 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -387,6 +387,7 @@ public: k_param_land_lock_rpy, k_param_takeoff_alt_req, + k_param_updown_countdown, // the k_param_* space is 9-bits in size // 511: reserved }; @@ -493,11 +494,10 @@ public: AP_Float acro_yaw_p; AP_Float acro_balance_roll; AP_Float acro_balance_pitch; - AP_Int8 acro_trainer; + AP_Int8 acro_trainer; AP_Float acro_rp_expo; - - AP_Int16 takeoff_alt_req; - + AP_Int16 takeoff_alt_req; + AP_Int16 updown_countdown; // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 9bd19347bd..c5345e4476 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -536,6 +536,10 @@ # define TAKEOFF_ALT_REQ 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. #endif +#ifndef UPDOWN_COUNTDOWN +# define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#endif + // RTL Mode #ifndef RTL_ALT_FINAL # define RTL_ALT_FINAL 0 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 00f30c0d77..8b9c3f3f8e 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -824,7 +824,10 @@ void Copter::update_updownStatus2(uint8_t status) if (status == 3) { set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); - gcs().update_zr_fly_status(status); + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = 255; + gcs().update_zr_fly_status(&zr_flying_status_t); } } diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 73c6da0d99..661c446ef2 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -7,9 +7,7 @@ class ParametersG2; class GCS_Copter; class Mode { - public: - // Auto Pilot Modes enumeration enum class Number : uint8_t { STABILIZE = 0, // manual airframe angle with manual throttle @@ -37,7 +35,6 @@ public: SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers AUTOROTATE = 26, // Autonomous autorotation }; - // constructor Mode(void); @@ -134,7 +131,9 @@ protected: RC_Channel *&channel_throttle; RC_Channel *&channel_yaw; float &G_Dt; - + int16_t countdown ; + uint32_t last_takeoff_request_time; + bool is_countdown_enable = true; // note that we support two entirely different automatic takeoffs: // "user-takeoff", which is available in modes such as ALT_HOLD @@ -331,7 +330,6 @@ public: bool init(bool ignore_checks) override; void run() override; - bool requires_GPS() const override { return true; } bool has_manual_throttle() const override { return false; } bool allows_arming(bool from_gcs) const override { return false; }; @@ -480,6 +478,7 @@ private: int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) uint32_t condition_start; uint32_t last_time_send_updownload; + LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending) struct { @@ -1038,7 +1037,7 @@ protected: void land_run(bool disarm_on_land); void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; } - + private: void climb_start(); @@ -1048,10 +1047,11 @@ private: void loiterathome_run(); void build_path(); void compute_return_target(); - + RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc) bool _state_complete = false; // set to true if the current state is completed + uint32_t last_decent_time =0; struct { // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain Location origin_point; diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 885c8ec10c..9c4c2257e0 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -238,7 +238,10 @@ void ModeAuto::wp_start(const Location& dest_loc) if (copter.updownStatus < UpDown_InMission) { copter.updownStatus = UpDown_InMission; - gcs().update_zr_fly_status(copter.updownStatus); + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = 255; + gcs().update_zr_fly_status(&zr_flying_status_t); } } @@ -768,17 +771,30 @@ void ModeAuto::takeoff_run() if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req && copter.updownStatus < UpDown_RequestClimb) { - copter.updownStatus = UpDown_RequestClimb; - set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); - gcs().update_zr_fly_status(copter.updownStatus); + copter.updownStatus = UpDown_RequestClimb; + countdown = g.updown_countdown; + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = countdown; + gcs().update_zr_fly_status(&zr_flying_status_t); + last_takeoff_request_time = AP_HAL::millis(); + if(g.updown_countdown>0){ + is_countdown_enable =true; + }else{ + is_countdown_enable =false; + } + set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); } else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0 &&(AP_HAL::millis()-last_time_send_updownload>500)) { last_time_send_updownload = AP_HAL::millis(); copter.updownStatus = UpDown_TakeOffClimb; - gcs().update_zr_fly_status(copter.updownStatus); - } + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = 255; + gcs().update_zr_fly_status(&zr_flying_status_t); + } } } auto_takeoff_run(); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 02f6dd6d4c..550e80dd42 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -23,7 +23,15 @@ bool ModeBrake::init(bool ignore_checks) } _timeout_ms = 0; - + if(copter.updownStatus == UpDown_RequestClimb){ + countdown = g.updown_countdown; + } + + if(g.updown_countdown>0){ + is_countdown_enable =true; + }else{ + is_countdown_enable =false; + } return true; } @@ -67,6 +75,28 @@ void ModeBrake::run() copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT); } } + + if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable) + { + if((AP_HAL::millis()-last_takeoff_request_time)>1000) + { + last_takeoff_request_time = AP_HAL::millis(); + countdown --; + // mavlink_zr_flying_status_t zr_flying_status_t; + // zr_flying_status_t.updown_status = copter.updownStatus; + // zr_flying_status_t.countdown = countdown; + // gcs().update_zr_fly_status(&zr_flying_status_t); + if(countdown>0){ + gcs().send_text(MAV_SEVERITY_INFO,"继续任务倒计时(秒)：%d",countdown); + } + else if(countdown<=0){ + gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务"); + copter.updownStatus == UpDown_ContinueClimb; + set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); + } + } + } + } void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index f6160105de..65973331bd 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -70,10 +70,38 @@ void ModeRTL::run(bool disarm_on_land) if (copter.updownStatus < UpDown_RequestDescent) { copter.updownStatus = UpDown_RequestDescent; - gcs().update_zr_fly_status(copter.updownStatus); + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + countdown = g.updown_countdown; + zr_flying_status_t.countdown = countdown; + gcs().update_zr_fly_status(&zr_flying_status_t); + last_decent_time = AP_HAL::millis(); + if(g.updown_countdown>0){ + is_countdown_enable = true; + }else{ + is_countdown_enable = false; + } } - - if (copter.updownStatus == UpDown_ContinueDescent) + else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable) + { + if((AP_HAL::millis()- last_decent_time)>=1000){ + last_decent_time = AP_HAL::millis(); + countdown --; + // mavlink_zr_flying_status_t zr_flying_status_t; + // zr_flying_status_t.updown_status = copter.updownStatus; + // zr_flying_status_t.countdown = countdown; + // gcs().update_zr_fly_status(&zr_flying_status_t); + if(countdown>0){ + gcs().send_text(MAV_SEVERITY_INFO,"着陆倒计时(秒): %d",countdown); + } + else if(countdown<=0){ + gcs().send_text(MAV_SEVERITY_INFO,"开始着陆"); + copter.updownStatus == UpDown_ContinueDescent; + _state = RTL_Land; + } + } + } + else if (copter.updownStatus == UpDown_ContinueDescent) { _state = RTL_Land; } @@ -267,7 +295,10 @@ void ModeRTL::loiterathome_run() void ModeRTL::descent_start() { copter.updownStatus = UpDown_RTLDescent; - gcs().update_zr_fly_status(copter.updownStatus); + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = 255; + gcs().update_zr_fly_status(&zr_flying_status_t); _state = RTL_FinalDescent; _state_complete = false; @@ -409,7 +440,10 @@ void ModeRTL::land_run(bool disarm_on_land) land_run_vertical_control(); if(AP_HAL::millis()-last_send_time>=500){ - gcs().update_zr_fly_status(copter.updownStatus); + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = copter.updownStatus; + zr_flying_status_t.countdown = 255; + gcs().update_zr_fly_status(&zr_flying_status_t); } } diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index fc59abe118..950737a560 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -219,7 +219,7 @@ public: void send_rpm() const; bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t); bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t); - bool send_zr_flying_status(const uint8_t flying_status); + bool send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t); //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; @@ -838,7 +838,7 @@ public: void update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t ); void update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t ); - void update_zr_fly_status(uint8_t flying_status ); + void update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ); // minimum amount of time (in microseconds) that must remain in // the main scheduler loop before we are allowed to send any // mavlink messages. We want to prioritise the main flight diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index a3dd5c51e7..597bdee8c2 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ } } -void GCS::update_zr_fly_status(uint8_t flying_status) +void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ) { - for (uint8_t i = 0; i < num_gcs(); i++) - { - chan(i)->send_zr_flying_status(flying_status); - } + for (uint8_t i=0; isend_zr_flying_status(zr_flying_status_t); + } } void GCS::send_mission_item_reached_message(uint16_t mission_index) @@ -4836,9 +4836,9 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg) } } -bool GCS_MAVLINK::send_zr_flying_status(const uint8_t flying_status) +bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t) { - mavlink_msg_zr_flying_status_send(chan, flying_status,0); + mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t); return true; } diff --git a/modules/mavlink b/modules/mavlink index ca507c39ff..18cab69509 160000 --- a/modules/mavlink +++ b/modules/mavlink @@ -1 +1 @@ -Subproject commit ca507c39ff56b88ce78a072a691e4b0c2a95f527 +Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019 From 59e0797af06dc318782899d1bef849afaebf1cb3 Mon Sep 17 00:00:00 2001 From: yaozb Date: Fri, 31 Jul 2020 09:13:46 +0800 Subject: [PATCH 2/3] fix status == --- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_rtl.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 550e80dd42..e4ebef37bb 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -91,7 +91,7 @@ void ModeBrake::run() } else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务"); - copter.updownStatus == UpDown_ContinueClimb; + copter.updownStatus = UpDown_ContinueClimb; set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 65973331bd..dbd23ffa63 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -96,7 +96,7 @@ void ModeRTL::run(bool disarm_on_land) } else if(countdown<=0){ gcs().send_text(MAV_SEVERITY_INFO,"开始着陆"); - copter.updownStatus == UpDown_ContinueDescent; + copter.updownStatus = UpDown_ContinueDescent; _state = RTL_Land; } } From 0b250ceb638fc1538482f09c8a07bd08b49e3d2a Mon Sep 17 00:00:00 2001 From: yaozb Date: Sat, 1 Aug 2020 16:10:35 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E4=B8=8A?= =?UTF-8?q?=E9=94=81=E6=9B=B4=E9=9A=BE=E7=8A=B6=E6=80=81=E7=A0=81=EF=BC=8C?= =?UTF-8?q?=E8=B5=B7=E9=A3=9E=E9=99=8D=E8=90=BD=E5=80=92=E8=AE=A1=E6=97=B6?= =?UTF-8?q?=E5=88=86=E5=BC=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 25 +++++++++++++++++++++---- ArduCopter/Parameters.h | 10 ++++++---- ArduCopter/UserCode.cpp | 9 ++++++++- ArduCopter/config.h | 10 +++++++++- ArduCopter/mode.h | 3 ++- ArduCopter/mode_auto.cpp | 12 ++++++------ ArduCopter/mode_brake.cpp | 13 +++++++------ ArduCopter/mode_rtl.cpp | 14 +++++++------- 8 files changed, 66 insertions(+), 30 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index ae42848ed0..2d7e3e556c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -124,16 +124,33 @@ const AP_Param::Info Copter::var_info[] = { // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ GSCALAR(gcs_pid_mask, "GCS_PID_MASK", 0), - // @Param: TAKEOFF_ALT_REQ + // @Param: ZR_TK_REQ_ALT // @DisplayName: take off Altitude // @Description: // @Units: cm - // @Range: -1 5000 + // @Range:0 5000 // @Increment: 1 // @User: Standard - GSCALAR(takeoff_alt_req, "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ), + GSCALAR(zr_tk_req_alt, "ZR_TK_REQ_ALT", TAKEOFF_ALT_REQ), + + // @Param: ZR_TK_DELAY + // @DisplayName: take off delay at ZR_TK_REQ_ALT + // @Description: + // @Units: second + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + GSCALAR(zr_tk_delay, "ZR_TK_DELAY", ZR_TK_DELAY), + + // @Param: ZR_RTL_DELAY + // @DisplayName: rtl Altitude when at final decent alt + // @Description: + // @Units: second + // @Range: 0 360 + // @Increment: 1 + // @User: Standard + GSCALAR(zr_rtl_delay, "ZR_RTL_DELAY", ZR_RTL_DELAY), - GSCALAR(updown_countdown, "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN), #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT // @DisplayName: RTL Altitude diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ae0fce33b3..4149cccf99 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -386,8 +386,9 @@ public: k_param_land_lock_alt, k_param_land_lock_rpy, - k_param_takeoff_alt_req, - k_param_updown_countdown, + k_param_zr_tk_req_alt, + k_param_zr_tk_delay, + k_param_zr_rtl_delay, // the k_param_* space is 9-bits in size // 511: reserved }; @@ -496,8 +497,9 @@ public: AP_Float acro_balance_pitch; AP_Int8 acro_trainer; AP_Float acro_rp_expo; - AP_Int16 takeoff_alt_req; - AP_Int16 updown_countdown; + AP_Int16 zr_tk_req_alt; + AP_Int16 zr_tk_delay; + AP_Int16 zr_rtl_delay; // Note: keep initializers here in the same order as they are declared // above. Parameters() diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 0331ee2f3e..bfc76101d4 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -82,8 +82,15 @@ void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here static bool before_fly = true; - if(motors->armed()) + if(motors->armed()){ before_fly = false; + }else{ + updownStatus =UpDown_TakeOffStart; + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = updownStatus; + //TODO UPDATE COUNTDOWN + gcs().update_zr_fly_status(&zr_flying_status_t); + } if(before_fly){ uint8_t cnt,cacl_volt_pst; float delt_volt; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c5345e4476..8aea68fb63 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -537,7 +537,15 @@ #endif #ifndef UPDOWN_COUNTDOWN -# define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#define UPDOWN_COUNTDOWN 20 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. +#endif + +#ifndef ZR_TK_DELAY +#define ZR_TK_DELAY 5 +#endif + +#ifndef ZR_RTL_DELAY +#define ZR_RTL_DELAY 15 #endif // RTL Mode diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 661c446ef2..ff672091c8 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -133,7 +133,8 @@ protected: float &G_Dt; int16_t countdown ; uint32_t last_takeoff_request_time; - bool is_countdown_enable = true; + bool is_takeoff_delay_enable = true; + bool is_rtl_delay_enable = true; // note that we support two entirely different automatic takeoffs: // "user-takeoff", which is available in modes such as ALT_HOLD diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 9c4c2257e0..bb38f5fb0e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -768,24 +768,24 @@ void ModeAuto::takeoff_run() int32_t alt_rev = 0; if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_rev)) { - if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req + if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt && copter.updownStatus < UpDown_RequestClimb) { copter.updownStatus = UpDown_RequestClimb; - countdown = g.updown_countdown; + countdown = g.zr_tk_delay; mavlink_zr_flying_status_t zr_flying_status_t; zr_flying_status_t.updown_status = copter.updownStatus; zr_flying_status_t.countdown = countdown; gcs().update_zr_fly_status(&zr_flying_status_t); last_takeoff_request_time = AP_HAL::millis(); - if(g.updown_countdown>0){ - is_countdown_enable =true; + if(g.zr_tk_delay>0){ + is_takeoff_delay_enable =true; }else{ - is_countdown_enable =false; + is_takeoff_delay_enable =false; } set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); } - else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0 + else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 &&(AP_HAL::millis()-last_time_send_updownload>500)) { last_time_send_updownload = AP_HAL::millis(); diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index e4ebef37bb..9c0e5a6a66 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -24,13 +24,13 @@ bool ModeBrake::init(bool ignore_checks) _timeout_ms = 0; if(copter.updownStatus == UpDown_RequestClimb){ - countdown = g.updown_countdown; + countdown = g.zr_tk_delay; } - if(g.updown_countdown>0){ - is_countdown_enable =true; + if(g.zr_tk_delay>0){ + is_takeoff_delay_enable =true; }else{ - is_countdown_enable =false; + is_takeoff_delay_enable =false; } return true; } @@ -76,12 +76,12 @@ void ModeBrake::run() } } - if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable) + if(copter.updownStatus == UpDown_RequestClimb&&is_takeoff_delay_enable) { if((AP_HAL::millis()-last_takeoff_request_time)>1000) { last_takeoff_request_time = AP_HAL::millis(); - countdown --; + // mavlink_zr_flying_status_t zr_flying_status_t; // zr_flying_status_t.updown_status = copter.updownStatus; // zr_flying_status_t.countdown = countdown; @@ -94,6 +94,7 @@ void ModeBrake::run() copter.updownStatus = UpDown_ContinueClimb; set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD); } + countdown --; //TODO Actually 1 second longer } } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index dbd23ffa63..929a998264 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -72,21 +72,20 @@ void ModeRTL::run(bool disarm_on_land) copter.updownStatus = UpDown_RequestDescent; mavlink_zr_flying_status_t zr_flying_status_t; zr_flying_status_t.updown_status = copter.updownStatus; - countdown = g.updown_countdown; + countdown = g.zr_rtl_delay; zr_flying_status_t.countdown = countdown; gcs().update_zr_fly_status(&zr_flying_status_t); last_decent_time = AP_HAL::millis(); - if(g.updown_countdown>0){ - is_countdown_enable = true; + if(g.zr_rtl_delay>0){ + is_rtl_delay_enable = true; }else{ - is_countdown_enable = false; + is_rtl_delay_enable = false; } } - else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable) + else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable) { if((AP_HAL::millis()- last_decent_time)>=1000){ - last_decent_time = AP_HAL::millis(); - countdown --; + last_decent_time = AP_HAL::millis(); // mavlink_zr_flying_status_t zr_flying_status_t; // zr_flying_status_t.updown_status = copter.updownStatus; // zr_flying_status_t.countdown = countdown; @@ -99,6 +98,7 @@ void ModeRTL::run(bool disarm_on_land) copter.updownStatus = UpDown_ContinueDescent; _state = RTL_Land; } + countdown --; //TODO shiji countdown+1 s } } else if (copter.updownStatus == UpDown_ContinueDescent)