positioning.cxx
2.0 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
#include <string>
#include <thread>
#include <sys/time.h>
#include "uav_core.h"
#include "uav_logger.h"
#include "uav_platform.h"
#include "uav_data_type.h"
#include "uav_time_sync.h"
#include "uav_positioning.h"
#include "uav_sdk_app_info.h"
uint64_t get_local_time_us(void)
{
struct timeval tv;
gettimeofday(&tv, NULL);
return tv.tv_sec * 1000000 + tv.tv_usec;
}
void positioningThreadFunc(void)
{
//wait register success
if(false == wait_register_ready(100)){
LOG_ERROR("waitHandShakeRegister failed");
return;
}
if(UAV_TimeSync_Init() != UAV_ERROR_SYSTEM_MODULE_CODE_SUCCESS){
LOG_ERROR("UAV_TimeSync_Init failed");
return;
}
if(UAV_Positioning_Init() != UAV_ERROR_SYSTEM_MODULE_CODE_SUCCESS){
LOG_ERROR("UAV_Positioning_Init failed");
return;
}
T_UAVPositioningEventInfo eventInfo[2];
T_UAVPositioningPositionInfo positionInfo[2];
while(1){
sleep(1);
///< 模拟填写两个事件信息
eventInfo[0].eventIndex = 0;
UAV_TimeSync_TransferToAircraftTime(get_local_time_us(), &eventInfo[0].timestamp);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
eventInfo[1].eventIndex = 1;
UAV_TimeSync_TransferToAircraftTime(get_local_time_us(), &eventInfo[1].timestamp);
///< 通过事件信息获取定位信息
if(UAV_ERROR_SYSTEM_MODULE_CODE_SUCCESS != UAV_Positioning_GetPositioning_Sync(2, eventInfo, positionInfo)) {
LOG_ERROR("UAV_Positioning_GetPositioning_Sync failed");
return;
}
}
}
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("positioning");
UAV_Uart_Init("/dev/pts/20",115200);
std::thread positioningThread(positioningThreadFunc);
positioningThread.detach();
return UAV_Core_ApplicationStart();
}