From a0fe5ab78e29b61daa3c977f469df3c91d867308 Mon Sep 17 00:00:00 2001 From: zbr Date: Wed, 26 May 2021 14:15:59 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E6=B5=81=EF=BC=8C=E7=AD=89=E5=BE=85?= =?UTF-8?q?=E6=97=B6=E9=97=B4=E5=A2=9E=E5=8A=A0=E5=8F=82=E6=95=B0=E9=85=8D?= =?UTF-8?q?=E7=BD=AE=EF=BC=8C=E5=B8=A7=E5=A4=B4=E5=88=A4=E6=96=AD=E6=94=B9?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Parameters.cpp | 8 ++++---- ArduCopter/UserCode.cpp | 1 + ArduCopter/mode_auto.cpp | 10 ++++++---- ArduCopter/version.h | 6 +++--- flow_zrv4.sh | 3 +++ libraries/AC_Flow_Measure/AC_Flow_Measure.cpp | 20 ++++++++++--------- libraries/AC_WPNav/AC_WPNav.cpp | 1 + libraries/AC_WPNav/AC_WPNav.h | 1 + 8 files changed, 30 insertions(+), 20 deletions(-) create mode 100755 flow_zrv4.sh diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 0aded5903b..d92ddf87de 100644 --- a/ArduCopter/Parameters.cpp +++ b/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: diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 3ddd189e9f..2054b487a8 100644 --- a/ArduCopter/UserCode.cpp +++ b/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 diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a2f8e9f795..3cd6e827bc 100644 --- a/ArduCopter/mode_auto.cpp +++ b/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; diff --git a/ArduCopter/version.h b/ArduCopter/version.h index f6f1fd9ec6..edb1651049 100644 --- a/ArduCopter/version.h +++ b/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 diff --git a/flow_zrv4.sh b/flow_zrv4.sh new file mode 100755 index 0000000000..bc07027de9 --- /dev/null +++ b/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 diff --git a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp index 3f9e19ab1a..27a221d12e 100755 --- a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp +++ b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp @@ -12,10 +12,8 @@ #define MY_USE_DEBUG 0 #if MY_USE_DEBUG -#include - # 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;itxspace()); 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; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index db4044622f..2d32f82d64 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/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 }; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 603a44b6d0..091c19c505 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/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: