From 056d98ed6353301c6409a2ef9f1e0173acdcd518 Mon Sep 17 00:00:00 2001 From: z Date: Wed, 18 Mar 2020 14:20:49 +0800 Subject: [PATCH] backup commit,some test modify --- ArduCopter/mode_loiter.cpp | 5 ----- libraries/AC_Avoidance/AC_Avoid.cpp | 8 +------- libraries/AC_WPNav/AC_Loiter.cpp | 1 + libraries/AC_WPNav/AC_WPNav_OA.cpp | 7 ------- 4 files changed, 2 insertions(+), 19 deletions(-) diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 03184a1310..8597624c14 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -113,11 +113,6 @@ void ModeLoiter::run() // Loiter State Machine Determination AltHoldModeState loiter_state = get_alt_hold_state(target_climb_rate); - static uint8_t last_state; - if(last_state != loiter_state){ - gcs().send_text(MAV_SEVERITY_INFO, "loiter_state %d ",loiter_state ); - last_state = loiter_state; - } // Loiter State Machine switch (loiter_state) { diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 8f37b20320..4fe87078fe 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -461,8 +461,7 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec // get the margin to the fence in cm const float margin_cm = fence->get_margin() * 100.0f; - gcs().send_text(MAV_SEVERITY_INFO, "---- margin_cm %f ----",margin_cm ); - + // get stopping distance as an offset from the vehicle Vector2f stopping_offset; switch ((AC_Avoid::BehaviourType)_behavior.get()) { @@ -515,8 +514,6 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec // implement stopping behaviour const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset; const float dist_cm = safe_sqrt(dist_sq_cm); - gcs().send_text(MAV_SEVERITY_INFO, "---- dist_cm %f ----",dist_cm ); - if (dist_cm >= radius_cm - margin_cm) { // vehicle has already breached margin around fence // if stopping point is even further from center (i.e. in wrong direction) then adjust speed to zero @@ -581,7 +578,6 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // get the margin to the fence in cm const float margin_cm = fence->get_margin() * 100.0f; - gcs().send_text(MAV_SEVERITY_INFO, "---- margin_cm %f , speed: %f ----",margin_cm ,desired_speed); // calculate stopping distance as an offset from the vehicle (only used for BEHAVIOR_STOP) // add a margin so we look forward far enough to intersect with circular fence @@ -628,8 +624,6 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // implement stopping behaviour const Vector2f stopping_point_plus_margin = position_NE_rel + stopping_offset; const float dist_cm = safe_sqrt(dist_sq_cm); - gcs().send_text(MAV_SEVERITY_INFO, "---- margin_cm %f ,adjust_velocity_exclusion_circles ----",dist_cm ); - if (dist_cm < radius_cm + margin_cm) { // vehicle has already breached margin around fence // if stopping point is closer to center (i.e. in wrong direction) then adjust speed to zero diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 9567adaad1..38647ac45f 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -309,6 +309,7 @@ void AC_Loiter::calc_desired_velocity(float nav_dt) if (_avoid != nullptr) { _avoid->adjust_velocity(_pos_control.get_pos_xy_p().kP(), _accel_cmss, desired_vel, nav_dt); } + // send adjusted feed forward acceleration and velocity back to the Position Controller _pos_control.set_desired_accel_xy(_desired_accel.x, _desired_accel.y); _pos_control.set_desired_velocity_xy(desired_vel.x, desired_vel.y); diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 6ca9114c5a..2faa2c0666 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -83,13 +83,6 @@ bool AC_WPNav_OA::update_wpnav() const Location destination_loc(_destination_oabak); Location oa_origin_new, oa_destination_new; const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new); - - static uint8_t last_state; - - if(last_state != oa_retstate){ - gcs().send_text(MAV_SEVERITY_INFO, "---- oa_retstate %d ----",oa_retstate ); - last_state = oa_retstate; - } switch (oa_retstate) { case AP_OAPathPlanner::OA_NOT_REQUIRED: if (_oa_state != oa_retstate) {