第一个点不绕圈问题

ArduCopter/mode_auto.cpp

@@ -47,7 +47,6 @@ bool ModeAuto::init(bool ignore_checks)
         mission.start_or_resume();
         wp_nav_index = 0;   // nav 重新开始计数
         updown_state = 0;   // 起飞悬停状态清0
-        // gcs().send_text(MAV_SEVERITY_INFO, "auto init:up:%d,wp:%d",updown_state,wp_nav_index);
         return true;
     } else {
         return false;
@@ -150,8 +149,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
     if (g.zr_tk_delay > 0 && g.zr_tk_req_alt > 0 && updown_state == 0){  // 有设置起飞到高度悬停
         updown_state = 1;
         dest.alt = g.zr_tk_req_alt;
-        gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
+        copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
-        copter.Log_Write_Data(DATA_NOT_BOTTOMED, (uint16_t)copter.updownStatus);
     }
     if (!copter.current_loc.initialised()) {
         // vehicle doesn't know where it is ATM.  We should not
@@ -233,7 +231,6 @@ void ModeAuto::wp_start(const Location& dest_loc)
     if (copter.updownStatus < UpDown_InMission)
     {
         copter.updownStatus = UpDown_InMission;
-        gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
         copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
         gcs().send_message(MSG_ZR_FLYING_STATUS);
     }
@@ -445,7 +442,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         break;
 
     case MAV_CMD_NAV_LOITER_TURNS:              //18 Loiter N Times
-        // do_roi(cmd);
+        // do_roi(cmd); // 绕圈指向中间
         do_circle(cmd);
         break;
 
@@ -770,7 +767,6 @@ void ModeAuto::takeoff_run()
         {
             updown_state = 3;   // 到达悬停高度，状态更新到等待
             copter.updownStatus = UpDown_RequestClimb;
-            gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
             copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
             gcs().send_message(MSG_ZR_FLYING_STATUS);
             countdown = g.zr_tk_delay;
@@ -785,7 +781,6 @@ void ModeAuto::takeoff_run()
                 else if (countdown <= 0){
                     gcs().send_text(MAV_SEVERITY_INFO, "NOTICE: 执行继续任务");
                     copter.updownStatus = UpDown_ContinueClimb;
-                    gcs().send_text(MAV_SEVERITY_INFO, "updownStatus:%d",copter.updownStatus);
                     copter.Log_Write_Data(DATA_UPDOWN_STATUS, (uint16_t)copter.updownStatus);
                     gcs().send_message(MSG_ZR_FLYING_STATUS);
                     updown_state = 4;
@@ -806,7 +801,6 @@ void ModeAuto::takeoff_run()
         }else if(copter.updownStatus > UpDown_RequestClimb){
             if(updown_state < 4){   // 地面站弹框确定，发送新的值，状态位同步更新
                 updown_state = 4;
-                gcs().send_text(MAV_SEVERITY_INFO, "gcs ctl updownStatus:%d",copter.updownStatus);
                 takeoff_start(last_loc);
             }
         }
@@ -819,6 +813,55 @@ void ModeAuto::takeoff_run()
 //      called by auto_run at 100hz or more
 void ModeAuto::wp_run()
 {
+    // 到航点绕圈
+    if(g.zr_8_circle == 2 && wp_nav_index == 0){
+        const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
+        static uint32_t last_100ms;
+        uint32_t tnow = AP_HAL::millis();
+        if (tnow - last_100ms > 100) {
+            Vector3f stopping_point;
+            last_100ms = tnow;
+            wp_nav->get_wp_stopping_point(stopping_point);
+            
+            AP::logger().Write("ATES", "TimeUS,Indx,Rech,IniY,Ymod,px,py,pz", "QHBBBfff",
+                                    AP_HAL::micros64(),
+                                    wp_nav_index,
+                                    reached_wp_dest,
+                                    circle_init_yaw,
+                                    auto_yaw.mode(),
+                                    stopping_point.x,
+                                    stopping_point.y,
+                                    stopping_point.z );
+        }
+        if(reached_wp_dest){
+            static autopilot_yaw_mode last_yaw_mode;
+            if(circle_init_yaw == false){   // 如果需要转向到下一个航点
+                if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) {   //转到角度
+                    circle_init_yaw = true;
+                    auto_yaw.set_mode(last_yaw_mode);
+                    copter.circle_nav->init();
+                    gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
+                }else{
+                        // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
+                        if (auto_yaw.mode() != AUTO_YAW_FIXED) {    // 需要转角度，yaw mode使用AUTO_YAW_FIXED
+                            // wp_nav->set_wp_destination(last_loc);// 设置第一个WayPoint点为期望值
+                            last_yaw_mode = auto_yaw.mode();
+                            auto_yaw.set_mode(AUTO_YAW_FIXED);
+                        }
+                        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true);
+                }
+                return;
+            }
+        }
+        if(circle_init_yaw)
+        {   // 如果需要转向到下一个航点
+            copter.circle_nav->turn_2_circle();
+            pos_control->update_z_controller();
+            // Yaw 由 circle决定
+            attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
+            return;
+        }
+    }
     // process pilot's yaw input
     float target_yaw_rate = 0;
     if (!copter.failsafe.radio) {
@@ -845,39 +888,6 @@ void ModeAuto::wp_run()
     // call z-axis position controller (wpnav should have already updated it's alt target)
     pos_control->update_z_controller();
 
-
-    // 到航点绕圈
-    if(g.zr_8_circle == 2 && wp_nav_index == 0){
-        const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
-        if(reached_wp_dest){
-            static autopilot_yaw_mode last_yaw_mode;
-            if(circle_init_yaw == false){   // 如果需要转向到下一个航点
-                if (abs(wrap_180_cd(ahrs.yaw_sensor-wp_yaw_cd)) <= 200) {   //转到角度
-                    circle_init_yaw = true;
-                    auto_yaw.set_mode(last_yaw_mode);
-                    copter.circle_nav->init();
-                    gcs().send_text(MAV_SEVERITY_INFO,"Auto run circle");
-                    // gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",wp_yaw_cd,ahrs.yaw_sensor);
-                }else{
-                    if (auto_yaw.mode() != AUTO_YAW_FIXED) {    // 需要转角度，yaw mode使用AUTO_YAW_FIXED
-                        last_yaw_mode = auto_yaw.mode();
-                        // gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
-                        auto_yaw.set_mode(AUTO_YAW_FIXED);
-                    }
-                    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), wp_yaw_cd,true);
-                }
-                return;
-            }else{   // 如果需要转向到下一个航点
-                copter.circle_nav->turn_2_circle();
-                pos_control->update_z_controller();
-                // Yaw 由 circle决定
-                attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
-
-                return;
-            }
-        }
-    }
-
     // call attitude controller
     if (auto_yaw.mode() == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
@@ -969,19 +979,14 @@ void ModeAuto::circle_run()
             if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {   //转到角度
                 circle_init_yaw = true;
                 auto_yaw.set_mode(last_yaw_mode);
-                // gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor);
             }else{
-                if(wp_nav->set_wp_destination(last_loc)) {  // 设置第一个WayPoint点为期望值
+                // if(wp_nav->set_wp_destination(last_loc)) {  // 设置第一个WayPoint点为期望值
                     if (auto_yaw.mode() != AUTO_YAW_FIXED) {    // 需要转角度，yaw mode使用AUTO_YAW_FIXED
                         last_yaw_mode = auto_yaw.mode();
-                        // gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
                         auto_yaw.set_mode(AUTO_YAW_FIXED);
                     }
                     attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing,true);
-                }else{  // 第一个WayPoint点获取失败，直接绕圈
+                // }
-                    circle_init_yaw = true;
-                    gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---");
-                }
                 
                 return;
             }
@@ -1269,8 +1274,24 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
     loiter_time_max = cmd.p1;
     // Set wp navigation target
     wp_start(target_loc);
-    if(wp_nav_index < 2){  // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了
+    if(wp_nav_index < 1){  // 传给绕圈用，只有第一个wp需要绕圈，后面就不赋值了
         last_loc = target_loc;
+        AP_Mission::Mission_Command next_cmd;
+        for(int i = cmd.index; i < cmd.index+5;i++)
+        {
+            mission.read_cmd_from_storage(i,next_cmd);   // 读取原本航点
+            // gcs_send_text_fmt(MAV_SEVERITY_DEBUG, "read[%d]: %d",i,cmd_rtl.id);
+            if (mission.is_nav_cmd(next_cmd)) {   // 是否是Waypoint
+                float wp_distance = cmd.content.location.get_distance(next_cmd.content.location);
+                if(wp_distance < 5.0 && wp_nav_index < 2){  // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点
+                    continue;
+                }else{
+                    circle_init_yaw = false;
+                    wp_yaw_cd = cmd.content.location.get_bearing_to(next_cmd.content.location);
+                    break;
+                }  
+            }
+        }
     }
     // if no delay as well as not final waypoint set the waypoint as "fast"
     AP_Mission::Mission_Command temp_cmd;
@@ -1287,36 +1308,26 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
             case MAV_CMD_NAV_SPLINE_WAYPOINT:
                 // if next command's lat, lon is specified then do not slowdown at this waypoint
                 if ((temp_cmd.content.location.lat != 0) || (temp_cmd.content.location.lng != 0)) {
-                    fast_waypoint = true;
+                    // fast_waypoint = true;
-                    AP_Mission::Mission_Command next_2_cmd;
+    
-                    static int32_t last_yaw_cd;
+                    if(copter.wp_nav->get_fast_waypoint() == 2){
-                    if(mission.get_next_nav_cmd(cmd.index, next_2_cmd)){
+                        static float last_dist = 0;
-                        float wp_distance = next_2_cmd.content.location.get_distance(temp_cmd.content.location);
+                        static int32_t last_yaw_cd;
-                        if(wp_distance < 5.0 && wp_nav_index < 2){  // 第一个wp需要转向相关。两个航点距离太近，正常是仿地降高度航点，则取再下一个航点
+                        float wp_distance = cmd.content.location.get_distance(temp_cmd.content.location);
-                            // gcs().send_text(MAV_SEVERITY_CRITICAL, "-wp too close:%.2f",wp_distance);
+                        wp_yaw_cd = cmd.content.location.get_bearing_to(temp_cmd.content.location);
-                            if(mission.get_next_nav_cmd(cmd.index+2, temp_cmd)){
+
-                                wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location);
+                        int16_t delt_cd = abs(wp_yaw_cd - last_yaw_cd);
-                                // gcs().send_text(MAV_SEVERITY_CRITICAL, "-b new rad:%d",wp_yaw_cd);
+                        // delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd;
-                            }
+                        fast_waypoint = (delt_cd > 6000)?false:true;
-                        }else{
+                        if(wp_distance - last_dist > 0  && wp_nav_index > 0){   // P2.5转弯校准用 //TODO P2.5需要修改这里 
-                            wp_yaw_cd = next_2_cmd.content.location.get_bearing_to(temp_cmd.content.location);
+                            // gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----");
-                                // gcs().send_text(MAV_SEVERITY_CRITICAL, "-a new rad:%d",wp_yaw_cd);
-                        }
-                        if(copter.wp_nav->get_fast_waypoint() == 2){
-                            static float last_dist = 0;
-                            int16_t delt_cd = abs(wp_yaw_cd - last_yaw_cd);
-                            // delt_cd = (delt_cd > 0)? delt_cd:360+delt_cd;
-                            fast_waypoint = (delt_cd > 6000)?false:true;
-                            // gcs().send_text(MAV_SEVERITY_CRITICAL, "%d,a:%d,d:%d,f:%d,p:%.2f",cmd.index,wp_yaw_cd,delt_cd,fast_waypoint,wp_distance);
-                            if(wp_distance - last_dist > 0  && wp_nav_index > 0){   // P2.5转弯校准用 //TODO P2.5需要修改这里 
-                                // gcs().send_text(MAV_SEVERITY_CRITICAL, "---- First Turn, Do Sth ----");
-                            }
-                            last_dist = wp_distance;
-                            last_yaw_cd = wp_yaw_cd;
-                        }else{
-                            fast_waypoint = copter.wp_nav->get_fast_waypoint();
                         }
+                        last_dist = wp_distance;
+                        last_yaw_cd = wp_yaw_cd;
+                    }else{
+                        fast_waypoint = copter.wp_nav->get_fast_waypoint();
                     }
+                    
                 }
                 break;
             case MAV_CMD_NAV_RETURN_TO_LAUNCH:
@@ -1712,7 +1723,6 @@ bool ModeAuto::verify_takeoff()
     }else{
         return reached_wp_dest;
     }
-
 }
 
 // verify_land - returns true if landing has been completed
@@ -2044,7 +2054,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
         return false;
     }
     
