增加串口API频率设置参数

ArduCopter/Parameters.cpp

@@ -783,6 +783,7 @@ const AP_Param::Info Copter::var_info[] = {
     GOBJECT(g2, "",  ParametersG2),
 
     GOBJECT(zr_app, "ZR",  AC_ZR_App),
+    GOBJECT(zr_serial_api, "ZR",  AC_ZR_Serial_API),
     AP_VAREND
 };
 

ArduCopter/Parameters.h

@@ -403,6 +403,7 @@ public:
         k_param_zr_takeoff_prt_ps,
 
         k_param_zr_app = 322, // 322 zr param
+        k_param_zr_serial_api = 323, // 322 zr param
 
         // the k_param_* space is 9-bits in size
         // 511: reserved

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -33,6 +33,23 @@
 
 AC_ZR_Serial_API *AC_ZR_Serial_API::_singleton;
 
+
+// table of user settable parameters
+const AP_Param::GroupInfo AC_ZR_Serial_API::var_info[] = {
+    // @Param: _TYPE
+    // @DisplayName: user type
+    // @Description: parameters test
+    // @Values: 0:None,1:other
+    // @RebootRequired: True
+    // @User: zrzk
+    AP_GROUPINFO("_SER_DBG",           0,  AC_ZR_Serial_API,  parm_msg_debug,   1),
+
+    AP_GROUPINFO("_SER_DAT_FQ",        1,  AC_ZR_Serial_API,  parm_data_freq,     1 ),
+    AP_GROUPINFO("_SER_STU_FQ",          2,  AC_ZR_Serial_API,  parm_status_freq, 1),
+
+    AP_GROUPEND
+};
+
 // constructor
 AC_ZR_Serial_API::AC_ZR_Serial_API()
 {
@@ -67,6 +84,12 @@ void AC_ZR_Serial_API::init(const AP_SerialManager& serial_manager)
     data_trans_init =  init_host;
 }
 
+void AC_ZR_Serial_API::get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq)
+{
+    debug = parm_msg_debug; 
+    data_freq = parm_data_freq; 
+    status_freq = parm_status_freq; 
+}
 
 /**
  * @brief 数据写入串口
@@ -166,16 +189,22 @@ void AC_ZR_Serial_API::get_vehicle_euler_angles(Vector3f euler)
 
 void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t home_distance,uint16_t volt_mv,uint8_t bat_remaining)
 {
-    return; // for test ,don't send
-    flight_mode = mode;
-  
     static uint8_t delay_cnt;
+    uint8_t freq_cnt = 1;
     delay_cnt += 1;
-    if(delay_cnt < 50){
+    uint8_t freq = (uint8_t)parm_status_freq;
+    if(freq == 0){
+        return;
+    }else{
+        freq_cnt = 100/freq;
+    }
+
+    if(delay_cnt < freq_cnt){
         return;
     }else{
         delay_cnt = 0;
     }
+    
 
     uint8_t crc_sum = 0;
 
@@ -198,8 +227,16 @@ void AC_ZR_Serial_API::get_vehicle_status(uint8_t mode,uint8_t in_air,uint32_t h
 void AC_ZR_Serial_API::get_vehicle_flight_data(Vector3l pos,Vector3l vel,Vector3f euler)
 {
     static uint8_t delay_cnt;
+    uint8_t freq_cnt = 1;
     delay_cnt += 1;
-    if(delay_cnt < 10){
+    uint8_t freq = (uint8_t)parm_data_freq;
+    if(freq == 0){
+        return;
+    }else{
+        freq_cnt = 100/freq;
+    }
+
+    if(delay_cnt < freq_cnt){
         return;
     }else{
         delay_cnt = 0;

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -79,7 +79,13 @@ public:
 
     bool data_trans_init;
 
+    void get_param_value(uint8_t &debug,uint8_t &data_freq,uint8_t &status_freq);
 
+    static const struct AP_Param::GroupInfo var_info[];
+protected:
+    AP_Int8         parm_msg_debug;
+    AP_Int8         parm_data_freq;
+    AP_Int8         parm_status_freq;
 private:
 
     bool check_uart(AP_HAL::UARTDriver *_port);
@@ -197,7 +203,6 @@ private:
     uint8_t *pktbuf;
     uint16_t pktoffset;
     uint16_t bufsize;
-    uint8_t flight_mode;
 
 };