add flow test

ArduCopter/Parameters.cpp

@@ -55,6 +55,8 @@ const AP_Param::Info Copter::var_info[] = {
     // @Description: For mark board name last
   // @User: @Binsir
     GSCALAR(sysid_board_name_2nd,   "SYSID_BDNAME_P2",    1001),
+    // 0:rs100, 1:M66,  2:RF610   3:zrzk.20qt2
+    GSCALAR(sysid_type,   "SYSID_TYPE",   0 ),
 
     // @Param: SYSID_THISMAV
     // @DisplayName: MAVLink system ID of this vehicle
@@ -1631,11 +1633,25 @@ const char* Copter::get_sysid_board_id(void)
 
      int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;
-
-    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+     int8_t type = g.sysid_type;
+    switch (type)
+    {
+        case 0:
             snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
-    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
-    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
+            break;
+        case 1:
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
+            break;
+        case 2:
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
+            break;
+        case 3:
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.0.3 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+            break;
+        
+        default:
+            break;
+    }
     AP::logger().Write_Message(buf);
     return buf;
 }

ArduCopter/Parameters.h

@@ -204,7 +204,7 @@ public:
 
         k_param_sysid_board_name_1st =106, //add by @Binsir
         k_param_sysid_board_name_2nd=107, //add by @Binsir
-        k_param_sysid_board_id=108, // modify by @Brown
+        k_param_sysid_type=108, // modify by @Brown
 
         // 110: Telemetry control
         //
@@ -392,7 +392,7 @@ public:
 
     AP_Int16        format_version;
 
-    // AP_Int32        sysid_board_id; // modify by @Brown
+    AP_Int8        sysid_type; // modify by @Brown
     AP_Int16        sysid_board_name_1st;// modify by @Binsir
     AP_Int16         sysid_board_name_2nd;// modify by @Binsir
 

libraries/AP_OpticalFlow/OpticalFlow.cpp

@@ -83,6 +83,12 @@ const AP_Param::GroupInfo OpticalFlow::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_ADDR", 5,  OpticalFlow, _address,   0),
 
+    // @Param: _THRES
+    // @Description: min surface_quality value, below this value don't use flow
+    // @Range: 0 127
+    // @User: Brown
+    AP_GROUPINFO("_THRES", 6,  OpticalFlow, _threshold,   100),
+
     // the parameter description below is for GCSs (like MP) that use master for the parameter descriptions.  This should be removed when Copter-3.7 is released
     // @Param: _ENABLE
     // @DisplayName: Optical flow enable/disable
@@ -161,7 +167,8 @@ void OpticalFlow::update(void)
     }
 
     // only healthy if the data is less than 0.5s old
-    _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
+    // _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500);
+    _flags.healthy = (AP_HAL::millis() - _last_update_ms < 500)&&(_state.surface_quality < _threshold);
 }
 
 void OpticalFlow::handle_msg(const mavlink_message_t &msg)
@@ -175,12 +182,21 @@ void OpticalFlow::handle_msg(const mavlink_message_t &msg)
         backend->handle_msg(msg);
     }
 }
-
+#include <GCS_MAVLink/GCS.h>
 void OpticalFlow::update_state(const OpticalFlow_state &state)
 {
     _state = state;
     _last_update_ms = AP_HAL::millis();
 
+    // static uint8_t last_quaty;
+    // if(abs(last_quaty - _state.surface_quality)> 30){
+    //     last_quaty = _state.surface_quality;
+    //     gcs().send_text(MAV_SEVERITY_INFO,"thr:%d,q:%d",_threshold,_state.surface_quality);
+    // }
+    if(_state.surface_quality < _threshold )
+    {
+        _state.surface_quality = 0;
+    }
     // write to log and send to EKF if new data has arrived
     AP::ahrs_navekf().writeOptFlowMeas(quality(),
                                        _state.flowRate,

libraries/AP_OpticalFlow/OpticalFlow.h

@@ -111,6 +111,7 @@ private:
     AP_Int16 _yawAngle_cd;          // yaw angle of sensor X axis with respect to vehicle X axis - centi degrees
     AP_Vector3f _pos_offset;        // position offset of the flow sensor in the body frame
     AP_Int8  _address;              // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
+    AP_Int8  _threshold;              // address on the bus (allows selecting between 8 possible I2C addresses for px4flow)
 
     // method called by backend to update frontend state:
     void update_state(const OpticalFlow_state &state);