-    copter.flow_measure.set_wp_index(cmd.index - copter.wp_nav->get_index_start()); // 修正测流起始航点
+    copter.flow_measure.set_wp_index(wp_nav_index - copter.wp_nav->get_index_start()); // 修正测流起始航点
     copter.flow_measure.set_reach_flow_wp(true);  //到达航点标志置1
     if(copter.flow_measure.get_flow_update_state() < 2){
         loiter_time = millis();

ArduCopter/mode_rtl.cpp

@@ -429,8 +429,13 @@ void ModeRTL::descent_run()
     if (tnow - last_1s > 1000) {
         last_1s = tnow;
         
-        copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt);
+        // copter.Log_Write_Data(DATA_UPDOWN_SPEED,rtl_path.descent_target.alt);
-        copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt);
+        // copter.Log_Write_Data(DATA_UPDOWN_SPEED,copter.current_loc.alt);
+        AP::logger().Write("RTES", "TimeUS,Stat,Dalt,Calt", "QBii",
+                                   AP_HAL::micros64(),
+                                   copter.updownStatus,
+                                   rtl_path.descent_target.alt,
+                                   copter.current_loc.alt);
         if(labs(last_alt - copter.current_loc.alt) < 30){
             timeout_count += 1;
         }else{

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.17-RC4"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.17-RC5"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,17,FIRMWARE_VERSION_TYPE_OFFICIAL

d100h.sh

@@ -1,3 +1,3 @@
 ./waf configure --board d100h
 ./waf copter
-cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/d100h-v4.0.16-AVDv6.px4
+cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/d100h-v4.0.17-RC5.px4