From 80e93f3f71c39bbd2ba0dad190c2aa00eb841439 Mon Sep 17 00:00:00 2001 From: "Brown.Z" Date: Sat, 26 Mar 2022 17:09:32 +0800 Subject: [PATCH] =?UTF-8?q?AC=5FZR=5FApp:=20=E5=A2=9E=E5=8A=A0=E9=99=8D?= =?UTF-8?q?=E8=90=BD=E5=87=8F=E9=80=9F=EF=BC=9BZR=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E5=90=8D=E8=B0=83=E6=95=B4=20ArduCopter:=20loiter=E7=AD=89?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F=E5=A2=9E=E5=8A=A0=E9=99=8D=E8=90=BD=E5=87=8F?= =?UTF-8?q?=E9=80=9F=E8=B0=83=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode.cpp | 17 +++++++++-- ArduCopter/mode.h | 2 ++ ArduCopter/mode_loiter.cpp | 8 +++-- libraries/AC_ZR_APP/AC_ZR_App.cpp | 51 ++++++++++++++++++++++++++----- libraries/AC_ZR_APP/AC_ZR_App.h | 10 ++++++ modules/DroneCAN/dronecan_dsdlc | 1 - modules/DroneCAN/libcanard | 1 - modules/DroneCAN/pydronecan | 1 - 8 files changed, 77 insertions(+), 14 deletions(-) delete mode 160000 modules/DroneCAN/dronecan_dsdlc delete mode 160000 modules/DroneCAN/libcanard delete mode 160000 modules/DroneCAN/pydronecan diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 9e2083fc5d..0e1f524a6c 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -575,8 +575,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) { @@ -592,7 +595,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(); @@ -1024,3 +1028,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()); +} diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 7d5f6e9371..53781bd74f 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -295,6 +295,8 @@ public: GCS_Copter &gcs(); void set_throttle_takeoff(void); uint16_t get_pilot_speed_dn(void); + + uint16_t get_land_deceleration(); // end pass-through functions }; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 366228cbac..b1f5fd637c 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -87,8 +87,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) { @@ -106,7 +109,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(); diff --git a/libraries/AC_ZR_APP/AC_ZR_App.cpp b/libraries/AC_ZR_APP/AC_ZR_App.cpp index 3c8feccf17..c1b72dbf58 100644 --- a/libraries/AC_ZR_APP/AC_ZR_App.cpp +++ b/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 }; @@ -143,4 +156,28 @@ bool AC_ZR_App::check_deadline(uint32_t &date) 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); } \ No newline at end of file diff --git a/libraries/AC_ZR_APP/AC_ZR_App.h b/libraries/AC_ZR_APP/AC_ZR_App.h index 6678c4a0f9..169541d873 100644 --- a/libraries/AC_ZR_APP/AC_ZR_App.h +++ b/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; diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc deleted file mode 160000 index c00fc457e2..0000000000 --- a/modules/DroneCAN/dronecan_dsdlc +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c00fc457e22c0b71e3a385f092b519830ef58ea2 diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard deleted file mode 160000 index 8e66f73398..0000000000 --- a/modules/DroneCAN/libcanard +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8e66f733981c8acb5bed67d57b0079238e611cfa diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan deleted file mode 160000 index 8fb1b18ab3..0000000000 --- a/modules/DroneCAN/pydronecan +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8fb1b18ab3250695d85fe2943dfc68e8b45024b7