降落锁定第一版，落地后期望值任会增加

ArduCopter/Attitude.cpp

@@ -156,9 +156,39 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
 // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
 uint16_t Copter::get_pilot_speed_dn()
 {
-    if (g2.pilot_speed_dn == 0) {
+    int16_t speed_dn_now ;
+
+    if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
+        speed_dn_now = g2.pilot_speed_dn2;
+    else if(get_alt_above_ground_cm() < g2.land_alt_low)
+        speed_dn_now = g.land_speed;
+    else
+    {
+        speed_dn_now = g2.pilot_speed_dn;
+    }
+    
+   if (speed_dn_now == 0) {
         return abs(g.pilot_speed_up);
     } else {
-        return abs(g2.pilot_speed_dn);
+        return abs(speed_dn_now);
+    }
+    // if (g2.pilot_speed_dn == 0) {
+    //     return abs(g.pilot_speed_up);
+    // } else {
+    //     return abs(g2.pilot_speed_dn);
+    // }
+}
+
+int32_t Copter::get_alt_above_ground_cm()
+{
+    int32_t alt_above_ground;
+    if (rangefinder_alt_ok() && current_loc.alt < g.land_slow_2nd_alt) {
+        alt_above_ground = rangefinder_state.alt_cm_filt.get();
+    } else {
+        bool navigating = pos_control->is_active_xy();
+        if (!navigating || !current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) {
+            alt_above_ground = current_loc.alt;
+        }
     }
+    return alt_above_ground;
 }

ArduCopter/Copter.h

@@ -685,6 +685,7 @@ private:
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn();
+    int32_t get_alt_above_ground_cm();
 
 #if ADSB_ENABLED == ENABLED
     // avoidance_adsb.cpp

ArduCopter/Parameters.cpp

@@ -223,7 +223,12 @@ const AP_Param::Info Copter::var_info[] = {
     // @Range: 30 200
     // @Increment: 10
     // @User: Standard
-    GSCALAR(land_speed,             "LAND_SPEED",    LAND_SPEED),
+    GSCALAR(land_speed,             "LAND_SPEED",    100),
+    GSCALAR(land_speed_2nd,                "LAND_SPEED_2ND",    50),
+    GSCALAR(land_slow_2nd_alt,             "LAND_SL_2ND_ALT",    1000),
+    GSCALAR(land_lock_alt,             "LAND_LOCK_ALT",    100),
+    GSCALAR(land_lock_rpy,             "LAND_LOCK_RPY",    1),
+    
 
     // @Param: LAND_SPEED_HIGH
     // @DisplayName: Land speed high
@@ -980,6 +985,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 #endif
 
 
+    AP_GROUPINFO("PILOT_SPEED_DN2", 38, ParametersG2, pilot_speed_dn2, 100),
+    AP_GROUPINFO("PILOT_DN2_ALT", 39, ParametersG2, pilot_slow_dn2_alt, 100),
+
 
     AP_GROUPEND
 };
@@ -1625,7 +1633,7 @@ const char* Copter::get_sysid_board_id(void)
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;   
 
     // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.1 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
-    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.1 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.2 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
     return buf;
 }
 

ArduCopter/Parameters.h

@@ -381,6 +381,11 @@ public:
        k_param_sysid_dl3,
        k_param_sysid_dl4,
 
+        k_param_land_speed_2nd,
+        k_param_land_slow_2nd_alt,
+        k_param_land_lock_alt,
+        k_param_land_lock_rpy,
+
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -433,6 +438,14 @@ public:
     AP_Int16        pilot_speed_up;    // maximum vertical ascending velocity the pilot may request
     AP_Int16        pilot_accel_z;               // vertical acceleration the pilot may request
 
+
+    AP_Int16        land_speed_2nd;
+    AP_Int16        land_slow_2nd_alt;
+    AP_Int8        land_lock_alt;
+    AP_Int8        land_lock_rpy;
+
+
+    
     // Throttle
     //
     AP_Int8         failsafe_throttle;
@@ -582,6 +595,8 @@ public:
 
     // Additional pilot velocity items
     AP_Int16    pilot_speed_dn;
+    AP_Int16    pilot_speed_dn2;
+    AP_Int16    pilot_slow_dn2_alt;
 
     // Land alt final stage
     AP_Int16 land_alt_low;

ArduCopter/mode.cpp

