增加降落减速；ZR参数名调整

ArduCopter/UserCode.cpp

@@ -41,16 +41,6 @@ void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
 
-        // static bool init_prx = false;
-        // if (init_prx == false ) {
-        //     static uint32_t prx_init_time = AP_HAL::millis();
-        //     uint32_t tnow = AP_HAL::millis();
-        //     if(tnow - prx_init_time > 5000){
-        //         init_prx = true;
-        //         init_proximity();
-        //         gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%ldms -> %ldms ---",prx_init_time,tnow);
-        //     }
-        // }
 }
 #endif
 

ArduCopter/mode.cpp

@@ -512,8 +512,11 @@ void Mode::land_run_vertical_control(bool pause_descent)
     bool ignore_descent_limit = false;
     if (!pause_descent) {
 
+        // @Brown, loiter land auto deceleration
+        int16_t  land_speed_now = get_land_deceleration();
+
         // do not ignore limits until we have slowed down for landing
-        ignore_descent_limit = (MAX(g2.land_alt_low,100) > get_alt_above_ground_cm()) || copter.ap.land_complete_maybe;
+        ignore_descent_limit = (MAX(g2.land_alt_low,2000) > get_alt_above_ground_cm()) || copter.ap.land_complete_maybe; // 最低高度100改2000,最低20m开始降速
 
         float max_land_descent_velocity;
         if (g.land_speed_high > 0) {
@@ -529,7 +532,8 @@ void Mode::land_run_vertical_control(bool pause_descent)
         cmb_rate = 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_cmss(), 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 PRECISION_LANDING == ENABLED
         const bool navigating = pos_control->is_active_xy();
@@ -826,3 +830,12 @@ uint16_t Mode::get_pilot_speed_dn()
 {
     return copter.get_pilot_speed_dn();
 }
+/**
+ * @brief 根据高度，获取降落减速速度
+ * @author ZRZK
+ * @return uint16_t 降落速度
+ */
+uint16_t Mode::get_land_deceleration()
+{
+    return copter.zr_app.get_land_deceleration(get_alt_above_ground_cm(),get_pilot_speed_dn());
+}

ArduCopter/mode.h

@@ -253,6 +253,7 @@ public:
     void set_throttle_takeoff(void);
     uint16_t get_pilot_speed_dn(void);
 
+    uint16_t get_land_deceleration();
     // end pass-through functions
 };
 

ArduCopter/mode_loiter.cpp

@@ -81,8 +81,11 @@ void ModeLoiter::run()
     float target_yaw_rate = 0.0f;
     float target_climb_rate = 0.0f;
 
+    // @Brown, loiter land auto deceleration
+    int16_t  land_speed_now = get_land_deceleration();
     // set vertical speed and acceleration limits
-    pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
+    // pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
+    pos_control->set_max_speed_accel_z(-land_speed_now, g.pilot_speed_up, g.pilot_accel_z);  // 替换速度
 
     // process pilot inputs unless we are in radio failsafe
     if (!copter.failsafe.radio) {
@@ -100,7 +103,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/AC_ZR_APP/AC_ZR_App.cpp

@@ -24,15 +24,28 @@ const AP_Param::GroupInfo AC_ZR_App::var_info[] = {
     // @Description: parameters test
     // @Values: 0:None,1:other
     // @RebootRequired: True
-    // @User: Advanced
+    // @User: zrzk
     AP_GROUPINFO("_TYPE",           0,  AC_ZR_App,  _type,          1),
 
-    AP_GROUPINFO("_ID_TYPE",        1,  AC_ZR_App,  sysid_type,     0 ),
-    AP_GROUPINFO("_ID_BOARD_ID",    2,  AC_ZR_App,  sysid_board_id, 98765432U),
-    AP_GROUPINFO("_ID_DL1",         3,  AC_ZR_App,  sysid_dl1, 0x8175),
-    AP_GROUPINFO("_ID_DL2",         4,  AC_ZR_App,  sysid_dl2, 0x6fda),
-    AP_GROUPINFO("_ID_DL3",         5,  AC_ZR_App,  sysid_dl3, 0xf38f),
-    AP_GROUPINFO("_ID_DL4",         6,  AC_ZR_App,  sysid_dl4, 0xbf48),
+    AP_GROUPINFO("_SYS_TYPE",        1,  AC_ZR_App,  sysid_type,     0 ),
+    AP_GROUPINFO("_SYS_ID",          2,  AC_ZR_App,  sysid_board_id, 98765432U),
+    AP_GROUPINFO("_SYS_DL1",         3,  AC_ZR_App,  sysid_dl1, 0x8175),
+    AP_GROUPINFO("_SYS_DL2",         4,  AC_ZR_App,  sysid_dl2, 0x6fda),
+    AP_GROUPINFO("_SYS_DL3",         5,  AC_ZR_App,  sysid_dl3, 0xf38f),
+    AP_GROUPINFO("_SYS_DL4",         6,  AC_ZR_App,  sysid_dl4, 0xbf48),
+
+    // @Param: _SL_*
+    // @DisplayName: ZR_SL_*
+    // @Description: 降落减速设置，3个梯度
+    // @Values: 值单位cm和cm/s
+    // @RebootRequired: True
+    // @User: zrzk
+    AP_GROUPINFO("_SL_ALT_HI",         7,  AC_ZR_App,  land_slow_alt_high, 3000),
+    AP_GROUPINFO("_SL_ALT_MI",         8,  AC_ZR_App,  land_slow_alt_mid, 1500),
+    AP_GROUPINFO("_SL_ALT_LO",         9,  AC_ZR_App,  land_slow_alt_low, 150),
+    AP_GROUPINFO("_SL_SPD_HI",         10,  AC_ZR_App,  land_slow_speed_dn_high, 150),
+    AP_GROUPINFO("_SL_SPD_MI",         11,  AC_ZR_App,  land_slow_speed_dn_mid, 50),
+    AP_GROUPINFO("_SL_SPD_LO",         12,  AC_ZR_App,  land_slow_speed_dn_low, 30),
 
     AP_GROUPEND
 };
@@ -144,3 +157,27 @@ uint32_t AC_ZR_App::get_zr_sysid()
 {
     return (uint32_t)sysid_board_id;
 }
+
+/**
+ * @brief 降落根据高度梯度减速
+ * 
+ * @param alt_cm 当前飞机高度
+ * @param sys_speed_dn 系统设置的降落速度
+ * @return uint16_t 减速后速度
+ */
+uint16_t AC_ZR_App::get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn)
+{
+    int16_t speed_dn_now = sys_speed_dn; // 没到减速阶段则使用系统降落速度
+
+    if(alt_cm < land_slow_alt_low){ // 最低处减速
+        speed_dn_now = land_slow_speed_dn_low;
+    }
+    else if(alt_cm < land_slow_alt_mid){ // 中高处减速
+        speed_dn_now = land_slow_speed_dn_mid;
+    }
+    else if(alt_cm < land_slow_alt_high){  // 高处减速
+        speed_dn_now = land_slow_speed_dn_high;
+    }
+
+    return abs(speed_dn_now);
+}

libraries/AC_ZR_APP/AC_ZR_App.h

@@ -40,6 +40,9 @@ public:
     void get_deadline_params(uint32_t &deadline);
     bool check_deadline(uint32_t &date);
     uint32_t get_zr_sysid();
+
+    uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn);
+
     static const struct AP_Param::GroupInfo var_info[];
 
 protected:
