gimbal_manager.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
#include <string>
#include <thread>
#include "uav_core.h"
#include "uav_logger.h"
#include "uav_platform.h"
#include "uav_data_type.h"
#include "uav_gimbal_manager.h"
#include "uav_sdk_app_info.h"
static void hgaimbal_manager_routine(void)
{
bool rlt = false;
T_UAVGimbalManagerRotation rotation;
UAV_GimbalManager_Init();
//wait register success
if(false == wait_register_ready(100)){
LOG_ERROR("waitHandShakeRegister failed");
return;
}
rlt = UAV_GimbalManager_Reset(UAV_GIMBAL_RESET_MODE_PITCH_AND_YAW);
LOG_INFO("gimbal reset: {}",rlt);
std::this_thread::sleep_for(std::chrono::seconds(3));
rotation.rotationMode = UAV_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE;
rotation.pitch = 2.1f;
rotation.roll = 0.7f;
rotation.yaw = 4.6f;
rlt = UAV_GimbalManager_Rotate(rotation);
LOG_INFO("relative rlt: {}, roll: {}, yaw: {}, pitch: {}", rlt, rotation.roll, rotation.yaw, rotation.pitch);
std::this_thread::sleep_for(std::chrono::seconds(3));
rotation.rotationMode = UAV_GIMBAL_ROTATION_MODE_ABSOLUTE_ANGLE;
rotation.pitch = 20.1f;
rotation.roll = 7.7f;
rotation.yaw = -46.0f;
UAV_GimbalManager_Rotate(rotation);
LOG_INFO("absolute rlt: {}, roll: {}, yaw: {}, pitch: {}", rlt, rotation.roll, rotation.yaw, rotation.pitch);
std::this_thread::sleep_for(std::chrono::seconds(3));
E_UAVGimbalMode mode;
mode = UAV_GIMBAL_MODE_FREE;
rlt = UAV_GimbalManager_SetMode(mode);
LOG_INFO("setmode rlt: {}, mode: {}", rlt,(int32_t)mode);
std::this_thread::sleep_for(std::chrono::seconds(3));
rlt = UAV_GimbalManager_SetPitchRangeExtensionEnabled( true);
LOG_INFO("SetPitchRangeExtensionEnabled rlt: {}", rlt);
std::this_thread::sleep_for(std::chrono::seconds(3));
E_UavGimbalAxis axis;
uint8_t maxSpeedPercentage;
axis = UAV_GIMBAL_AXIS_PITCH;
maxSpeedPercentage=44;
rlt = UAV_GimbalManager_SetControllerMaxSpeedPercentage( axis,maxSpeedPercentage);
LOG_INFO("SetControllerMaxSpeedPercentage rlt: {} ", rlt);
std::this_thread::sleep_for(std::chrono::seconds(3));
uint8_t smoothingFactor;
axis = UAV_GIMBAL_AXIS_PITCH;
smoothingFactor=4;
rlt = UAV_GimbalManager_SetControllerSmoothFactor( axis,smoothingFactor);
LOG_INFO("SetControllerSmoothFactor rlt: {} ", rlt);
std::this_thread::sleep_for(std::chrono::seconds(3));
rlt = UAV_GimbalManager_RestoreFactorySettings();
LOG_INFO("RestoreFactorySettings rlt: {}", rlt);
std::this_thread::sleep_for(std::chrono::seconds(3));
}
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("Gimbal");
// UAV_Uart_Init("/dev/pts/20",115200);
UAV_Network_Init(LOCALHOST_ETHERNET);
std::thread gaimbal_manager(hgaimbal_manager_routine);
gaimbal_manager.detach();
return UAV_Core_ApplicationStart();
}