测流航点序号调整

ArduCopter/mode_auto.cpp

@@ -2053,8 +2053,10 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
     if ( !copter.wp_nav->reached_wp_destination() ) {
         return false;
     }
-    
-    copter.flow_measure.set_wp_index(wp_nav_index - copter.wp_nav->get_index_start()); // 修正测流起始航点
+    uint8_t flow_index = wp_nav_index+1;
+    if(copter.flow_measure.get_flow_update_state() > 3){
+        copter.flow_measure.set_wp_auto_index(flow_index); // 第一次，先发个数据给测流
+    }
     copter.flow_measure.set_reach_flow_wp(true);  //到达航点标志置1
     if(copter.flow_measure.get_flow_update_state() < 2){
         loiter_time = millis();
@@ -2071,7 +2073,10 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
     {  
         // states = 1;
         copter.flow_measure.set_reach_flow_wp(false); 
-        copter.flow_measure.set_flow_date_flag(false); 
+        copter.flow_measure.set_flow_date_flag(false);
+        gcs().send_text(MAV_SEVERITY_INFO, "get data wp:#%i",flow_index);
+        // copter.flow_measure.set_wp_auto_index(flow_index); // 修正测流起始航点
+        wp_nav_index += 1;   // nav 计数
         return true; // 如果获取到流速，返回1
     }
     // check if timer has run out

all.sh

@@ -4,33 +4,33 @@ starttime=`date +'%Y-%m-%d %H:%M:%S'`
 
 
 ./waf configure --board rs100 > build_log.txt
-# ./waf clean >> build_log.txt
+./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
 cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.0.17-RC4.px4
 echo "finish build rs100 "
 ./waf configure --board rs100h >> build_log.txt
-# ./waf clean >> build_log.txt
+./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
 cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100h-v4.0.17-RC4.px4
 echo "finish build rs100h "
 
 ./waf configure --board d100 >> build_log.txt
-# ./waf clean >> build_log.txt
+./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
 cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.0.17-RC4.px4
 echo "finish build d100 "
 
 ./waf configure --board d100h >> build_log.txt
-# ./waf clean >> build_log.txt
+./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
 cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100h-v4.0.17-RC4.px4
 echo "finish build d100h "
 
-./waf configure --board zr-hexa >> build_log.txt
+# ./waf configure --board zr-hexa >> build_log.txt
 # ./waf clean >> build_log.txt
-./waf copter >> build_log.txt
-cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.0.17-RC4.px4
-echo "finish build zr-hexa "
+# ./waf copter >> build_log.txt
+# cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.0.17-RC4.px4
+# echo "finish build zr-hexa "
 
 endtime=`date +'%Y-%m-%d %H:%M:%S'`
 date -R

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -303,6 +303,10 @@ bool Flow_Measure::parse(uint8_t temp)
  */
 void Flow_Measure::mav_send_flow_measure(mavlink_channel_t chan)    
 {
+    if(data_to_mp.index == 0){
+        // gcs().send_text(MAV_SEVERITY_NOTICE, "--- err: wp:%d,don't send", data_to_mp.index);
+        return;
+    }
     mavlink_msg_flow_measure_send(chan,
         data_to_mp.cmd,
         data_to_mp.index,
@@ -513,6 +517,15 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
         //     last_state = flow_update_state;
         //     gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow_update_state:%d",flow_update_state);
         // }
+        
+        if(flow_update_state < GET_DATA){
+            
+            set_flow_date_flag(false);
+        }
+        
+        if(!index_sw || !start_sw){
+            data_to_mp.index = auto_index;
+        }
         switch (flow_update_state)
         {
 
@@ -671,7 +684,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
 
     }else if(!get_reach_flow_wp() && !start_sw  ){
         flow_update_state = START_MEASURE;
-        
+
         if(!index_sw){
             manual_cnt = 0;
             set_wp_index(manual_cnt);

libraries/AC_Flow_Measure/AC_Flow_Measure.h

@@ -125,6 +125,7 @@ public:
     void set_radar_mode(uint8_t mode){radar_mode = mode;};
     void set_radar_sens(uint8_t sens){radar_sens = sens;};
     void set_radar_debug(uint8_t debug){radar_debug = debug;};
+    void set_wp_auto_index(uint16_t index){auto_index = index;};
 
 private:
     static Flow_Measure *_singleton;
@@ -222,6 +223,7 @@ private:
     uint8_t radar_debug;
 
     ch2int16 roll,pitch;
+    uint16_t auto_index;
 };
 
 namespace AP {

libraries/AP_Mission/AP_Mission.cpp

@@ -2073,7 +2073,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
         cmd_rtl.content.location.lng = r_lng;
         cmd_rtl.id = MAV_CMD_NAV_WAYPOINT;
         // cmd_rtl.content.location.options = 0;
-        // cmd_rtl.p1 = 2;     // 到航点后延时2s
+        cmd_rtl.p1 = 2;     // 到航点后延时2s
         // cmd_rtl.content.location.flags.relative_alt = 1;
 
         replace_cmd(cmd_rtl_index, cmd_rtl);