data_channel.cxx
3.9 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
#include <string>
#include <thread>
#include "uav_core.h"
#include "uav_logger.h"
#include "uav_platform.h"
#include "uav_data_type.h"
#include "uav_sdk_app_info.h"
#include "uav_low_speed_data_channel.h"
#include "uav_high_speed_data_channel.h"
#include "proto/tracerMessage.pb.h"
//channel callback
static void myWorkModeControlCallback(int32_t srcChannelID, int data_type, const uint8_t *data, uint32_t len)
{
std::string str;
autel::protocol::TracerWorkModeControlMessage reqMsg;
LOG_INFO("myChannelRecvDataCallback from srcChannelID:{}", int(srcChannelID));
if(!reqMsg.ParseFromArray(data, len)){
LOG_ERROR("parseMsg TracerWorkModeControlMessage failed");
return;
}
LOG_INFO("seq:{}, workMode:{}, dxNum:{}, dxFreq:{}", reqMsg.seq(), reqMsg.workmode(), reqMsg.dxnum(), reqMsg.dxfreq());
//response
autel::protocol::TracerWorkModeControlRspMessage rspMsg;
rspMsg.set_seq(reqMsg.seq());
rspMsg.set_rlt(0);
rspMsg.SerializeToString(&str);
UAV_LowSpeedDataChannel_SendData(srcChannelID, PAYLOAD_TRACER_WORKMODE_CONTROL, (const uint8_t *)str.c_str(), str.size()); //ack
}
static void tracerThreadFunc(void)
{
std::string str;
//创建低速数据通道回调
UAV_LowSpeedDataChannel_Init();
UAV_LowSpeedDataChannel_RegRecvDataCallback(myWorkModeControlCallback);
//wait register success
if(false == wait_register_ready(100)){
LOG_ERROR("waitHandShakeRegister failed");
return;
}
while(1){
sleep(1);
//1-发布检测结果
autel::protocol::tracerMeasureInfoMessage tracerMeasureInfoMsg;
tracerMeasureInfoMsg.set_workstatus(1);
tracerMeasureInfoMsg.set_tarnum(3);
for(int i = 0 ; i < 3 ; i ++){
autel::protocol::singleTargetInfo *info = tracerMeasureInfoMsg.add_tarinfo();
info->set_tnum(1);
info->set_tfreq(2);
info->set_tamp(3);
info->set_tmfw(4);
info->set_tmfy(5);
info->set_tffw(6);
info->set_tffy(7);
}
tracerMeasureInfoMsg.SerializeToString(&str);
UAV_LowSpeedDataChannel_SendData(UAV_CHANNEL_ADDRESS_MASTER_RC_APP, PAYLOAD_TRACER_MEASURE_INFO, (const uint8_t *)str.c_str(), str.size());
sleep(1);
//2-发布错频信息
autel::protocol::TracerStaggerFreqMessage TracerStaggerFreqMsg;
TracerStaggerFreqMsg.set_seq(101);
TracerStaggerFreqMsg.set_freqband(102);
TracerStaggerFreqMsg.set_timstamp(103);
TracerStaggerFreqMsg.set_reserv(104);
TracerStaggerFreqMsg.SerializeToString(&str);
UAV_LowSpeedDataChannel_SendData(UAV_CHANNEL_ADDRESS_MASTER_RC_APP, PAYLOAD_TRACER_STAGGER_FREQ, (const uint8_t *)str.c_str(), str.size());
sleep(1);
//3-发布系统状态信息
autel::protocol::TracerSystemInfoMessage TracerSystemInfoMsg;
TracerSystemInfoMsg.set_workstatus(12);
TracerSystemInfoMsg.set_faultstatus(13);
TracerSystemInfoMsg.set_netstatus(14);
TracerSystemInfoMsg.set_ssid("unknow");
TracerSystemInfoMsg.set_ipaddress("192.168.1.111");
TracerSystemInfoMsg.SerializeToString(&str);
UavHighSpeedDataChannel_SendDataStreamData(5, (const uint8_t *)str.c_str(), str.size());
UAV_LowSpeedDataChannel_SendData(UAV_CHANNEL_ADDRESS_MASTER_RC_APP, PAYLOAD_TRACER_SYSTEM_INFO, (const uint8_t *)str.c_str(), str.size());
}
}
int main(int argc, char *argv[])
{
T_AUserInfo usrInfo;
logger_init(std::string(argv[1]));
if(false == uav_sdk_app_info_init(&usrInfo)) {
LOG_ERROR("uav_sdk_app_info_init failed");
return -1;
}
UAV_Core_Init(&usrInfo);
UAV_Core_SetAlias("dataChannel");
// UAV_Uart_Init("/dev/pts/20",115200);
UAV_Uart_Init(argv[2], 115200); ///< 便于调试串口进行验证
std::thread tracerThread(tracerThreadFunc);
tracerThread.detach();
return UAV_Core_ApplicationStart();
}