From bd615a6892a701db91dd108ee2136b56f9fd02fb Mon Sep 17 00:00:00 2001 From: yaozb Date: Wed, 29 Jul 2020 10:04:10 +0800 Subject: [PATCH] =?UTF-8?q?=E8=B5=B7=E9=A3=9E=E7=9D=80=E9=99=86=E6=B7=BB?= =?UTF-8?q?=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