From 2f31c5fd10b51ba366e001fabbaf0fae8344f3ba Mon Sep 17 00:00:00 2001 From: wsk <2098271690@qq.com> Date: Sat, 4 May 2024 20:26:18 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/新建 DOC 文档.doc | Bin 0 -> 9216 bytes src/main.cpp | 243 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 243 insertions(+) create mode 100644 doc/新建 DOC 文档.doc create mode 100644 src/main.cpp diff --git a/doc/新建 DOC 文档.doc b/doc/新建 DOC 文档.doc new file mode 100644 index 0000000000000000000000000000000000000000..1501f025ed83acccdf1f8fd0096a3bcdda050cc4 GIT binary patch literal 9216 zcmeI1U2GKB701ua?Ap7wv5jLK$3VT11snqIm_TY0i0zmZ2RF9CByIyqOfU(EU`qy? zeu_=g1|rnNRnj&Oh!TlPNR1*_ZPbsn4OQB-Ra+%UMg348%tNI<^kIvts)?FRfB)GX zYPQ(fG>8b<$Nue|xpVK_bI-Z=+?hLXygqm0`ENIzvjVxzJbNcdTFIn*hhq^+wVBI- z&ff`wAeU*z;jCOm7I+(7EAu{#k;XwjFNFjop$y8Q0_H#!$ma}!FujVpJF9Ae1-94- ze+MjOci1rLsC^o*7B6*-nCO;@2h+)ky+1tSjQq**DAaykuH**WL908at2XVmGIO1% zeWlYD&R?7yuYB7|>krx^wyT(`|5K)t_7wv)cZzLaH)+-cLk6(^s14Z$(w%nD_MwOT zP^>f%h2Lxc7jpBPH0crPF2Bhq#m;|KRSeMlp9?i`4b;LssDo>v9^~c*m=6m;w*L@m zBcxyvEQaeL4NG7tEQ2Pv0hYs!umV;>Gkh3)Xn|F56Rd_ca5H=aT462R0=L3CSP!>B z8?-|Q+zy?v0q%g0!ba$VJ7E)a!^hw*=z(77gUzr7`e7^F4eH|n>Br#{a4&3wA-E6j zhX-H~cEE$M6I4%JNvXwyHH?B|cvSv(Ahf{X-vc+aM3}*kYdiNIW|C+n*pc?UwAX^e z7Fg$|dDgedvFli#AJRkZx$}O=!96x#VrM>IFIIyi> zhP@!ImKt>IeCWw2)3t-oH9V$KR)rqsrEO!{=De0LcWw1(%5h_8RAWzBBQ;&;si+#M zY9=3*t*fnOC`(^+{h)sP(qvlhaeBBBGg*dMqKsH?lo7L~VTryd!?9&yiOo?)Y_b}x zmeQk_!LwNdh}*gl=P)|fI_qi@Npgk{*W7T?-B>q9mg|;y@mSpTVuwFtDT-wxn+Ev` zTWwDmH1>13hv4xjse<}GhSo3@R>sDhCon{bd5***&+O?YH7;>Nx0@XeP2Z9$T^{#b zH^%>A*D_nyRW)f!OB(Z~UwmNX>7Re?%yA}M?U>_QJnpzoX^9)phqYYk{BK5SKMTt~ zhWZGhmg$-vBW$|mB{YJ>g5@^R`6)ZC#|;U+Yga(+AUN)!C#rLyC&`kqC$FH}Tc3Nq zHPS8W$(iFCL~X0X7D~OO>&D&Klr40k(ZR5lUHnEiBw%0(`Xyzil8i14px>}P%;e6c z`^j5FU3^FHa9)hwHQTvugLYcMOcFRwitv`w(z847EA>`^v=~X@1t)QY_uYA zMNnB)^6<+)u2tNM#*z*%NX)!wsD*h>SSqZ_`J&vgU>UfV@VkhxyMi!#3t_X3ko9>x zW#6-3xW97erRq}Gr52`^rfy#B_^$8yaX;yo`*Zv%f382zzt(pGHz*B~K}Ap*Gz9a5 z#$bKW9&8Wp3+@jlm{(U;nh3m~MfdvDf>gpU^DF#HzuK?$>-+@z-5P8Q&INyY-&!R~ zPsG1V2z4`8ws+IDc0)F(!pCi9r|>OH%Au)Pwp@#KZW+D!j!q^TAG!S8n7O!&&hn)y zb*#m!$}Fozlvjx?gES0$u2N80q2qol&+1_yK>eb})!`4y0 z6LqBSA^5U$$VpjBfryyg=BevJI~NM%Z{Pf>J$WR_oooAE$z99MViZ-pue9#|?*3PE zalN2!EKzsiIZT&imgyukY`(x$$5H*0;Xy7mm_=!H^-^gED}tWjiWS|ng2D&#X;km4 zod5IPx)K$hCSAc&B2O193(-GK;g=ZhyQz4TR8Ni>bkS4k2Q7D_-|s(YC1%^7wZNK*v`RBa1X0QonTGtcNHkTgR^ZtK(rWs+jzj}IbTC8#W z3(cwV*E$xBCrYm$I}u+@nkHQ`&Mx?PzgIs#;jQX(t!1p$3C@IXH2E>Z3juq+yd<;H zR4BUn#FxHt{>|Rq)!+I`!fssloj>ZGb&v~s$JO;QKr+wqqdS}c?c=-vypY;D3h90K z`=GXd2HKx{15m;K3~^q<_0>V!h5Gh$C!1Lhu_ZMc<{V&CYb4x8r06B3iLFJosdn`0 zf5}1xnfGw64KhjG3cq~3H+@>^x!R2+ch~8Ew{tn0Z-4rd#a)fsR$5$*%UkOGw66`< zzk>E|{sC;=*uOzzEQXD=nW5)oecxG4syF_vlWeDPH_l+wb?x^zl17&=*RjTzq%hU} zrSAY=V)`w`H<^d{+r$3;F3zs$Y3S)1om@}lHU9OjZsbB8d-=00zJiq#x zuQUHvy!qk=^5O+~+v#jQM04^8Wsr>KrgV$u=1Goa<8jbj{1Ir*Xin-I+HXMp`yF6T zn*ix5-(XhDf_(KaC<7T;Ut$vZwyR11A72#OG&-N`yD0D7Qt2a!(q}nVUnEU{il(`T zWQO>WzK?+Nb*wSejVF8Y=P14zA~NmdTj=eidnh?%_u#qErI+lhe6WhIf@}C)$(O%o rz795X-ptp*wH&YJx@ +#include +#include +#include +#include +using namespace std; +#include "../../inc/ViewLink.h" +#include "cmdline.h" + +bool g_bConnected = false; +int VLK_ConnStatusCallback(int iConnStatus, const char* szMessage, int iMsgLen, void* pUserParam) +{ + if (VLK_CONN_STATUS_TCP_CONNECTED == iConnStatus) + { + cout << "TCP Gimbal connected !!!" << endl; + g_bConnected = true; + } + else if (VLK_CONN_STATUS_TCP_DISCONNECTED == iConnStatus) + { + cout << "TCP Gimbal disconnected !!!" << endl; + g_bConnected = false; + } + else if (VLK_CONN_STATUS_SERIAL_PORT_CONNECTED == iConnStatus) + { + cout << "serial port connected !!!" << endl; + g_bConnected = true; + } + else if (VLK_CONN_STATUS_SERIAL_PORT_DISCONNECTED == iConnStatus) + { + cout << "serial port disconnected !!!" << endl; + g_bConnected = false; + } + else + { + cout << "unknown connection stauts: " << iConnStatus << endl; + g_bConnected = false; + } + + return 0; +} + +int VLK_DevStatusCallback(int iType, const char* szBuffer, int iBufLen, void* pUserParam) +{ + if (VLK_DEV_STATUS_TYPE_MODEL == iType) + { + VLK_DEV_MODEL* pModel = (VLK_DEV_MODEL*)szBuffer; + cout << "model code: " << pModel->cModelCode << ", model name: " << pModel->szModelName << endl; + } + else if (VLK_DEV_STATUS_TYPE_CONFIG == iType) + { + VLK_DEV_CONFIG* pDevConfig = (VLK_DEV_CONFIG*)szBuffer; + cout << "VersionNO: " << pDevConfig->cVersionNO << ", DeviceID: " << pDevConfig->cDeviceID << ", SerialNO: " << pDevConfig->cSerialNO << endl; + } + else if (VLK_DEV_STATUS_TYPE_TELEMETRY == iType) + { + /* + * once device is connected, telemetry information will keep updating, + * in order to avoid disturbing user input, comment out printing telemetry information + */ + // VLK_DEV_TELEMETRY* pTelemetry = (VLK_DEV_TELEMETRY*)szBuffer; + // cout << "Yaw: " << pTelemetry->dYaw << ", Pitch: " << pTelemetry->dPitch << ", sensor type: " << pTelemetry->emSensorType << ", Zoom mag times: " << pTelemetry->sZoomMagTimes << endl; + } + else + { + cout << "error: unknown status type: " << iType << endl; + } + + + return 0; +} + +int main(int argc, char *argv[]) +{ + // parse cmd line + cmdline::parser a; + a.add("type", 't', "connection type", true, "tcp", cmdline::oneof("serial", "tcp")); + a.add("ip", 'i', "gimbal tcp ip", false, "192.168.2.119"); + a.add("port", 'p', "gimbal tcp port", false, 2000); + a.add("serial", 's', "serial port name", false, "/dev/ttyS0"); + a.add("baudrate", 'b', "serial port baudrate", false, 115200); + a.parse_check(argc, argv); + + // print sdk version + cout << "ViewLink SDK version: " << GetSDKVersion() << endl; + + // initialize sdk + int iRet = VLK_Init(); + if (VLK_ERROR_NO_ERROR != iRet) + { + cout << "VLK_Init failed, error: " << iRet << endl; + return -1; + } + + // register device status callback + VLK_RegisterDevStatusCB(VLK_DevStatusCallback, NULL); + + // connect device + if (0 == a.get("type").compare("tcp")) + { + VLK_CONN_PARAM param; + memset(¶m, 0, sizeof(param)); + param.emType = VLK_CONN_TYPE_TCP; + strncpy(param.ConnParam.IPAddr.szIPV4, a.get("ip").c_str(), sizeof(param.ConnParam.IPAddr.szIPV4) - 1); + param.ConnParam.IPAddr.iPort = a.get("port"); + + cout << "connecting gimbal ip: " << a.get("ip") << ", port: " << a.get("port") << "..." << endl; + iRet = VLK_Connect(¶m, VLK_ConnStatusCallback, NULL); + if (VLK_ERROR_NO_ERROR != iRet) + { + cout << "VLK_Connect failed, error: " << iRet << endl; + goto quit; + } + } + else if (0 == a.get("type").compare("serial")) + { + VLK_CONN_PARAM param; + memset(¶m, 0, sizeof(param)); + param.emType = VLK_CONN_TYPE_SERIAL_PORT; + strncpy(param.ConnParam.SerialPort.szSerialPortName, a.get("serial").c_str(), sizeof(param.ConnParam.SerialPort.szSerialPortName) - 1); + param.ConnParam.SerialPort.iBaudRate = a.get("baudrate"); + + cout << "connecting gimbal serial: " << a.get("serial") << ", baudrate: " << a.get("baudrate") << "..." << endl; + iRet = VLK_Connect(¶m, VLK_ConnStatusCallback, NULL); + if (VLK_ERROR_NO_ERROR != iRet) + { + cout << "VLK_Connect failed, error: " << iRet << endl; + goto quit; + } + } + else + { + cout << "unknown conntion type !!!" << endl; + goto quit; + } + + + cout << "wait device connected..." << endl; + while (1) + { + if (g_bConnected) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + + while (1) + { + cout << "press \'w\' move up \n"; + cout << "press \'s\' move down \n"; + cout << "press \'a\' move left \n"; + cout << "press \'d\' move right \n"; + cout << "press \'h\' move to home posiion \n"; + cout << "press \'1\' zoom in, \'2\' zoom out\n"; + cout << "press \'3\' begin track, \'4\' stop track\n"; + cout << "press \'5\' visible with ir pseudo , \'6\' visible with ir white , \'7\' visible with ir black\n"; + cout << "press \'i\' open ir, \'v\' open visible\n"; + cout << "press \'c\' exit"<< endl; + char input; + cin >> input; + if (input == 'w' || input == 'W') + { + VLK_Move(0, 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_Stop(); + } + else if (input == 's' || input == 'S') + { + VLK_Move(0, -1000); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_Stop(); + } + else if (input == 'a' || input == 'A') + { + VLK_Move(-1000, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_Stop(); + } + else if (input == 'd' || input == 'D') + { + VLK_Move(1000, 0); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_Stop(); + } + else if (input == 'h' || input == 'H') + { + VLK_Home(); + } + else if (input == '1') + { + VLK_ZoomIn(1); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_StopZoom(); + } + else if (input == '2') + { + VLK_ZoomOut(4); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + VLK_StopZoom(); + } + else if (input == '3') + { + VLK_TRACK_MODE_PARAM param; + memset(¶m, 0, sizeof(param)); + param.emTrackSensor = VLK_SENSOR_VISIBLE1; + param.emTrackTempSize = VLK_TRACK_TEMPLATE_SIZE_AUTO; + VLK_TrackTargetPositionEx(¶m, 100, 100, 1280, 720); + } + else if (input == '4') + { + VLK_DisableTrackMode(); + } + else if (input == '5'){ + VLK_SetImageColor(VLK_IMAGE_TYPE_VISIBLE1,1,VLK_IR_COLOR_PSEUDOHOT); + } + else if (input == '6'){ + VLK_SetImageColor(VLK_IMAGE_TYPE_VISIBLE1,1,VLK_IR_COLOR_WHITEHOT); + } + else if (input == '7'){ + VLK_SetImageColor(VLK_IMAGE_TYPE_VISIBLE1,1,VLK_IR_COLOR_BLACKHOT); + } + else if (input == 'i'){ + VLK_SetImageColor(VLK_IMAGE_TYPE_IR1,0,VLK_IR_COLOR_WHITEHOT); + } + else if (input == 'v'){ + VLK_SetImageColor(VLK_IMAGE_TYPE_VISIBLE1,0,VLK_IR_COLOR_WHITEHOT); + } + else if (input == 'c' || input == 'C') + { + break; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + +quit: + // uninitial sdk + VLK_UnInit(); + + system("PAUSE"); + return 0; +}