From dc9b6080c46d062139528fd0161894ee905efb14 Mon Sep 17 00:00:00 2001 From: z Date: Wed, 18 Mar 2020 14:16:17 +0800 Subject: [PATCH] backup commit,some test modify --- ArduCopter/UserCode.cpp | 15 ++++++++++++++- ArduCopter/mode.cpp | 3 ++- ArduCopter/mode_loiter.cpp | 5 +++++ libraries/AC_Avoidance/AC_Avoid.cpp | 8 +++++++- libraries/AC_WPNav/AC_Loiter.cpp | 1 - libraries/AC_WPNav/AC_WPNav.cpp | 7 +++++++ libraries/AC_WPNav/AC_WPNav.h | 4 ++++ libraries/AC_WPNav/AC_WPNav_OA.cpp | 7 +++++++ 8 files changed, 46 insertions(+), 4 deletions(-) diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 0803c691f4..dba13fa5ad 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -38,8 +38,17 @@ void Copter::userhook_SlowLoop() static int32_t last_pos_lng; camera.get_last_trigger_pos(last_pos_lat,last_pos_lng); + + // static uint8_t cnt; + // cnt++; + // if(cnt > 9){ + // gcs().send_text(MAV_SEVERITY_INFO, "cam pos: %d,%d",last_pos_lat,last_pos_lng); + // gcs().send_text(MAV_SEVERITY_INFO, "gps pos: %d,%d",current_loc.lat,current_loc.lng); + // cnt = 0; + // } // mission.set_last_trigger_pos(last_pos_lat,last_pos_lng); - if(mode_auto.mission._type_resume) + if(mode_auto.mission._type_resume && last_pos_lat!=0) + // if(0) mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),last_pos_lat,last_pos_lng); else mode_auto.mission.auto_resume_mission((uint8_t)control_mode,motors->armed(),current_loc.lat,current_loc.lng); @@ -51,6 +60,10 @@ void Copter::userhook_SlowLoop() void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here + // if(!motors->armed()) + // { + // camera.set_trigger_distance(0); // @Brown , stop take photos + // } } #endif diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 8656ee8c92..54577169a5 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -276,7 +276,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason) gcs().send_message(MSG_HEARTBEAT); #if ADSB_ENABLED == ENABLED - adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED)); + // adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::RTL) || (mode == Mode::Number::GUIDED)); + adsb.set_is_auto_mode((mode == Mode::Number::AUTO) || (mode == Mode::Number::GUIDED)); #endif #if AC_FENCE == ENABLED diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 8597624c14..03184a1310 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -113,6 +113,11 @@ 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 4fe87078fe..8f37b20320 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -461,7 +461,8 @@ 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()) { @@ -514,6 +515,8 @@ 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 @@ -578,6 +581,7 @@ 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 @@ -624,6 +628,8 @@ 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 38647ac45f..9567adaad1 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -309,7 +309,6 @@ 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.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 8ff6bba6ae..8b090c412d 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -66,6 +66,13 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Values: 0:Disable,1:Enable // @User: Advanced AP_GROUPINFO("RFND_USE", 10, AC_WPNav, _rangefinder_use, 1), + + // @Param: FAST_WP + // @DisplayName: Set whether to use fast waypoint + // @Description: This controls the use of fast waypoint + // @Values: 0:Disable,1:Enable + // @User: Advanced + AP_GROUPINFO("FAST_WP", 11, AC_WPNav, _fast_wp, 0), AP_GROUPEND }; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index fc2ec9cf5c..a440188faf 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -223,6 +223,8 @@ public: static const struct AP_Param::GroupInfo var_info[]; + bool get_fast_waypoint(bool fast) { return _fast_wp; } + protected: // segment types, either straight or spine @@ -316,4 +318,6 @@ protected: AP_Int8 _rangefinder_use; bool _rangefinder_healthy; float _rangefinder_alt_cm; + + AP_Int8 _fast_wp; //modify by @Brown }; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 2faa2c0666..6ca9114c5a 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -83,6 +83,13 @@ 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) {