增加测流等待时长

ArduCopter/mode_auto.cpp

@@ -1897,7 +1897,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
     // start timer if necessary
     if (loiter_time == 0) {
         loiter_time = millis();
-        loiter_time_max = wp_nav->get_measure_time() + 10;// 最大测流时间
+        loiter_time_max = wp_nav->get_measure_time() + 20;// 最大测流时间
     }
 
     if(copter.flow_measure.get_flow_date_flag())  // 

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -363,6 +363,13 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
 
     static uint8_t manual_cnt = 0;  //  手动测流计数
     measure_time = time;
+// #if USE_SITL
+
+//     if(1){ 
+// #else
+
+//     if((get_reach_flow_wp() || start_sw )){ 
+// #endif
     if((get_reach_flow_wp() || start_sw )){ 
         // set_auto_measure_mode(true);
         static uint8_t repeat;
@@ -491,11 +498,13 @@ void Flow_Measure::data_package()
 {
 #if USE_SITL    
 
-    data_to_mp.index++;
+    // data_to_mp.index+=1;
+    static uint8_t cnt_index;
+    cnt_index +=1;
         data_to_mp.hh_elevation += 500;
         data_to_mp.flow_velocity += 100;
-    if(data_to_mp.index>10){
-        data_to_mp.index = 1;
+    if(cnt_index>10){
+        cnt_index = 1;
         data_to_mp.hh_elevation = 1000;
         data_to_mp.flow_velocity = 100;
         
@@ -504,17 +513,23 @@ void Flow_Measure::data_package()
     pack_data.value.head = FLOW_HEAD;
     pack_data.value.flag = 0x03;
     pack_data.value.length = 0x1C;
-    // if(data_to_mp.index == 0)
-    //     data_to_mp.index = 1;
-    // pack_data.value.index = data_to_mp.index;
-    pack_data.value.index = (data_to_mp.index) | ((data_to_mp.index+1) << 8);
+    // if(cnt_index == 0)
+    //     cnt_index = 1;
+    // pack_data.value.index = cnt_index;
+    pack_data.value.index = (cnt_index) | ((cnt_index+1) << 8);
+    data_to_mp.index+= pack_data.value.index;
     pack_data.value.alt = data_to_mp.hh_elevation;
     my_endianswap(pack_data.data,3,2);
     my_endianswap(pack_data.data,5,4);
     memcpy( data_to_mp.radar_value, test_ladar_data,sizeof(test_ladar_data));
 
     memcpy( pack_data.value.radar_array, data_to_mp.radar_value,22);
-    // memcpy( data_to_mp.raw_array, test_ladar_arr,44);  // 拷贝打包的数据到raw_array
+    
+    memcpy( data_to_mp.raw_array, pack_data.data,44);  // 拷贝打包的数据到raw_array
+    
+    radar_alt = pack_data.value.radar_array[0]<<8 | pack_data.value.radar_array[1];
+    gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] %2d, %3ds, %4.2fm, %2.2fm, %4d",cnt_index,measure_time,data_to_mp.hh_elevation/100.0,radar_alt/1000.0,data_to_mp.flow_velocity);
+
     return;
 #else
     pack_data.value.head = FLOW_HEAD;
@@ -543,7 +558,13 @@ void Flow_Measure::data_package()
 void Flow_Measure::update(void)
 {
 #if USE_SITL
-    if(data_ready){
+    data_ready = 1;
+    is_data_flag = 1;
+    static uint8_t cnt;
+    cnt+=1;
+    if(data_ready && cnt > 50){
+        cnt = 0;
+    // if(1){
         if(is_data_flag){
             // data_to_mp.index++;
             // if(data_to_mp.index>10)
@@ -560,6 +581,7 @@ void Flow_Measure::update(void)
                 data_to_mp.flow_velocity = (int32_t)(flow_value.f*100);
 
             data_package();
+            gcs().send_message(MSG_FLOW_MEASURE);
         }
         else{
             data_to_mp.cmd = 1; // ready to send