测流模式，手动测流关联航点序号

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;

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);

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

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;

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

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;
-        }
     }
 }
 

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;