@@ -509,7 +509,7 @@ void Mode::make_safe_spool_down()
 int32_t Mode::get_alt_above_ground_cm(void)
 {
     int32_t alt_above_ground;
-    if (copter.rangefinder_alt_ok()) {
+    if (copter.rangefinder_alt_ok() && copter.current_loc.alt < g.land_slow_2nd_alt) {
         alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
     } else {
         bool navigating = pos_control->is_active_xy();
@@ -533,18 +533,10 @@ void Mode::land_run_vertical_control(bool pause_descent)
     const float precland_acceptable_error = 15.0f;
     const float precland_min_descent_speed = 10.0f;
 
-
 // modify by @Brown , Staging land speed control
-        if(get_alt_above_ground_cm() < 100)
+    int16_t  land_speed_now = g.land_speed;
-            g.land_speed = 20;
+    if(get_alt_above_ground_cm() < g.land_slow_2nd_alt)
-        else if(get_alt_above_ground_cm() < 500)
+        land_speed_now = g.land_speed_2nd;
-            g.land_speed = 30;
-        else if(get_alt_above_ground_cm() < 1000)
-            g.land_speed = 50;
-        else if(get_alt_above_ground_cm() < 2000)
-            g.land_speed = 100;
-        else if(get_alt_above_ground_cm() < 3000)
-            g.land_speed = 150;
 
     float cmb_rate = 0;
     if (!pause_descent) {
@@ -556,16 +548,19 @@ void Mode::land_run_vertical_control(bool pause_descent)
         }
 
         // Don't speed up for landing.
-        max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
+        // max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
+        max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(land_speed_now));
 
         // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
         cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
 
         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
-        cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
+        // cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
+        cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(land_speed_now));
 
         if (doing_precision_landing && copter.rangefinder_alt_ok() && copter.rangefinder_state.alt_cm > 35.0f && copter.rangefinder_state.alt_cm < 200.0f) {
-            float max_descent_speed = abs(g.land_speed)*0.5f;
+            // float max_descent_speed = abs(g.land_speed)*0.5f;
+            float max_descent_speed = abs(land_speed_now)*0.5f;
             float land_slowdown = MAX(0.0f, pos_control->get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
             cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
         }
@@ -637,6 +632,30 @@ void Mode::land_run_horizontal_control()
         pos_control->override_vehicle_velocity_xy(-target_vel_rel);
     }
 #endif
+    static uint8_t prt_once;
+    // bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && copter.rangefinder_alt_ok() && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
+    bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
+    if(land_lock_attitude)
+    {
+        if(prt_once == 0)
+        {
+            gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode");
+            copter.Log_Write_Event(DATA_LAND_READY_LOCK);
+            prt_once = 1;
+        }
+        if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt*2){
+            target_roll = 0;
+            target_pitch = 0;
+            target_yaw_rate = 0;
+            loiter_nav->soften_for_landing();
+            if(prt_once == 1)
+            {
+                gcs().send_text(MAV_SEVERITY_INFO, "Land Lock attitude!");
+                copter.Log_Write_Event(DATA_LAND_LOCK_ATT);
+                prt_once = 2;
+            }
+        }
+    }
 
     // process roll, pitch inputs
     loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);

ArduCopter/mode.h

@@ -245,6 +245,7 @@ public:
     void set_throttle_takeoff(void);
     float get_avoidance_adjusted_climbrate(float target_rate);
     uint16_t get_pilot_speed_dn(void);
+    uint16_t get_pilot_speed_dn2(void);
 
     // end pass-through functions
 };

ArduCopter/mode_loiter.cpp

@@ -79,8 +79,24 @@ void ModeLoiter::run()
     float target_climb_rate = 0.0f;
     float takeoff_climb_rate = 0.0f;
 
+    // @Brown, loiter land auto slow down
+    int16_t  land_speed_now = get_pilot_speed_dn();
+    // static int16_t last_land_speed = land_speed_now;
+    // if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
+    //     land_speed_now = g2.pilot_speed_dn2;
+    // else if(get_alt_above_ground_cm() < g2.land_alt_low)
+    //     land_speed_now = g.land_speed;
+
+    // if(last_land_speed != land_speed_now)
+    // {
+    //     // gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d,dn2:%4d,dn:%4d",land_speed_now,g2.pilot_speed_dn2,g.land_speed);
+    //     gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d",land_speed_now);
+    //     last_land_speed = land_speed_now;
+    // }
+
     // initialize vertical speed and acceleration
-    pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    // pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control->set_max_speed_z(-land_speed_now, g.pilot_speed_up);
     pos_control->set_max_accel_z(g.pilot_accel_z);
 
     // process pilot inputs unless we are in radio failsafe
@@ -99,7 +115,8 @@ void ModeLoiter::run()
 
         // get pilot desired climb rate
         target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
-        target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+        // target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+        target_climb_rate = constrain_float(target_climb_rate, -land_speed_now, g.pilot_speed_up);
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
         loiter_nav->clear_pilot_desired_acceleration();

libraries/AP_Logger/AP_Logger.h

@@ -92,6 +92,9 @@ enum Log_Event : uint8_t {
     DATA_STANDBY_ENABLE = 74,
     DATA_STANDBY_DISABLE = 75,
 
+    DATA_LAND_READY_LOCK = 76,
+    DATA_LAND_LOCK_ATT = 77,
+
     DATA_SURFACED = 163,
     DATA_NOT_SURFACED = 164,
     DATA_BOTTOMED = 165,

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -120,6 +120,11 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
                 }
                 // if checksum matches extract contents
                 if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
+
+                    uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4];
+                    if(strength < 100 || strength == 0xFFFF){
+                        return false;
+                    }
                     // calculate distance
                     uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
                     if (dist >= BENEWAKE_DIST_MAX_CM) {