测流数据传输测试成功

ArduCopter/Copter.h

@@ -1037,7 +1037,8 @@ public:
     
     void zr_SuperSlowLoop();
     void zr_SlowLoop();
-
+    bool flow_start_sw;
+    bool flow_index_sw;
 };
 
 extern Copter copter;

ArduCopter/GCS_Mavlink.cpp

@@ -124,6 +124,11 @@ void GCS_MAVLINK_Copter::send_zr_flying_status(){
     );
 }
 
+void GCS_MAVLINK_Copter::send_flow_measure_data(){
+
+    copter.flow_measure.mav_send_flow_measure(chan);
+}
+
 void GCS_MAVLINK_Copter::send_position_target_local_ned()
 {
 #if MODE_GUIDED_ENABLED == ENABLED

ArduCopter/GCS_Mavlink.h

@@ -28,6 +28,7 @@ protected:
     void send_position_target_local_ned() override;
 
     void send_zr_flying_status() override;
+    void send_flow_measure_data() override;
     
     MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override;
     MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override;

ArduCopter/UserCode.cpp

@@ -11,6 +11,7 @@ void Copter::userhook_init()
 
     relay.on(1);
     set_mode(Mode::Number::LOITER, ModeReason::STARTUP);
+    flow_measure.init(serial_manager);
 
 }
 #endif
@@ -33,6 +34,7 @@ void Copter::userhook_50Hz()
 void Copter::userhook_MediumLoop()
 {
     // put your 10Hz code here
+    flow_measure.update();
    
 }
 #endif
@@ -51,6 +53,12 @@ void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
     zr_SuperSlowLoop();
+    
+    // RC6 测流开关，RC9 计数开关
+    uint8_t measure_time = wp_nav->get_measure_time();
+    int32_t real_alt = gps.location().alt;  
+    flow_measure.flow_auto_measure(flow_start_sw,flow_index_sw, real_alt, measure_time);
+
 }
 #endif
 
@@ -59,21 +67,40 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag)
 {
     // put your aux switch #1 handler here (CHx_OPT = 47)
 
-    // switch (ch_flag) {
-    //     case 2: {
-    //         relay.on(2);
-    //         break;
-    //     }
-    //     case 0: {
-    //         relay.off(2);
-    //         break;
-    //     }
-    // }
+    switch (ch_flag) {
+        case 2: {
+            // relay.on(2);
+            flow_start_sw = true;
+            
+            gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow measure: on!");
+            break;
+        }
+        case 0: {
+            // relay.off(2);
+            flow_start_sw = false;
+            gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow measure: off!");
+            break;
+        }
+    }
 }
 
 void Copter::userhook_auxSwitch2(uint8_t ch_flag)
 {
     // put your aux switch #2 handler here (CHx_OPT = 48)
+    switch (ch_flag) {
+        case 2: {
+            // relay.on(2);
+            flow_index_sw = true;
+            gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow cnt: on!");
+            break;
+        }
+        case 0: {
+            // relay.off(2);
+            flow_index_sw = false;
+            gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow cnt: off!");
+            break;
+        }
+    }
 }
 
 void Copter::userhook_auxSwitch3(uint8_t ch_flag)

ArduCopter/zr_flight.cpp

@@ -94,12 +94,12 @@ void Copter::zr_SuperSlowLoop(){
 
     // gcs().send_message(MSG_ZR_FLYING_STATUS);
 
-    // RC6 测流开关，RC9 计数开关
-    uint16_t start_sw = RC_Channels::rc_channel(CH_6)->get_radio_in();
-    uint16_t index_sw = RC_Channels::rc_channel(CH_9)->get_radio_in();
-    uint8_t measure_time = wp_nav->get_measure_time();
-    int32_t real_alt = gps.location().alt;  
-    flow_measure.flow_auto_measure(start_sw,index_sw, real_alt, measure_time);
+    // // RC6 测流开关，RC9 计数开关
+    // uint16_t flow_start_sw = RC_Channels::rc_channel(CH_6)->get_radio_in();
+    // uint16_t flow_index_sw = RC_Channels::rc_channel(CH_9)->get_radio_in();
+    // uint8_t measure_time = wp_nav->get_measure_time();
+    // int32_t real_alt = gps.location().alt;  
+    // flow_measure.flow_auto_measure(flow_start_sw,flow_index_sw, real_alt, measure_time);
 
 }
 #endif

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -7,7 +7,7 @@
  */
 /* AC_Flow_Measure.cpp */
 #include "AC_Flow_Measure.h"
-
+#include <GCS_MAVLink/GCS.h>
 #include <AP_HAL/AP_HAL.h>  // AP_HAL::millis()
 
 #define MY_USE_DEBUG 0
@@ -17,7 +17,7 @@
                         __FUNCTION__, __LINE__, \
                         ## args)
 #else 
