flight_control.cxx
2.1 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
#include <string>
#include <thread>
#include "uav_core.h"
#include "uav_logger.h"
#include "uav_platform.h"
#include "uav_sdk_app_info.h"
#include "uav_flight_control.h"
static void flightControlThreadFunc(void)
{
T_UAVFlightControllerRidInfo ridInfo;
UAV_FlightControl_Init(ridInfo);
//wait register success
if(false == wait_register_ready(100)){
LOG_ERROR("waitHandShakeRegister failed");
return;
}
E_UAVFlithtControl_Mode mode;
UAV_FlightControl_SetControlMode(UAV_FLIGHTCONTROL_MODE_POS_CTL);
UAV_FlightControl_GetControlMode(mode);
LOG_INFO("flight control mode: {}", (int32_t)mode);
E_UAVFlightControl_RCLost_Action action;
UAV_FlightControl_SetRCLostAction(UAV_FLIGHTCONTROL_RC_LOST_ACTION_GOHOME);
UAV_FlightControl_GetRCLostAction(action);
LOG_INFO("flight control rc lost action: {}", (int32_t)action);
bool status;
UAV_FlightControl_SetRtkPositionEnableStatus(true);
UAV_FlightControl_GetRtkPositionEnableStatus(status);
LOG_INFO("flight control rtk position enable status: {}", status);
T_UAVFlightControlSpeed speed;
speed.x = 123;
speed.y = 234;
speed.z = 345;
speed.yaw = 456;
speed.heading_mode = UAV_HEADING_MODE_ALONG;
speed.obstacle_mode = UAV_OBSTACLE_MODE_LOITER;
UAV_FlightControl_SpeedControl(speed);
T_UAVFlightControlPos position;
position.latitude = 123;
position.longitude = 234;
position.altitude = 345;
position.obstacle_mode = UAV_OBSTACLE_MODE_OBS;
UAV_FlightControl_POSControl(position);
while(1)
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("flight_controlV1");
UAV_Uart_Init("/dev/pts/20",115200);
std::thread flightControlThread(flightControlThreadFunc);
flightControlThread.detach();
return UAV_Core_ApplicationStart();
}