00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef _DJI2MAV_MAVHANDLER_H_
00010 #define _DJI2MAV_MAVHANDLER_H_
00011
00012
00013 #ifndef DEFAULT_SENDER_LIST_SIZE
00014 #define DEFAULT_SENDER_LIST_SIZE 16
00015 #endif
00016
00017 #ifndef DEFAULT_SEND_BUF_SIZE
00018 #define DEFAULT_SEND_BUF_SIZE 512
00019 #endif
00020
00021 #ifndef DEFAULT_RECV_BUF_SIZE
00022 #define DEFAULT_RECV_BUF_SIZE 4096
00023 #endif
00024
00025 #include <mavlink/common/mavlink.h>
00026 #include <new>
00027 #include <string>
00028
00029 #include "msgManager.h"
00030 #include "log.h"
00031
00032 namespace dji2mav {
00033
00034 class MavHandler {
00035 public:
00041 MavHandler(uint8_t mySysid, uint16_t maxMngNum) {
00042 DJI2MAV_DEBUG("Going to construct Handler with maxMngNum "
00043 "%u...", maxMngNum);
00044
00045 m_mySysid = mySysid;
00046 m_maxListSize = maxMngNum;
00047
00048 try {
00049 m_mngList = new MsgManager*[m_maxListSize];
00050 memset(m_mngList, 0, m_maxListSize * sizeof(MsgManager*));
00051 m_sysidRecord = new uint8_t[m_maxListSize];
00052 memset(m_sysidRecord, 0, m_maxListSize * sizeof(uint8_t));
00053 } catch(std::bad_alloc &m) {
00054 DJI2MAV_FATAL( "Fail to allocate memory for mngList! "
00055 "Exception: %s!", m.what() );
00056 exit(EXIT_FAILURE);
00057 }
00058
00059 DJI2MAV_DEBUG("...finish constructing Handler.");
00060 }
00061
00062
00063 ~MavHandler() {
00064 DJI2MAV_DEBUG("Going to destruct Handler...");
00065
00066 if(NULL != m_mngList) {
00067 for(uint16_t i = 0; i < m_maxListSize; ++i) {
00068 if(NULL != m_mngList[i]) {
00069 delete m_mngList[i];
00070 m_mngList[i] = NULL;
00071 }
00072 }
00073 delete []m_mngList;
00074 m_mngList = NULL;
00075 }
00076
00077 if(NULL != m_sysidRecord) {
00078 delete []m_sysidRecord;
00079 m_sysidRecord = NULL;
00080 }
00081
00082 DJI2MAV_DEBUG("...finish destructing Handler.");
00083 }
00084
00085
00091 inline bool isValidMngIdx(uint16_t mngIdx) {
00092 return (mngIdx < m_maxListSize && NULL != m_mngList[mngIdx]);
00093 }
00094
00095
00100 inline uint8_t getMySysid() {
00101 return m_mySysid;
00102 }
00103
00104
00109 inline uint16_t getMaxListSize() {
00110 return m_maxListSize;
00111 }
00112
00113
00119 uint8_t findMngSysid(uint16_t mngIdx) {
00120 if( isValidMngIdx(mngIdx) ) {
00121 return m_sysidRecord[mngIdx];
00122 } else {
00123 DJI2MAV_ERROR("Fail to get sysid because Manager index %u "
00124 "is not matched!", mngIdx);
00125 return 0;
00126 }
00127 }
00128
00129
00136 bool setMngSysid(uint16_t mngIdx, uint8_t sysid) {
00137 if( isValidMngIdx(mngIdx) ) {
00138 if(0 != sysid) {
00139 m_sysidRecord[mngIdx] = sysid;
00140 return true;
00141 } else {
00142 DJI2MAV_ERROR("Fail to set sysid %u! The sysid should "
00143 "in range 1~255!", sysid);
00144 return false;
00145 }
00146 } else {
00147 DJI2MAV_ERROR("Fail to set sysid because Manager index %u "
00148 "is not matched!", mngIdx);
00149 return false;
00150 }
00151 }
00152
00153
00159 int findMngIdx(uint8_t sysid) {
00160 if(0 == sysid) {
00161 DJI2MAV_ERROR("Fail to get mngIdx for invalid input %u!",
00162 sysid);
00163 return -2;
00164 }
00165 for(uint16_t i = 0; i < m_maxListSize; ++i) {
00166 if(sysid == m_sysidRecord[i])
00167 return i;
00168 }
00169 DJI2MAV_ERROR("Cannot find out the Manager index using sysid "
00170 "%u!", sysid);
00171 return -1;
00172 }
00173
00174
00180 void updateSysid(uint16_t mngIdx, uint8_t sysid) {
00181 if(m_sysidRecord[mngIdx] != sysid) {
00182 DJI2MAV_WARN("Switch sysid of Manager with index %u from "
00183 "%u to %u.", mngIdx, m_sysidRecord[mngIdx], sysid);
00184 m_sysidRecord[mngIdx] = sysid;
00185 }
00186 }
00187
00188
00195 int registerSender(uint16_t mngIdx, uint16_t bufSize) {
00196 if( !isValidMngIdx(mngIdx) ) {
00197 DJI2MAV_ERROR("Invalid mngIdx %u while maxListSize is %u!",
00198 mngIdx, m_maxListSize);
00199 return -2;
00200 } else {
00201 return m_mngList[mngIdx]->registerSender(bufSize);
00202 }
00203 }
00204
00205
00211 int registerSender(uint16_t mngIdx) {
00212 if( !isValidMngIdx(mngIdx) ) {
00213 DJI2MAV_ERROR("Invalid mngIdx %u while maxListSize is %u!",
00214 mngIdx, m_maxListSize);
00215 return -2;
00216 } else {
00217 return m_mngList[mngIdx]->registerSender();
00218 }
00219 }
00220
00221
00233 bool establish(uint16_t mngIdx, std::string targetIP,
00234 uint16_t targetPort, uint16_t srcPort,
00235 uint16_t senderListSize = DEFAULT_SENDER_LIST_SIZE,
00236 uint16_t sendBufSize = DEFAULT_SEND_BUF_SIZE,
00237 uint16_t recvBufSize = DEFAULT_RECV_BUF_SIZE) {
00238
00239 DJI2MAV_INFO("Going to establish connection for Manager "
00240 "with index %u. The size of sender list is set to "
00241 "%u, the size of send buf and recv buf are set to "
00242 "%u and %u...", mngIdx, senderListSize, sendBufSize,
00243 recvBufSize);
00244
00245 if(mngIdx >= m_maxListSize) {
00246 DJI2MAV_ERROR("The manager index %u exceeds the max "
00247 "manager list size %u!", mngIdx, m_maxListSize);
00248 return false;
00249 }
00250
00251 if(NULL != m_mngList[mngIdx]) {
00252 DJI2MAV_ERROR("The manager with index %u has been set "
00253 "before!", mngIdx);
00254 return false;
00255 }
00256
00257 try {
00258 m_mngList[mngIdx] = new MsgManager(senderListSize,
00259 sendBufSize, recvBufSize);
00260 } catch(std::bad_alloc &m) {
00261 DJI2MAV_FATAL( "Fail to allocate memory for msgManager "
00262 "with index %u! Exception: %s!", mngIdx, m.what() );
00263 exit(EXIT_FAILURE);
00264 }
00265
00266 bool ret = m_mngList[mngIdx]->establish(targetIP, targetPort,
00267 srcPort);
00268 if(ret) {
00269 DJI2MAV_INFO("...succeed in establishing connection "
00270 "for Manager %u.", mngIdx);
00271 } else {
00272 DJI2MAV_ERROR("...fail to establish connection for "
00273 "Manager with index %u!", mngIdx);
00274 }
00275
00276 return ret;
00277 }
00278
00279
00287 bool sendEncodedMsgToMng(uint16_t mngIdx, uint16_t senderIdx,
00288 const mavlink_message_t &msg) {
00289
00290 DJI2MAV_TRACE("Going to send encoded message to Manager "
00291 "with index %u...", mngIdx);
00292
00293 if( !isValidMngIdx(mngIdx) ) {
00294 DJI2MAV_ERROR("Fail to send encoded message because "
00295 "invalid mngIdx %u is met while mngListSize "
00296 "is %u!", mngIdx, m_maxListSize);
00297 return false;
00298 }
00299
00300 uint16_t len = mavlink_msg_to_send_buffer(
00301 m_mngList[mngIdx]->getSendBuf(senderIdx), &msg);
00302 int bytes_sent = m_mngList[mngIdx]->send(senderIdx, len);
00303
00304 if(bytes_sent < 0) {
00305 DJI2MAV_ERROR("Fail to send encoded message while using "
00306 "Manager with index %u!", mngIdx);
00307 return false;
00308 }
00309 if(bytes_sent != len) {
00310 DJI2MAV_ERROR("For Manager #%u, a %u-bytes message was "
00311 "generated but only %d-bytes were sent!", mngIdx,
00312 len, bytes_sent);
00313 return false;
00314 }
00315
00316 DJI2MAV_TRACE("...succeed in sending encoded message to "
00317 "Manager with index %u.", mngIdx);
00318 return true;
00319
00320 }
00321
00322
00330 bool sendEncodedMsgToSys(uint8_t sysid, uint16_t senderIdx,
00331 const mavlink_message_t &msg) {
00332
00333 uint16_t idx = findMngIdx(sysid);
00334 DJI2MAV_TRACE("Convert sysid %u to Manager index %u for "
00335 "sending encoded message.", sysid, idx);
00336
00337 if(0 != idx) {
00338 return sendEncodedMsgToMng(idx, senderIdx, msg);
00339 } else {
00340 DJI2MAV_ERROR("Fail to send encoded msg because of "
00341 "%u sysid is met after convertion!", idx);
00342 return false;
00343 }
00344
00345 }
00346
00347
00356 int recvFromMng(uint16_t mngIdx, mavlink_message_t *recvMsgList,
00357 mavlink_status_t *recvStatusList, uint16_t listSize) {
00358
00359 DJI2MAV_TRACE("Going to fetch message from Manager with "
00360 "index %u to a list of size %u...", mngIdx, listSize);
00361
00362 if( !isValidMngIdx(mngIdx) ) {
00363 DJI2MAV_ERROR("Fail to receive message because invalid"
00364 "mngIdx %u is met while mngListSize is %u!",
00365 mngIdx, m_maxListSize);
00366 return -2;
00367 }
00368
00369 int bytes_recv = m_mngList[mngIdx]->recv();
00370 if(bytes_recv > 0) {
00371 DJI2MAV_TRACE("Datagram: ");
00372 uint16_t cnt = 0;
00373 uint8_t* bufPtr = m_mngList[mngIdx]->getRecvBuf();
00374 for(int i = 0; i < bytes_recv; ++i, ++bufPtr) {
00375 #ifdef DJI2MAV_LOG_TRACE
00376 printf("%02x", *bufPtr);
00377 #endif
00378 if(cnt == listSize) {
00379 DJI2MAV_ERROR("Fail to parse all datagram because "
00380 "recvMsgList of size %u is not enough!",
00381 listSize);
00382 break;
00383 }
00384
00385 if( mavlink_parse_char(MAVLINK_COMM_0, *bufPtr,
00386 &recvMsgList[cnt], &recvStatusList[cnt]) ) {
00387 #ifdef DJI2MAV_LOG_TRACE
00388 if(i != bytes_recv) {
00389 printf("\n");
00390 DJI2MAV_TRACE("Another datagram: ");
00391 }
00392 #endif
00393 ++cnt;
00394 }
00395 }
00396 #ifdef DJI2MAV_LOG_TRACE
00397 printf("\n");
00398 #endif
00399 if(cnt > 0)
00400 updateSysid(mngIdx, recvMsgList[0].sysid);
00401
00402 DJI2MAV_TRACE("Totally %u messages are received from "
00403 "Manager of index %u.", cnt, mngIdx);
00404 return cnt;
00405 } else if(-1 == bytes_recv) {
00406 DJI2MAV_TRACE("No datagram received from Manager with "
00407 "index %d!", mngIdx);
00408 return 0;
00409 } else {
00410 DJI2MAV_TRACE("Receiving process of Manager with index %u "
00411 "fail! Return value: %d!", mngIdx, bytes_recv);
00412 return -1;
00413 }
00414
00415 }
00416
00417
00425 int recvFromAll(mavlink_message_t* recvMsgList,
00426 mavlink_status_t* recvStatusList, uint16_t listSize) {
00427
00428 int cnt = 0;
00429 bool err = false;
00430 for(uint16_t i = 0; i < m_maxListSize; ++i) {
00431 if(NULL != m_mngList[i]) {
00432 int r = recvFromMng(i, recvMsgList + cnt,
00433 recvStatusList + cnt, listSize - cnt);
00434 DJI2MAV_TRACE("Execute receiving process on Manager "
00435 "with index %u and listSize %u. Return value "
00436 "is %d.", i, listSize - cnt, r);
00437 if(r >= 0) {
00438 DJI2MAV_TRACE("Receive %d msg from Manager with "
00439 "index %u.", r, i);
00440 cnt += r;
00441 } else {
00442 DJI2MAV_ERROR("Fail to recv msg from Manager "
00443 "with index %u!", i);
00444 err = true;
00445 }
00446 }
00447 }
00448 if(err) {
00449 DJI2MAV_ERROR("Fail to execute receiving process on some "
00450 "Manager!");
00451 return -1;
00452 } else {
00453 return cnt;
00454 }
00455
00456 }
00457
00458
00459 private:
00460 MsgManager** m_mngList;
00461 uint16_t m_maxListSize;
00462 uint8_t* m_sysidRecord;
00463
00464 uint8_t m_mySysid;
00465
00466
00467 };
00468
00469 }
00470
00471
00472 #endif