diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index c20afcb71d..e0f099a678 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -40,17 +40,7 @@ void Copter::userhook_SlowLoop() 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 diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index d7d38df4fe..16f3fb9386 100644 --- a/ArduCopter/mode.cpp +++ b/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()); +} diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 8b3bffe7bc..26cea23e9b 100644 --- a/ArduCopter/mode.h +++ b/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 }; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index a2acd4618e..d1706964c2 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/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(); diff --git a/libraries/AC_ZR_APP/AC_ZR_App.cpp b/libraries/AC_ZR_APP/AC_ZR_App.cpp index 4a27a8c6ea..e723a5c845 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/CrashDebug b/modules/CrashDebug new file mode 160000 index 0000000000..5999650864 --- /dev/null +++ b/modules/CrashDebug @@ -0,0 +1 @@ +Subproject commit 599965086437137ec0fe66e185611f43f335f889 diff --git a/modules/DroneCAN/DSDL b/modules/DroneCAN/DSDL new file mode 160000 index 0000000000..c09dac9dd3 --- /dev/null +++ b/modules/DroneCAN/DSDL @@ -0,0 +1 @@ +Subproject commit c09dac9dd35133edc8ba08f59b71c64d0e6328bd diff --git a/modules/DroneCAN/dronecan_dsdlc b/modules/DroneCAN/dronecan_dsdlc new file mode 160000 index 0000000000..285a56e827 --- /dev/null +++ b/modules/DroneCAN/dronecan_dsdlc @@ -0,0 +1 @@ +Subproject commit 285a56e82741886b2bc7a5878d907aa443337cd8 diff --git a/modules/DroneCAN/libcanard b/modules/DroneCAN/libcanard new file mode 160000 index 0000000000..95cb5c3b23 --- /dev/null +++ b/modules/DroneCAN/libcanard @@ -0,0 +1 @@ +Subproject commit 95cb5c3b23f5b4b049eac57631b127ed6a2247dc diff --git a/modules/DroneCAN/pydronecan b/modules/DroneCAN/pydronecan new file mode 160000 index 0000000000..4c5b1eb297 --- /dev/null +++ b/modules/DroneCAN/pydronecan @@ -0,0 +1 @@ +Subproject commit 4c5b1eb297eadb78ea14f582ccb2b7b4343b9483 diff --git a/modules/gsoap b/modules/gsoap new file mode 160000 index 0000000000..e1f690585d --- /dev/null +++ b/modules/gsoap @@ -0,0 +1 @@ +Subproject commit e1f690585d4803402584962bfaa8240ecaf1db30