subscribe.cxx
3.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
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
#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_fc_subscription.h"
static void myGimbalAngleMsgCB(void *data)
{
LOG_INFO("myGimbalAngleMsgCB");
T_UAVSubscriptionGimbalAngle *gimbalAngle = (T_UAVSubscriptionGimbalAngle *)data;
// pthread_mutex_lock(&gimbalMutex);
// lampAngle = gimbalAngle->x;//获取云台角度
// pthread_mutex_unlock(&gimbalMutex);
LOG_INFO("云台角度: %d | %d",gimbalAngle->x,lampAngle);
}
static void myPositionFusedMsgCB(void *data)
{
T_UAVSubscriptionPositionFused *posSt = (T_UAVSubscriptionPositionFused*)data;
LOG_INFO("longitude:{}, latitude:{}, altitude:{}, visibleSatelliteNumber:{}", posSt->longitude, posSt->latitude, posSt->altitude, posSt->visibleSatelliteNumber);
}
static void myPositionGpsMsgCB(void *data)
{
T_UAVSubscriptionGpsPosition *posSt = (T_UAVSubscriptionGpsPosition*)data;
LOG_INFO("longitude:{}, latitude:{}, altitude:{}", posSt->x, posSt->y, posSt->z);
}
static void myEulerAngularVelocityMsgCB(void *data)
{
T_UAVSubscriptionAngularRateRaw *eulerSt = (T_UAVSubscriptionAngularRateRaw*)data;
LOG_INFO("wroll:{}, wpitch:{}, wyaw:{}", eulerSt->x, eulerSt->y, eulerSt->z);
}
static void myVelocityComponentMsgCB(void *data)
{
T_UAVSubscriptionGpsVelocity *velCompSt = (T_UAVSubscriptionGpsVelocity*)data;
LOG_INFO("vX:{}, vY:{}, vZ:{}", velCompSt->x, velCompSt->y, velCompSt->z);
}
static void myEulerAngularMsgCB(void *data)
{
T_UAVSubscriptionEulerAngular *eulerSt = (T_UAVSubscriptionEulerAngular*)data;
LOG_INFO("roll:{}, pitch:{}, yaw:{}", eulerSt->roll, eulerSt->pitch, eulerSt->yaw);
}
static void sub_routine(void)
{
//wait register success
if(false == wait_register_ready(100)){
LOG_ERROR("waitHandShakeRegister failed");
return;
}
//订阅
UAV_SubscribeTopic(UAV_SUBSCRIPTION_TOPIC_GIMBAL_ANGLE, 0, myGimbalAngleMsgCB);
//订阅飞机位置信息
UAV_SubscribeTopic(UAV_SUBSCRIPTION_TOPIC_POSITION_FUSED, 0, myPositionFusedMsgCB);
// 订阅飞机欧拉角速度信息
UAV_SubscribeTopic(UAV_SUBSCRIPTION_TOPIC_ANGULAR_RATE_RAW, 0, myEulerAngularVelocityMsgCB);
// 订阅飞机速度分量信息
UAV_SubscribeTopic(UAV_SUBSCRIPTION_TOPIC_GPS_VELOCITY, 0, myVelocityComponentMsgCB);
// 订阅飞机姿态欧拉角信息
UAV_SubscribeTopic(UAV_SUBSCRIPTION_TOPIC_EULER_ANGULAR_INFO, 0, myEulerAngularMsgCB);
while (true)
{
///< do business
std::this_thread::sleep_for(std::chrono::seconds(1));
}
}
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("subscribe");
UAV_Uart_Init("/dev/ttyS1",115200);
std::thread subscribe(sub_routine);
subscribe.detach();
return UAV_Core_ApplicationStart();
}