diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 165c5626f6..03f85ddd39 100644 --- a/ArduCopter/Parameters.cpp +++ b/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 }; diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8abe8c2f00..a705ba6d23 100644 --- a/ArduCopter/Parameters.h +++ b/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 diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp b/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp index a7a28609f7..150d643be9 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp +++ b/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; diff --git a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h b/libraries/AC_ZR_APP/AC_ZR_Serial_API.h index a3b419c5b6..3d21be646e 100644 --- a/libraries/AC_ZR_APP/AC_ZR_Serial_API.h +++ b/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; }; \ No newline at end of file