From f2ef8eec8cd00be054b59bb03c6b6aa327223a8a Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 15 Jul 2016 10:21:10 -0700 Subject: [PATCH] Copter: use velocity output from AC_PrecLand --- ArduCopter/Copter.h | 2 -- ArduCopter/Log.cpp | 28 ++++++++++++++-------------- ArduCopter/control_land.cpp | 29 ++++++++++++++++++----------- ArduCopter/precision_landing.cpp | 2 +- 4 files changed, 33 insertions(+), 28 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 80729eb01e..2cb820bd3e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -293,8 +293,6 @@ private: uint32_t start_ms; } takeoff_state; - uint32_t precland_last_update_ms; - // altitude below which we do no navigation in auto takeoff float auto_takeoff_no_nav_alt_cm; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index c44bbc213e..0312ab88a6 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -651,12 +651,11 @@ struct PACKED log_Precland { LOG_PACKET_HEADER; uint64_t time_us; uint8_t healthy; - float bf_angle_x; - float bf_angle_y; - float ef_angle_x; - float ef_angle_y; + uint8_t target_acquired; float pos_x; float pos_y; + float vel_x; + float vel_y; }; // Write an optical flow packet @@ -668,19 +667,20 @@ void Copter::Log_Write_Precland() return; } - const Vector2f &bf_angle = precland.last_bf_angle_to_target(); - const Vector2f &ef_angle = precland.last_ef_angle_to_target(); - const Vector3f &target_pos_ofs = precland.last_target_pos_offset(); + Vector2f target_pos_rel = Vector2f(0.0f,0.0f); + Vector2f target_vel_rel = Vector2f(0.0f,0.0f); + precland.get_target_position_relative_cm(target_pos_rel); + precland.get_target_velocity_relative_cms(target_vel_rel); + struct log_Precland pkt = { LOG_PACKET_HEADER_INIT(LOG_PRECLAND_MSG), time_us : AP_HAL::micros64(), healthy : precland.healthy(), - bf_angle_x : degrees(bf_angle.x), - bf_angle_y : degrees(bf_angle.y), - ef_angle_x : degrees(ef_angle.x), - ef_angle_y : degrees(ef_angle.y), - pos_x : target_pos_ofs.x, - pos_y : target_pos_ofs.y + target_acquired : precland.target_acquired(), + pos_x : target_pos_rel.x, + pos_y : target_pos_rel.y, + vel_x : target_vel_rel.x, + vel_y : target_vel_rel.y }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); #endif // PRECISION_LANDING == ENABLED @@ -833,7 +833,7 @@ const struct LogStructure Copter::log_structure[] = { { LOG_HELI_MSG, sizeof(log_Heli), "HELI", "Qff", "TimeUS,DRRPM,ERRPM" }, { LOG_PRECLAND_MSG, sizeof(log_Precland), - "PL", "QBffffff", "TimeUS,Heal,bX,bY,eX,eY,pX,pY" }, + "PL", "QBBffff", "TimeUS,Heal,TAcq,pX,pY,vX,vY" }, { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget), "GUID", "QBffffff", "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" }, { LOG_THROW_MSG, sizeof(log_Throw), diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 39645761b8..f981bc27b6 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -174,10 +174,10 @@ void Copter::land_run_vertical_control(bool pause_descent) #endif // compute desired velocity - const float precland_acceptable_error = 25.0f; - const float precland_min_descent_speed = -10.0f; + const float precland_acceptable_error = 15.0f; + const float precland_min_descent_speed = 10.0f; int32_t alt_above_ground = land_get_alt_above_ground(); - + float cmb_rate = 0; if (!pause_descent) { float max_land_descent_velocity; @@ -196,9 +196,10 @@ void Copter::land_run_vertical_control(bool pause_descent) // 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)); - if (doing_precision_landing && alt_above_ground < 300.0f) { - float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(abs(g.land_speed)/precland_acceptable_error)); - cmb_rate = MIN(precland_min_descent_speed, cmb_rate+land_slowdown); + if (doing_precision_landing && alt_above_ground < 200.0f) { + float max_descent_speed = abs(g.land_speed)/2.0f; + 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); } } @@ -251,12 +252,18 @@ void Copter::land_run_horizontal_control() #if PRECISION_LANDING == ENABLED bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired(); // run precision landing - if (doing_precision_landing && precland_last_update_ms != precland.last_update_ms()) { - Vector3f target_pos; - precland.get_target_position(target_pos); + if (doing_precision_landing) { + Vector2f target_pos, target_vel_rel; + if (!precland.get_target_position_cm(target_pos)) { + target_pos.x = inertial_nav.get_position().x; + target_pos.y = inertial_nav.get_position().y; + } + if (!precland.get_target_velocity_relative_cms(target_vel_rel)) { + target_vel_rel.x = -inertial_nav.get_velocity().x; + target_vel_rel.y = -inertial_nav.get_velocity().y; + } pos_control.set_xy_target(target_pos.x, target_pos.y); - pos_control.freeze_ff_xy(); - precland_last_update_ms = precland.last_update_ms(); + pos_control.override_vehicle_velocity_xy(-target_vel_rel); } #else bool doing_precision_landing = false; diff --git a/ArduCopter/precision_landing.cpp b/ArduCopter/precision_landing.cpp index 702984448f..b377bd9075 100644 --- a/ArduCopter/precision_landing.cpp +++ b/ArduCopter/precision_landing.cpp @@ -26,6 +26,6 @@ void Copter::update_precland() } } - copter.precland.update(height_above_ground_cm); + copter.precland.update(height_above_ground_cm, rangefinder_alt_ok()); } #endif