From 1267444028ab60ad4277e5a211f9290325bff2fb Mon Sep 17 00:00:00 2001 From: zbr Date: Tue, 28 Dec 2021 16:32:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B5=8B=E6=B5=81=E6=A8=A1=E5=BC=8F=EF=BC=8C?= =?UTF-8?q?=E6=89=8B=E5=8A=A8=E6=B5=8B=E6=B5=81=E5=85=B3=E8=81=94=E8=88=AA?= =?UTF-8?q?=E7=82=B9=E5=BA=8F=E5=8F=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/Copter.h | 4 +- ArduCopter/GCS_Mavlink.cpp | 8 +- ArduCopter/UserCode.cpp | 43 +------- ArduCopter/mode_auto.cpp | 14 +-- ArduCopter/zr_flight.cpp | 99 +++++++++++++++++++ libraries/AC_Flow_Measure/AC_Flow_Measure.cpp | 34 +++---- libraries/AC_Flow_Measure/AC_Flow_Measure.h | 11 ++- 7 files changed, 143 insertions(+), 70 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 53e429b99e..0af4b9746c 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -1083,8 +1083,8 @@ public: /****** 测流 ******/ void zr_mission_flow_mode(); - bool flow_start_sw; - bool flow_index_sw; + void add_flow_wp(); + void get_mindist_flow_wp(); }; extern Copter copter; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 18bd56d267..3b15ebfe95 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -179,13 +179,19 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const } const Vector3f &targets = copter.attitude_control->get_att_target_euler_cd(); const Mode *flightmode = copter.flightmode; + float wp_dist; + if(copter.control_mode != Mode::Number::AUTO && copter.g.zr_mission_type == 1){ + wp_dist = copter.flow_measure.flow_wp_distance; + }else{ + wp_dist = flightmode->wp_distance() * 1.0e-2f; + } mavlink_msg_nav_controller_output_send( chan, targets.x * 1.0e-2f, targets.y * 1.0e-2f, targets.z * 1.0e-2f, flightmode->wp_bearing() * 1.0e-2f, - MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX), + MIN(wp_dist, UINT16_MAX),// MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX), copter.pos_control->get_alt_error() * 1.0e-2f, 0, flightmode->crosstrack_error() * 1.0e-2f); diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 0585469875..2b854bd57a 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -146,19 +146,18 @@ void Copter::userhook_SuperSlowLoop() void Copter::userhook_auxSwitch1(uint8_t ch_flag) { // put your aux switch #1 handler here (CHx_OPT = 47) - if(g.zr_mission_type == 1){ // 测流模式 switch (ch_flag) { case 2: { // relay.on(2); - flow_start_sw = true; + flow_measure.flow_start_sw = true; gcs().send_text(MAV_SEVERITY_NOTICE, "NOTICE:开启手动测流"); break; } case 0: { // relay.off(2); - flow_start_sw = false; + flow_measure.flow_start_sw = false; gcs().send_text(MAV_SEVERITY_NOTICE, "NOTICE:关闭手动测流"); break; } @@ -201,23 +200,7 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag) void Copter::userhook_auxSwitch2(uint8_t ch_flag) { // put your aux switch #2 handler here (CHx_OPT = 48) - - if(g.zr_mission_type == 1){ // 测流模式 - switch (ch_flag) { - case 2: { - // relay.on(2); - flow_index_sw = true; - gcs().send_text(MAV_SEVERITY_NOTICE, "NOTICE:测流序号递增"); - break; - } - case 0: { - // relay.off(2); - flow_index_sw = false; - gcs().send_text(MAV_SEVERITY_NOTICE, "NOTICE:测流序号不变"); - break; - } - } - } + } void Copter::userhook_auxSwitch3(uint8_t ch_flag) @@ -305,24 +288,4 @@ void Copter::zr_mkdir_in_takeoff() } } -void Copter::zr_mission_flow_mode() -{ - // RC6 测流开关，RC9 计数开关 - uint8_t measure_time = wp_nav->get_measure_time(); - int32_t real_alt; - flow_measure.set_radar_mode(g.zr_flow_type); //设置雷达模式，单测流或者三合一等 - flow_measure.set_radar_sens(g.zr_flow_sens); //设置雷达灵敏度 - - if(g.zr_flow_type == 2) // 单测流+指定水位高 - real_alt = g.zr_flow_real_elevation; // 直接设置水位高度 - else{ - 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); - if(control_mode != Mode::Number::AUTO){ - flow_measure.set_reach_flow_wp(false); - } - - // 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 51200cc236..669898103a 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -2084,9 +2084,9 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd if ( !copter.wp_nav->reached_wp_destination() ) { return false; } - 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_wp_auto_index(copter.flow_measure.flow_wp_index); // 第一次，先发个数据给测流 } copter.flow_measure.set_reach_flow_wp(true); //到达航点标志置1 if(copter.flow_measure.get_flow_update_state() < 2){ @@ -2105,9 +2105,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); - 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 计数 + gcs().send_text(MAV_SEVERITY_INFO, "get data wp:#%i",copter.flow_measure.flow_wp_index); + copter.flow_measure.set_wp_auto_index(copter.flow_measure.flow_wp_index); // 修正测流起始航点 + copter.flow_measure.flow_wp_index += 1; // nav 计数 + return true; // 如果获取到流速，返回1 } // check if timer has run out @@ -2115,8 +2116,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd copter.flow_measure.set_reach_flow_wp(false); gcs().send_text(MAV_SEVERITY_INFO, "获取数据超时,序号:#%i",cmd.index); - wp_nav_index += 1; // nav 计数 - copter.mission_nav_index = wp_nav_index; + copter.flow_measure.flow_wp_index += 1; // nav 计数 return true; } return false; diff --git a/ArduCopter/zr_flight.cpp b/ArduCopter/zr_flight.cpp index 17815e2e0c..82584464ff 100644 --- a/ArduCopter/zr_flight.cpp +++ b/ArduCopter/zr_flight.cpp @@ -375,4 +375,103 @@ void Copter::avoid_action(uint8_t area,uint8_t state,float dist) } +/** + * @brief 读取航点，存到_flow_wp中，用于测流航点距离计算 + * + */ +void Copter::add_flow_wp() +{ + // Vector3f* _flow_wp; // 动态存储坐标 + // uint16_t _flow_wp_points_count; // 动态存储计数 + flow_measure.max_flow_wp = mode_auto.mission.num_commands(); + AP_Mission::Mission_Command mission_cmd; + Vector2f pos_ne; + flow_measure._flow_wp_points_count = 0; + for(uint16_t i = 1; i < flow_measure.max_flow_wp - 1;i++) + { + if(mode_auto.mission.read_cmd_from_storage(i,mission_cmd)) { // 读取原本航点 + if (mode_auto.mission.is_nav_cmd(mission_cmd)) { // 是否是Waypoint + if(mission_cmd.content.location.get_vector_xy_from_origin_NE(pos_ne)) //坐标转换 + { + flow_measure._flow_wp[flow_measure._flow_wp_points_count] = pos_ne; // 存到动态存储中 + flow_measure._flow_wp_points_count += 1; + } + } + } + } +} +/** + * @brief 计算当前飞机位置与最近的航点 + * + */ +void Copter::get_mindist_flow_wp() +{ + uint8_t min_dist_index; + static uint8_t last_flow_index = 0; + static float last_dist = 0; + float min_dist = 0; + Vector2f posNE; + Location loc; + if(!ahrs.get_position(loc)){ + return; + } + if (!loc.get_vector_xy_from_origin_NE(posNE)) { + // not breached if we don't now where we are + return; + } + if(flow_measure.last_wp_update_time == 0 || flow_measure.last_wp_update_time != mode_auto.mission.last_change_time_ms()) // 航点更新 + { + flow_measure.last_wp_update_time = mode_auto.mission.last_change_time_ms(); + add_flow_wp(); + gcs().send_text(MAV_SEVERITY_INFO, "Mission update:%d,wp num:%d",flow_measure.last_wp_update_time,flow_measure._flow_wp_points_count); + } + if(flow_measure._flow_wp_points_count > 2){ + float wp_distance; + min_dist = norm(posNE.x-flow_measure._flow_wp[0].x,posNE.y-flow_measure._flow_wp[0].y); // 最小距离先复制第一个航电距离 + min_dist_index = 1; + for(uint16_t i = 1; i < flow_measure._flow_wp_points_count;i++) + { + wp_distance = norm(posNE.x-flow_measure._flow_wp[i].x,posNE.y-flow_measure._flow_wp[i].y)/100.0; + if(min_dist > wp_distance){ + min_dist_index = i ; + min_dist = wp_distance; + } + } + + flow_measure.flow_wp_index = min_dist_index; + flow_measure.flow_wp_distance = min_dist; + if(flow_measure.flow_wp_index != last_flow_index){ + last_flow_index = flow_measure.flow_wp_index; + mode_auto.mission.set_current_cmd(flow_measure.flow_wp_index + 2); + flow_measure.set_wp_auto_index(flow_measure.flow_wp_index); + gcs().send_text(MAV_SEVERITY_INFO, "update index:#%d",flow_measure.flow_wp_index); + } + if(flow_measure.flow_wp_index != last_flow_index || fabs(last_dist - flow_measure.flow_wp_distance) > 2){ + // gcs().send_text(MAV_SEVERITY_INFO, "close to:#%d ,d:%.2f",flow_measure.flow_wp_index,flow_measure.flow_wp_distance); + last_dist = flow_measure.flow_wp_distance; + } + } +} + +void Copter::zr_mission_flow_mode() +{ + // RC6 测流开关，RC9 计数开关 + uint8_t measure_time = wp_nav->get_measure_time(); + int32_t real_alt; + flow_measure.set_radar_mode(g.zr_flow_type); //设置雷达模式，单测流或者三合一等 + flow_measure.set_radar_sens(g.zr_flow_sens); //设置雷达灵敏度 + + if(g.zr_flow_type == 2) // 单测流+指定水位高 + real_alt = g.zr_flow_real_elevation; // 直接设置水位高度 + else{ + real_alt= gps.location().alt + gps.get_gps_delt_alt(); + } + flow_measure.flow_auto_measure(flow_measure.flow_start_sw, real_alt, measure_time); + if(control_mode != Mode::Number::AUTO){ + flow_measure.set_reach_flow_wp(false); + get_mindist_flow_wp(); + } + + // flow_measure.flow_auto_measure(1,1, real_alt, measure_time); +} #endif \ No newline at end of file diff --git a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp index 6654f219b2..142d1c8263 100755 --- a/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp +++ b/libraries/AC_Flow_Measure/AC_Flow_Measure.cpp @@ -65,6 +65,13 @@ Flow_Measure::Flow_Measure(void): */ bool Flow_Measure::init(const AP_SerialManager& serial_manager) { + // protect against repeated call to init + if (_flow_wp != nullptr) { + _flow_wp = nullptr; + } + // allocate arrays + _flow_wp = (Vector2f*)calloc(60, sizeof(Vector2f)); + _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Flow_Measure,0); if (_uart != nullptr) { _uart->begin(9600U); @@ -487,11 +494,9 @@ void Flow_Measure::my_endianswap(uint8_t * pData,uint8_t startIndex,uint8_t leng * @param alt 传入海拔高度 * @param time 测流历时 */ -void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uint8_t time) +void Flow_Measure::flow_auto_measure(bool start_sw,int32_t alt,uint8_t time) { - - static uint8_t manual_cnt = 0; // 手动测流计数 measure_time = time; // #if USE_SITL @@ -618,10 +623,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin // send_cmd(GET_DATA); if(get_radar_data()) { - if(index_sw){ - manual_cnt++; - } - + if(last_measure_sens != radar_sens){ flow_update_state = CHANGE_SENSITY; }else if(last_measure_time != measure_time){ @@ -629,11 +631,9 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin }else{ flow_update_state = GET_ANGLE; } - if(!start_sw && !index_sw){ - data_to_mp.index = auto_index; - }else{ - data_to_mp.index = manual_cnt; - } + + data_to_mp.index = auto_index; + get_gps_alt(alt + delt_alt); if(radar_mode != 1){ // 非单测流雷达 data_package(); @@ -651,16 +651,16 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin is_data_flag = 1; if(radar_mode == 1){ // 单测流 - if(manual_cnt == 1) + if(data_to_mp.index == 1) gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式] 序号, 历时, 角度, 流速, 数组"); gcs().send_text(MAV_SEVERITY_CRITICAL, "%2d, %2ds, %2.1f, %2.2f, 0x%02x%02x%02x%02x",get_wp_index(),measure_time,pitch.d/10.0,data_to_mp.flow_velocity/100.0, \ data_to_mp.radar_value[3],data_to_mp.radar_value[4],data_to_mp.radar_value[5],data_to_mp.radar_value[6]); }else if(radar_mode == 2){ // 指定空高 单测流 - if(manual_cnt == 1) + if(data_to_mp.index == 1) gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式]序号,历时,角度,水位,流速"); gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] %2d, %2ds, %2.1f, %4.2fm, %2.2fm/s",get_wp_index(),measure_time,pitch.d/10.0,alt/100.0,data_to_mp.flow_velocity/100.0); }else{ // 三合一 - if(manual_cnt == 1) + if(data_to_mp.index == 1) gcs().send_text(MAV_SEVERITY_CRITICAL, "[格式]序号,历时,角度,海拔,空高,流速"); gcs().send_text(MAV_SEVERITY_CRITICAL, "[数据] %2d, %2ds, %2.1f, %4.2fm, %2.2fm, %2.2fm/s",get_wp_index(),measure_time,pitch.d/10.0,alt/100.0,radar_alt/1000.0,data_to_mp.flow_velocity/100.0); @@ -684,10 +684,6 @@ 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; - } } } diff --git a/libraries/AC_Flow_Measure/AC_Flow_Measure.h b/libraries/AC_Flow_Measure/AC_Flow_Measure.h index 6f5abbad19..f0d8037f17 100755 --- a/libraries/AC_Flow_Measure/AC_Flow_Measure.h +++ b/libraries/AC_Flow_Measure/AC_Flow_Measure.h @@ -100,7 +100,7 @@ public: void send_cmd_to_radar(const uint8_t *data, uint16_t len); void my_endianswap(uint8_t * pData,uint8_t startIndex,uint8_t length); - void flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uint8_t time); + void flow_auto_measure(bool start_sw,int32_t alt,uint8_t time); void get_gps_alt(int32_t alt){data_to_mp.hh_elevation = alt;}; @@ -126,6 +126,15 @@ public: void set_radar_debug(uint8_t debug){radar_debug = debug;}; void set_wp_auto_index(uint16_t index){auto_index = index;}; + float flow_wp_distance; + bool flow_start_sw; + bool flow_index_sw; + uint8_t max_flow_wp; + uint8_t flow_wp_index; + Vector2f* _flow_wp; // 动态存储坐标 + uint16_t _flow_wp_points_count; // 动态存储计数 + uint32_t last_wp_update_time; // 上一次航点更新时间 + private: static Flow_Measure *_singleton;