添加起飞与降落到指定高度请求继续

ArduCopter/Copter.h

@@ -686,7 +686,7 @@ private:
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn();
     int32_t get_alt_above_ground_cm();
-
+    UpDownState updownStatus = UpDown_TakeOffStart;
 #if ADSB_ENABLED == ENABLED
     // avoidance_adsb.cpp
     void avoidance_adsb_update(void);
@@ -808,6 +808,10 @@ private:
     // mode.cpp
     bool set_mode(Mode::Number mode, ModeReason reason);
     bool set_mode(const uint8_t new_mode, const ModeReason reason) override;
+
+    void update_updownStatus2(uint8_t status) ;
+    bool update_updownStatus(const uint8_t status) override;
+
     void update_flight_mode();
     void notify_flight_mode();
 

ArduCopter/Parameters.cpp

@@ -122,6 +122,15 @@ 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
+    // @DisplayName: take off  Altitude
+    // @Description:  
+    // @Units: cm
+    // @Range: -1 5000
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(takeoff_alt_req,  "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ),
+
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude
@@ -977,7 +986,6 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Bitmask: 0:Continue if in Auto on RC failsafe, 1:Continue if in Auto on GCS failsafe, 2:Continue if in Guided on RC failsafe, 3:Continue if landing on any failsafe, 4:Continue if in pilot controlled modes on GCS failsafe
     // @User: Advanced
     AP_GROUPINFO("FS_OPTIONS", 36, ParametersG2, fs_options, 0),
-
 #if MODE_AUTOROTATE_ENABLED == ENABLED
     // @Group: AROT_
     // @Path: ../libraries/AC_Autorotation/AC_Autorotation.cpp

ArduCopter/Parameters.h

@@ -386,6 +386,7 @@ public:
         k_param_land_lock_alt,
         k_param_land_lock_rpy,
 
+        k_param_takeoff_alt_req,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -445,7 +446,7 @@ public:
     AP_Int8        land_lock_rpy;
 
 
-    
+
     // Throttle
     //
     AP_Int8         failsafe_throttle;
@@ -495,6 +496,8 @@ public:
     AP_Int8                 acro_trainer;
     AP_Float                acro_rp_expo;
 
+    AP_Int16        takeoff_alt_req;
+    
     // Note: keep initializers here in the same order as they are declared
     // above.
     Parameters()

ArduCopter/config.h

@@ -531,6 +531,11 @@
  #define ACRO_THR_MID_DEFAULT       0.0f
 #endif
 
+
+#ifndef TAKEOFF_ALT_REQ
+ # 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
+
 // 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.

ArduCopter/defines.h

@@ -122,10 +122,13 @@ enum RTLState {
 };
 //
 enum UpDownState {
+    UpDown_TakeOffStart =0,
     UpDown_TakeOffClimb,
+    UpDown_RequestClimb,
     UpDown_ContinueClimb,
     UpDown_InMission,
     UpDown_RTLDescent,
+    UpDown_RequestDescent,
     UpDown_ContinueDescent,
 };
 

ArduCopter/mode.cpp

@@ -812,6 +812,22 @@ bool Mode::set_mode(Mode::Number mode, ModeReason reason)
     return copter.set_mode(mode, reason);
 }
 
+bool Copter::update_updownStatus(const uint8_t status)
+{
+    copter.update_updownStatus2(status);
+    return true;
+}
+
+void Copter::update_updownStatus2(uint8_t status)
+{
+    copter.updownStatus = (UpDownState)status;
+    if (status == 3)
+    {
+        set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
+        gcs().update_zr_fly_status(status);
+    }
+}
+
 void Mode::set_land_complete(bool b)
 {
     return copter.set_land_complete(b);

ArduCopter/mode.h

@@ -479,7 +479,7 @@ private:
     // Delay Mission Scripting Command
     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 {
@@ -1066,7 +1066,7 @@ private:
     uint32_t _loiter_start_time;
 
     bool terrain_following_allowed;
-    UpDownState _updownStatus = UpDown_TakeOffClimb;
+    uint32_t last_send_time;
 };
 
 

ArduCopter/mode_auto.cpp

@@ -235,6 +235,11 @@ void ModeAuto::wp_start(const Location& dest_loc)
     if (auto_yaw.mode() != AUTO_YAW_ROI) {
         auto_yaw.set_mode_to_default(false);
     }
+    if (copter.updownStatus < UpDown_InMission)
+    {
+        copter.updownStatus = UpDown_InMission;
+        gcs().update_zr_fly_status(copter.updownStatus);
+    }
 }
 
 // auto_land_start - initialises controller to implement a landing
@@ -755,6 +760,27 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
 //      called by auto_run at 100hz or more
 void ModeAuto::takeoff_run()
 {
+    if (motors->armed() || copter.ap.auto_armed)
+    {
+        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 
+                        && copter.updownStatus < UpDown_RequestClimb)
+            {
+                copter.updownStatus = UpDown_RequestClimb;
+                set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
+                gcs().update_zr_fly_status(copter.updownStatus);
+            }            
+            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);
+            }
+        }
+    }
     auto_takeoff_run();
 }
 

ArduCopter/mode_rtl.cpp

@@ -66,14 +66,18 @@ void ModeRTL::run(bool disarm_on_land)
             }
             break;
         case RTL_FinalDescent:
-        //    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);
+
+            if (copter.updownStatus < UpDown_RequestDescent)
+            {
+                copter.updownStatus = UpDown_RequestDescent;
+                gcs().update_zr_fly_status(copter.updownStatus);              
+            }
+
+            if (copter.updownStatus == UpDown_ContinueDescent)
+            {          
+                _state = RTL_Land;
             }
+
             break;
         case RTL_Land:
             // do nothing - rtl_land_run will take care of disarming motors
@@ -262,8 +266,9 @@ void ModeRTL::loiterathome_run()
 // rtl_descent_start - initialise descent to final alt
 void ModeRTL::descent_start()
 {
+    copter.updownStatus  = UpDown_RTLDescent;
+    gcs().update_zr_fly_status(copter.updownStatus);
     _state = RTL_FinalDescent;
-   // _updownStatus = UpDown_RTLDescent;
     _state_complete = false;
 
     // Set wp navigation target to above home
@@ -401,7 +406,11 @@ void ModeRTL::land_run(bool disarm_on_land)
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
     land_run_horizontal_control();
-    land_run_vertical_control();
+    land_run_vertical_control();  
+    
+    if(AP_HAL::millis()-last_send_time>=500){
+       gcs().update_zr_fly_status(copter.updownStatus);
+    }
 }
 
 void ModeRTL::build_path()

libraries/AP_Vehicle/AP_Vehicle.h

@@ -52,7 +52,7 @@ public:
     static AP_Vehicle *get_singleton();
 
     bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0;
-
+    bool virtual update_updownStatus(const uint8_t status) =0;
     /*
       common parameters for fixed wing aircraft
      */

libraries/AP_Vehicle/ModeReason.h

@@ -53,4 +53,5 @@ enum class ModeReason : uint8_t {
   UNAVAILABLE,
   AUTOROTATION_START,
   AUTOROTATION_BAILOUT,
+  REQUEST_CMD,
 };

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 mavlink_zr_flying_status_t *zr_flying_status_t);
+    bool send_zr_flying_status(const uint8_t flying_status);
     //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(mavlink_zr_flying_status_t *zr_flying_status_t );
+   void   update_zr_fly_status(uint8_t flying_status );
     // 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

@@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
     }          
 }
 
-void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t )
+void GCS::update_zr_fly_status(uint8_t flying_status)
 {
-    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);
-    }          
+    for (uint8_t i = 0; i < num_gcs(); i++)
+    {
+        chan(i)->send_zr_flying_status(flying_status);
+    }
 }
 
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
@@ -4829,13 +4829,16 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
 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;
+    mavlink_msg_zr_flying_status_decode(&msg, &packet);
+    if (packet.updown_status == 3 || packet.updown_status == 7)
+    {
+        AP::vehicle()->update_updownStatus(packet.updown_status);
+    }
 }
 
-bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t)
+bool GCS_MAVLINK::send_zr_flying_status(const uint8_t flying_status)
 {
-    mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t);
+    mavlink_msg_zr_flying_status_send(chan, flying_status,0);
     return true;
 }
 

libraries/GCS_MAVLink/GCS_MAVLink.cpp

@@ -129,9 +129,10 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
     }
     const size_t written = mavlink_comm_port[chan]->write(buf, len);
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    if (written < len) {
-        AP_HAL::panic("Short write on UART: %lu < %u", written, len);
-    }
+    // if (written < len) {
+    //     AP_HAL::panic("Short write on UART: %lu < %u", written, len);
+    // }
+     (void)written;
 #else
     (void)written;
 #endif