@@ -52,6 +55,13 @@ protected:
     AP_Int32        sysid_dl3;
     AP_Int32        sysid_dl4;
 
+    AP_Int16        land_slow_alt_high;
+    AP_Int16        land_slow_alt_mid;
+    AP_Int16        land_slow_alt_low;
+    AP_Int16        land_slow_speed_dn_high;
+    AP_Int16        land_slow_speed_dn_mid;
+    AP_Int16        land_slow_speed_dn_low;
+
 private:
     static AC_ZR_App *_singleton;
 

modules/CrashDebug

@@ -0,0 +1 @@
+Subproject commit 599965086437137ec0fe66e185611f43f335f889

modules/DroneCAN/DSDL

@@ -0,0 +1 @@
+Subproject commit c09dac9dd35133edc8ba08f59b71c64d0e6328bd

modules/DroneCAN/dronecan_dsdlc

@@ -0,0 +1 @@
+Subproject commit 285a56e82741886b2bc7a5878d907aa443337cd8

modules/DroneCAN/libcanard

@@ -0,0 +1 @@
+Subproject commit 95cb5c3b23f5b4b049eac57631b127ed6a2247dc

modules/DroneCAN/pydronecan

@@ -0,0 +1 @@
+Subproject commit 4c5b1eb297eadb78ea14f582ccb2b7b4343b9483

modules/gsoap

@@ -0,0 +1 @@
+Subproject commit e1f690585d4803402584962bfaa8240ecaf1db30