add zr_flying_status have problem

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;

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 {

ArduCopter/mode.h

@@ -1066,6 +1066,7 @@ private:
     uint32_t _loiter_start_time;
 
     bool terrain_following_allowed;
+    UpDownState _updownStatus = UpDown_TakeOffClimb;
 };
 
 

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

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

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

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; i<num_gcs(); i++) {
+    //  mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
+    chan(i)->send_zr_flying_status(zr_flying_status_t);
+    }          
+}
+
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
 {
     for (uint8_t i=0; i<num_gcs(); i++) {
@@ -1944,7 +1953,6 @@ void GCS::setup_console()
     create_gcs_mavlink_backend(chan_parameters[0], *uart);
 }
 
-
 GCS_MAVLINK_Parameters::GCS_MAVLINK_Parameters()
 {
     AP_Param::setup_object_defaults(this, var_info);
@@ -4815,6 +4823,22 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
     c->set_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();