From ed434ce9e4e28fbc06640ad61056351470a2f6c5 Mon Sep 17 00:00:00 2001 From: z Date: Sun, 31 May 2020 19:04:09 +0800 Subject: [PATCH] =?UTF-8?q?=E9=99=8D=E8=90=BD=E9=94=81=E5=AE=9A=E7=AC=AC?= =?UTF-8?q?=E4=B8=80=E7=89=88=EF=BC=8C=E8=90=BD=E5=9C=B0=E5=90=8E=E6=9C=9F?= =?UTF-8?q?=E6=9C=9B=E5=80=BC=E4=BB=BB=E4=BC=9A=E5=A2=9E=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Attitude.cpp | 34 ++++++++++++- ArduCopter/Copter.h | 1 + ArduCopter/Parameters.cpp | 12 ++++- ArduCopter/Parameters.h | 15 ++++++ ArduCopter/mode.cpp | 49 +++++++++++++------ ArduCopter/mode.h | 1 + ArduCopter/mode_loiter.cpp | 21 +++++++- libraries/AP_Logger/AP_Logger.h | 3 ++ .../AP_RangeFinder_Benewake.cpp | 5 ++ 9 files changed, 120 insertions(+), 21 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 1ec6fd815e..ae677cac28 100644 --- a/ArduCopter/Attitude.cpp +++ b/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; } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 76371dba5f..2425399135 100644 --- a/ArduCopter/Copter.h +++ b/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 diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 631c88c6b0..b13171c192 100644 --- a/ArduCopter/Parameters.cpp +++ b/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; } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b83b00caed..f0ce11d546 100644 --- a/ArduCopter/Parameters.h +++ b/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; diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 54577169a5..41e0a2422f 100644 --- a/ArduCopter/mode.cpp +++ b/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) - g.land_speed = 20; - else if(get_alt_above_ground_cm() < 500) - 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; + int16_t land_speed_now = g.land_speed; + if(get_alt_above_ground_cm() < g.land_slow_2nd_alt) + land_speed_now = g.land_speed_2nd; 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); diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 74cda23977..5ba1dc8d60 100644 --- a/ArduCopter/mode.h +++ b/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 }; diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 8597624c14..feb8f6b251 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/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(); diff --git a/libraries/AP_Logger/AP_Logger.h b/libraries/AP_Logger/AP_Logger.h index e6e514e9d5..79f63d97d2 100644 --- a/libraries/AP_Logger/AP_Logger.h +++ b/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, diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp index f02a845988..825991ccb3 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp +++ b/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) {