diff --git a/applications/VK_Client.c b/applications/VK_Client.c index 161793f..00d78b3 100644 --- a/applications/VK_Client.c +++ b/applications/VK_Client.c @@ -7,8 +7,9 @@ #include "rtdef.h" #include "rtthread.h" #include "rttypes.h" +#include "VKFly/mavlink.h" +#include "mavlink_helpers.h" -#define LINK_4G char cmd_cmd[100] = {0}; char cmd_str[100] = {0}; @@ -82,8 +83,10 @@ int16_t EC800K_Reset(void) { int16_t client_tcp_connect(Cclient_t *client) { int16_t rtn = 0; - if (lte_dev.config_sta == false) return rtn; - if (client->connect_sta == true) return rtn; + if (lte_dev.config_sta == false) + return rtn; + if (client->connect_sta == true) + return rtn; if (lte_dev.reset_flag == DTU_CF_NEED_RESET) { /*查询PDP场景*/ @@ -94,7 +97,7 @@ int16_t client_tcp_connect(Cclient_t *client) { LTE_CMD_WAIT_LLONG)) { BSP_LTE_SendCmd(AT_QIDEACT, NETCLOSE_SUCCESS, NETCLOSE_FAILURE, LTE_CMD_WAIT_LLONG); - //client->error_cnt++; + // client->error_cnt++; return -1; } } @@ -134,16 +137,34 @@ int16_t client_tcp_connect(Cclient_t *client) { client->error_cnt = 0; lte_dev.reset_flag = DTU_CF_NO_NEED_RESET; - //rt_kprintf("%s\r", lte_dev.at_buf); + // rt_kprintf("%s\r", lte_dev.at_buf); #ifdef TRANSPARENT_MODE // Set_Tansparent_Mode(client); #endif -#ifdef LINK_4G +#ifdef EDU_TRAINING + mavlink_message_t msg = {0}; + uint8_t mav_buf[MAVLINK_MAX_PACKET_LEN] = {0}; + uint16_t mav_len = 0; + mavlink_dtu_status_t _status_t = {0}; + _status_t.csq = lte_dev.csq; + rt_memcpy(_status_t.imei,lte_dev.imei,sizeof(_status_t.imei)); + mavlink_msg_dtu_status_encode(1, MAV_COMP_ID_TELEMETRY_RADIO, &msg, + &_status_t); + mav_len = mavlink_msg_to_send_buffer(mav_buf, &msg); + + for (int i = 0; i < 3; i++) { + rt_device_write(lte_dev.com_dev, 0, mav_buf, mav_len); + } + rt_device_t fmu_com = fmu_com_get(); + if (fmu_com != RT_NULL) { + rt_device_write(fmu_com, 0, mav_buf, mav_len); + } +#else char buf[128] = {0}; - rt_sprintf(buf, "{\"sn\":\"%s\",\"client_id\":\"%s\",\"csq\":\"%d\"}\r\n", lte_dev.iccid, - lte_dev.imei,lte_dev.csq); + rt_sprintf(buf, "{\"sn\":\"%s\",\"client_id\":\"%s\",\"csq\":\"%d\"}\r\n", + lte_dev.iccid, lte_dev.imei, lte_dev.csq); rt_device_write(lte_dev.com_dev, 0, buf, sizeof(buf));