backup commit,some test modify

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
 

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

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) {
 

libraries/AC_Avoidance/AC_Avoid.cpp

@@ -461,6 +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;
@@ -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

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);

libraries/AC_WPNav/AC_WPNav.cpp

@@ -67,6 +67,13 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
     // @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
 };
 

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
 };

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) {