|
|
|
@ -2086,6 +2086,9 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
@@ -2086,6 +2086,9 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if(copter.flow_measure.get_flow_update_state() > 3){ |
|
|
|
|
if(wp_nav_index == 0){ |
|
|
|
|
copter.flow_measure.flow_wp_index = 1; |
|
|
|
|
} |
|
|
|
|
copter.flow_measure.set_wp_auto_index(copter.flow_measure.flow_wp_index); // 第一次,先发个数据给测流
|
|
|
|
|
} |
|
|
|
|
copter.flow_measure.set_reach_flow_wp(true); //到达航点标志置1
|
|
|
|
@ -2105,9 +2108,10 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
@@ -2105,9 +2108,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",copter.flow_measure.flow_wp_index); |
|
|
|
|
// 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 计数
|
|
|
|
|
wp_nav_index += 1; |
|
|
|
|
|
|
|
|
|
return true; // 如果获取到流速,返回1
|
|
|
|
|
} |
|
|
|
@ -2117,6 +2121,7 @@ bool ModeAuto::verify_nav_wp_flow_measure(const AP_Mission::Mission_Command& cmd
@@ -2117,6 +2121,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); |
|
|
|
|
copter.flow_measure.flow_wp_index += 1; // nav 计数
|
|
|
|
|
wp_nav_index += 1; |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
return false; |
|
|
|
|