diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 641ec12a6f..2d7e3e556c 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -124,14 +124,32 @@ 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), #if MODE_RTL_ENABLED == ENABLED // @Param: RTL_ALT diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8044110e3b..4149cccf99 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -386,7 +386,9 @@ public: k_param_land_lock_alt, k_param_land_lock_rpy, - k_param_takeoff_alt_req, + 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 }; @@ -493,11 +495,11 @@ 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 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 9bd19347bd..8aea68fb63 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -536,6 +536,18 @@ # 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 + +#ifndef ZR_TK_DELAY +#define ZR_TK_DELAY 5 +#endif + +#ifndef ZR_RTL_DELAY +#define ZR_RTL_DELAY 15 +#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..ff672091c8 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,10 @@ protected: RC_Channel *&channel_throttle; RC_Channel *&channel_yaw; float &G_Dt; - + int16_t countdown ; + uint32_t last_takeoff_request_time; + 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 @@ -331,7 +331,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 +479,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 +1038,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 +1048,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..bb38f5fb0e 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); } } @@ -765,20 +768,33 @@ 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; - set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); - gcs().update_zr_fly_status(copter.updownStatus); + copter.updownStatus = UpDown_RequestClimb; + 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.zr_tk_delay>0){ + is_takeoff_delay_enable =true; + }else{ + 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(); 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..9c0e5a6a66 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.zr_tk_delay; + } + + if(g.zr_tk_delay>0){ + is_takeoff_delay_enable =true; + }else{ + is_takeoff_delay_enable =false; + } return true; } @@ -67,6 +75,29 @@ void ModeBrake::run() copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT); } } + + 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(); + + // 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); + } + countdown --; //TODO Actually 1 second longer + } + } + } void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index f6160105de..929a998264 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.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.zr_rtl_delay>0){ + is_rtl_delay_enable = true; + }else{ + is_rtl_delay_enable = false; + } } - - if (copter.updownStatus == UpDown_ContinueDescent) + else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable) + { + if((AP_HAL::millis()- last_decent_time)>=1000){ + 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; + // 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; + } + countdown --; //TODO shiji countdown+1 s + } + } + 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