From e6b21f683f0b2d8b39c3883f95c6c6c266c0f6fd Mon Sep 17 00:00:00 2001 From: yaozb Date: Tue, 14 Jul 2020 09:28:41 +0800 Subject: [PATCH] add zr_flying_status have problem --- ArduCopter/GCS_Mavlink.cpp | 7 ++++++- ArduCopter/defines.h | 8 ++++++++ ArduCopter/mode.h | 1 + ArduCopter/mode_rtl.cpp | 10 +++++++++- Tools/autotest/locations.txt | 2 +- libraries/GCS_MAVLink/GCS.h | 3 +++ libraries/GCS_MAVLink/GCS_Common.cpp | 26 +++++++++++++++++++++++++- 7 files changed, 53 insertions(+), 4 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 82f5a6d182..be67766860 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1257,13 +1257,18 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) break; #endif -case MAVLINK_MSG_ID_BATTGO_INFO: + case MAVLINK_MSG_ID_BATTGO_INFO: handle_battgo_info(msg); break; case MAVLINK_MSG_ID_BATTGO_HISTORY: handle_battgo_history(msg); break; + + case MAVLINK_MSG_ID_ZR_FLYING_STATUS: + handle_zr_fly_status(msg); + break; + default: handle_common_message(msg); break; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 69341bc101..88dcfc6d84 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -120,6 +120,14 @@ enum RTLState { RTL_FinalDescent, RTL_Land }; +// +enum UpDownState { + UpDown_TakeOffClimb, + UpDown_ContinueClimb, + UpDown_InMission, + UpDown_RTLDescent, + UpDown_ContinueDescent, +}; // Safe RTL states enum SmartRTLState { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 5ba1dc8d60..9a17bbd84e 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1066,6 +1066,7 @@ private: uint32_t _loiter_start_time; bool terrain_following_allowed; + UpDownState _updownStatus = UpDown_TakeOffClimb; }; diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 857a3edb55..c9eeb35623 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -66,7 +66,14 @@ void ModeRTL::run(bool disarm_on_land) } break; case RTL_FinalDescent: - // do nothing + // gcs().send_text(MAV_SEVERITY_INFO,"_updownStatus:%d,_state:%d",_state,_state); + if(_updownStatus!= UpDown_ContinueDescent){ + mavlink_zr_flying_status_t zr_flying_status_t; + zr_flying_status_t.updown_status = UpDown_ContinueDescent; + gcs().update_zr_fly_status(&zr_flying_status_t); + gcs().send_text(MAV_SEVERITY_INFO,"into RTL_FinalDescent"); + set_mode(Mode::Number::BRAKE,ModeReason::MISSION_END); + } break; case RTL_Land: // do nothing - rtl_land_run will take care of disarming motors @@ -256,6 +263,7 @@ void ModeRTL::loiterathome_run() void ModeRTL::descent_start() { _state = RTL_FinalDescent; + // _updownStatus = UpDown_RTLDescent; _state_complete = false; // Set wp navigation target to above home diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index d752e3d0d7..f00f9d115a 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -1,6 +1,6 @@ #NAME=latitude,longitude,absolute-altitude,heading OSRF0=37.4003371,-122.0800351,0,353 -CYDS=24.6122682,118.0672663,20,0 +CYDS=24.6057183,118.4168828,20,0 CELIU=24.6054997,118.0614525,1,0 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270 CMAC=-35.363261,149.165230,584,353 diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 05935f7a3f..950737a560 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -219,6 +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 mavlink_zr_flying_status_t *zr_flying_status_t); //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t ); bool locked() const; @@ -447,6 +448,7 @@ protected: void manual_override(RC_Channel *c, int16_t value_in, uint16_t offset, float scaler, const uint32_t tnow, bool reversed = false); void handle_battgo_info(const mavlink_message_t &msg); void handle_battgo_history(const mavlink_message_t &msg); + void handle_zr_fly_status(const mavlink_message_t &msg); private: void log_mavlink_stats(); @@ -836,6 +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(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 f86068396b..90dab65c3e 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1923,6 +1923,15 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ chan(i)->send_battgo_history(battgo_history_t); } } + +void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t ) +{ + for (uint8_t i=0; isend_zr_flying_status(zr_flying_status_t); + } +} + void GCS::send_mission_item_reached_message(uint16_t mission_index) { for (uint8_t i=0; iset_override(override_value, tnow); } +/* + handle a SET_MODE MAVLink message + */ +void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg) +{ + mavlink_zr_flying_status_t packet; + mavlink_msg_zr_flying_status_decode(&msg, &packet); + //TODO handle data; +} + +bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t) +{ + mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t); + return true; +} + GCS &gcs() { return *GCS::get_singleton();