-//  # define Debug(fmt, args ...) copter.gcs().send_text(MAV_SEVERITY_INFO, fmt ## args)
+//  # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO,fmt, ## args)
  # define Debug(fmt, args ...) 
 #endif
 extern const AP_HAL::HAL& hal;
@@ -99,6 +99,7 @@ bool Flow_Measure::read(void)
     uint32_t nbytes = _uart->available();  
     while(nbytes-->0)
     {
+        // Debug("read:%d",nbytes);
         uint8_t temp = _uart->read();
         // uint8_t aaa[1] = {temp};
         // send_cmd_to_radar(aaa,1);
@@ -351,13 +352,13 @@ void Flow_Measure::my_endianswap(uint8_t * pData,uint8_t startIndex,uint8_t leng
  * @param alt 传入海拔高度
  * @param time 测流历时
  */
-void Flow_Measure::flow_auto_measure(uint16_t start_sw,uint16_t index_sw,int32_t alt,uint8_t time)
+void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uint8_t time)
 {   
   
 
     static uint8_t manual_cnt = 0;  //  手动测流计数
     measure_time = time;
-    if((get_reach_flow_wp() || start_sw > 1400)){ 
+    if((get_reach_flow_wp() || start_sw )){ 
         // set_auto_measure_mode(true);
         static uint8_t repeat;
 
@@ -366,6 +367,12 @@ void Flow_Measure::flow_auto_measure(uint16_t start_sw,uint16_t index_sw,int32_t
         // const struct Location &loc = gps.location();  
         // int16_t delt_alt = gps.get_gps_delt_alt();
         // int32_t real_alt = loc.alt + delt_alt;
+        // static uint8_t last_state = 5;
+        // if(last_state != flow_update_state)
+        // {
+        //     last_state = flow_update_state;
+        //     gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]flow_update_state:%d",flow_update_state);
+        // }
         switch (flow_update_state)
         {
 
@@ -414,7 +421,8 @@ void Flow_Measure::flow_auto_measure(uint16_t start_sw,uint16_t index_sw,int32_t
                     set_is_data_flag(false);
                     send_cmd(START_MEASURE);
                     repeat = 0;
-                    // gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
+                    // gcs().send_text(MAV_SEVERITY_NOTICE, "[INFO]设置测流历时: %3ds",measure_time);
+                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
                 }
                 // else if(index_sw > 1400)
                 // {
@@ -434,7 +442,7 @@ void Flow_Measure::flow_auto_measure(uint16_t start_sw,uint16_t index_sw,int32_t
                 // send_cmd(GET_DATA);
                 if(get_radar_data())
                 {
-                    if(index_sw > 1400){
+                    if(index_sw){
                         manual_cnt++;
                         set_wp_index(manual_cnt);
                     }
@@ -456,15 +464,15 @@ void Flow_Measure::flow_auto_measure(uint16_t start_sw,uint16_t index_sw,int32_t
                     set_is_data_flag(true);
                     send_cmd(GET_DATA);
                     repeat = 0;
-                    // gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
+                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
                 }
                 break;
         }
 
-    }else if(!get_reach_flow_wp() && start_sw < 1400 ){
+    }else if(!get_reach_flow_wp() && !start_sw  ){
         flow_update_state = 1;
         
-        if(index_sw < 1400){
+        if(!index_sw){
             manual_cnt = 0;
             set_wp_index(manual_cnt);
         }

libraries/AC_Flow_Measure/AC_Flow_Measure.h

@@ -93,7 +93,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(uint16_t start_sw,uint16_t index_sw,int32_t alt,uint8_t time);
+    void flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uint8_t time);
 
 
     void get_gps_alt(int32_t alt){data_to_mp.hh_elevation = alt;};

libraries/AC_WPNav/AC_WPNav.cpp

@@ -74,6 +74,9 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("FAST_WP",   11, AC_WPNav, _fast_wp, 0),
 
+    
+    AP_GROUPINFO("DELAY_TIME",   12, AC_WPNav, _measure_delay_time, 10),
+
     AP_GROUPEND
 };
 

libraries/GCS_MAVLink/GCS.h

@@ -220,6 +220,9 @@ public:
     void send_rpm() const;
     bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t);
     bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t);
+
+    virtual void send_flow_measure_data() {};
+    
     //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t );
     bool locked() const;
 

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -4506,10 +4506,14 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
         send_autopilot_version();
         break;
 
-        case MSG_ZR_FLYING_STATUS:
+    case MSG_ZR_FLYING_STATUS:
          CHECK_PAYLOAD_SIZE(ZR_FLYING_STATUS);
          send_zr_flying_status();
          break;
+    case MSG_FLOW_MEASURE:
+         CHECK_PAYLOAD_SIZE(ZR_FLYING_STATUS);
+         send_flow_measure_data();
+         break;
 
     case MSG_ESC_TELEMETRY: {
 #ifdef HAVE_AP_BLHELI_SUPPORT