From 0b250ceb638fc1538482f09c8a07bd08b49e3d2a Mon Sep 17 00:00:00 2001 From: yaozb Date: Sat, 1 Aug 2020 16:10:35 +0800 Subject: [PATCH] =?UTF-8?q?=E6=97=A0=E4=BA=BA=E6=9C=BA=E4=B8=8A=E9=94=81?= =?UTF-8?q?=E6=9B=B4=E9=9A=BE=E7=8A=B6=E6=80=81=E7=A0=81=EF=BC=8C=E8=B5=B7?= =?UTF-8?q?=E9=A3=9E=E9=99=8D=E8=90=BD=E5=80=92=E8=AE=A1=E6=97=B6=E5=88=86?= =?UTF-8?q?=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)