测流，等待时间增加参数配置，帧头判断改正

ArduCopter/Parameters.cpp

@@ -1668,16 +1668,16 @@ const char* Copter::get_sysid_board_id(void)
     switch (type)
     {
         case 0:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.5 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 1:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.3 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.5 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 2:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.3 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.5 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 3:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.5 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
             break;
         
         default:

ArduCopter/UserCode.cpp

@@ -58,6 +58,7 @@ void Copter::userhook_SuperSlowLoop()
     uint8_t measure_time = wp_nav->get_measure_time();
     int32_t real_alt = gps.location().alt + gps.get_gps_delt_alt();  
     flow_measure.flow_auto_measure(flow_start_sw,flow_index_sw, real_alt, measure_time);
+    // flow_measure.flow_auto_measure(1,1, real_alt, measure_time);
 
 }
 #endif

ArduCopter/mode_auto.cpp

@@ -1891,13 +1891,15 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
     
     copter.flow_measure.set_wp_index(cmd.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)
+    if(copter.flow_measure.get_flow_update_state() < 2){
+        loiter_time = millis();
+        loiter_time_max = wp_nav->get_measure_time() +  wp_nav->_measure_delay_max_time;// 最大测流时间
         return false;
-
+    }
     // start timer if necessary
     if (loiter_time == 0) {
         loiter_time = millis();
-        loiter_time_max = wp_nav->get_measure_time() + 20;// 最大测流时间
+        loiter_time_max = wp_nav->get_measure_time() + wp_nav->_measure_delay_max_time;// 最大测流时间
     }
 
     if(copter.flow_measure.get_flow_date_flag())  // 
@@ -1911,7 +1913,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
     if (((millis() - loiter_time) / 1000) >= loiter_time_max) {
 
         copter.flow_measure.set_reach_flow_wp(false); 
-        gcs().send_text(MAV_SEVERITY_INFO, "Time out ,Get nothing #%i",cmd.index);
+        gcs().send_text(MAV_SEVERITY_INFO, "获取数据超时,序号:#%i",cmd.index);
         return true;
     }
     return false;

ArduCopter/version.h

@@ -6,12 +6,12 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.1.3"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.1.5"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,3,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,1,5,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 1
-#define FW_PATCH 3
+#define FW_PATCH 5
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

flow_zrv4.sh

@@ -0,0 +1,3 @@
+./waf configure --board zr-v4
+./waf copter
+cp ./build/zr-v4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/flow-zrv4.px4

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -12,10 +12,8 @@
 
 #define MY_USE_DEBUG 0
 #if MY_USE_DEBUG
-#include <systemlib/err.h>
- # define Debug(fmt, args ...)     warnx("%s():%d: " fmt "\n",     \
-                        __FUNCTION__, __LINE__, \
-                        ## args)
+
+ # define Debug(fmt, args ...)  gcs().send_text(MAV_SEVERITY_INFO, "-- " fmt " --", ## args)
 #else 
 //  # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO,fmt, ## args)
  # define Debug(fmt, args ...) 
@@ -128,7 +126,7 @@ bool Flow_Measure::parse(uint8_t temp)
     {
         default:
         case DEVICE_ID:
-            if(temp == FLOW_HEAD || 1){
+            if(temp == FLOW_HEAD || temp == 0x00){
                 read_len = 0;
                 // radar_data.value.radar_arr[read_len] = temp;
                 radar_data.data[read_len] = temp;
@@ -172,19 +170,22 @@ bool Flow_Measure::parse(uint8_t temp)
             break;
         case LENGTH:
             data_len = temp;
+            if(data_len > 0x16){
+                data_len = 0x16;
+            }
             // radar_data.value.radar_arr[read_len] = temp;
                 radar_data.data[read_len] = temp;
             read_len += 1;
             MBCRC16(temp);
             flow_state = DATA;
-            Debug("LENGTH:0x%02x",temp);
+            Debug("LENGTH:0x%02x,len:%02x\n",temp,data_len);
             // Debug("Data:\n\r");
             break;
 
         case DATA:
             // radar_data.value.radar_arr[read_len] = temp;
                 radar_data.data[read_len] = temp;
-            // Debug("0x%02x ",temp);
+            Debug("0x%02x ",temp);
             MBCRC16(temp);
             read_len += 1;
             data_len--;
@@ -207,7 +208,7 @@ bool Flow_Measure::parse(uint8_t temp)
             // read_len += 1;
             // radar_data.value.crc_h = temp;
             flow_state = DEVICE_ID;
-            Debug("CRC2:0x%02x",temp);
+            // Debug("CRC2:0x%02x",temp);
             // Debug("CRC H:0x%02x , L:0x%02x",ucCRCLo,ucCRCHi);
             // Debug("crc_h:0x%02x , l:0x%02x",radar_data.value.crc_h,radar_data.value.crc_l);
             // for(int i = 0;i<read_len;i++){
@@ -291,6 +292,7 @@ void Flow_Measure::send_cmd_to_radar(const uint8_t *data, uint16_t len)
     if (_uart == nullptr) {
         return;
     }
+    Debug("---- tx len:%d,space:%d----",len,_uart->txspace());
     if (_uart->txspace() > len) {
         _uart->write(data, len);
     } else {
@@ -474,7 +476,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                     repeat = 0;
                     send_to_mavlink_cnt = 5;  // 发送5次Mavlink消息
                     
-                }else if(repeat++ > 2){
+                }else if(repeat++ > 4){
                     set_is_data_flag(true);
                     send_cmd(GET_DATA);
                     repeat = 0;

libraries/AC_WPNav/AC_WPNav.cpp

@@ -77,6 +77,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
     
     AP_GROUPINFO("DELAY_TIME",   12, AC_WPNav, _measure_delay_time, 10),
     AP_GROUPINFO("INDEX_STR",   13, AC_WPNav, _measure_index_start, 2),
+    AP_GROUPINFO("DELAY_MAX",   14, AC_WPNav, _measure_delay_max_time, 30),
 
     AP_GROUPEND
 };

libraries/AC_WPNav/AC_WPNav.h

@@ -226,6 +226,7 @@ public:
     bool get_fast_waypoint() const { return _fast_wp; }
     uint8_t get_measure_time(void){return (uint8_t)_measure_delay_time;};
     uint8_t get_index_start(void){return (uint8_t)_measure_index_start;};
+    AP_Int8     _measure_delay_max_time;  //modify by @Brown
 
 protected: