00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef _MAV2DJI_MAVWAYPOINT_H_
00010 #define _MAV2DJI_MAVWAYPOINT_H_
00011
00012
00013 #include <mavlink/common/mavlink.h>
00014 #include <new>
00015 #include <string>
00016 #include <cstdarg>
00017
00018 #include "dji_sdk_dji2mav/modules/mavModule.h"
00019 #include "waypointList.h"
00020 #include "dji_sdk_dji2mav/log.h"
00021
00022 namespace dji2mav{
00023
00024 class MavWaypoint : public MavModule{
00025 public:
00033 MavWaypoint(MavHandler &handler, std::string name,
00034 uint16_t gcsNum, ...) : MavModule(handler, name, 4096) {
00035
00036 DJI2MAV_DEBUG("Going to construct Waypoint module with name "
00037 "%s and gcsNum %u...", name.c_str(), gcsNum);
00038
00039 setMissionRequestListHook(NULL);
00040 setMissionRequestHook(NULL);
00041 setMissionAckHook(NULL);
00042 setMissionCountHook(NULL);
00043 setMissionItemHook(NULL);
00044 setMissionClearAllHook(NULL);
00045 setMissionSetCurrentHook(NULL);
00046
00047 va_list arg;
00048 va_start(arg, gcsNum);
00049 if(1 == gcsNum) {
00050 setMasterGcsIdx( (uint16_t)va_arg(arg, int) );
00051 } else {
00052 for(uint16_t i = 0; i < gcsNum; ++i) {
00053 employGcsSender( (uint16_t)va_arg(arg, int) );
00054 }
00055 }
00056 va_end(arg);
00057
00058
00059
00060 DJI2MAV_DEBUG("...finish constructing Waypoint module.");
00061
00062 }
00063
00064
00065 ~MavWaypoint() {
00066 DJI2MAV_DEBUG("Going to destruct Waypoint module...");
00067 DJI2MAV_DEBUG("...finish destructing Waypoint moduel.");
00068 }
00069
00070
00075 void passivelyReceive(mavlink_message_t &msg) {
00076 switch(msg.msgid) {
00077
00078 case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
00079 reactToMissionRequestList(getMasterGcsIdx(), msg);
00080 break;
00081 case MAVLINK_MSG_ID_MISSION_REQUEST:
00082 reactToMissionRequest(getMasterGcsIdx(), msg);
00083 break;
00084 case MAVLINK_MSG_ID_MISSION_ACK:
00085 reactToMissionAck(getMasterGcsIdx(), msg);
00086 break;
00087 case MAVLINK_MSG_ID_MISSION_COUNT:
00088 reactToMissionCount(getMasterGcsIdx(), msg);
00089 break;
00090 case MAVLINK_MSG_ID_MISSION_ITEM:
00091 reactToMissionItem(getMasterGcsIdx(), msg);
00092 break;
00093 case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
00094 reactToMissionClearAll(getMasterGcsIdx(), msg);
00095 break;
00096 case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
00097 reactToMissionSetCurrent(getMasterGcsIdx(), msg);
00098 break;
00099 default:
00100 DJI2MAV_WARN("No execution is defined for msgid #%u "
00101 "in Hotpoint module.", msg.msgid);
00102 break;
00103 }
00104 usleep(20000);
00105 }
00106
00107
00111 void activelySend() {
00112 }
00113
00114
00115 void reactToMissionRequestList(uint16_t gcsIdx,
00116 const mavlink_message_t &recvMsg) {
00117
00118 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00119 DJI2MAV_DEBUG("In Waypoint the compid is %u.",
00120 recvMsg.compid);
00121 }
00122
00123 DJI2MAV_DEBUG("In Waypoint mission request list with status: "
00124 "%d.", (int)m_status);
00125
00126 switch(m_status) {
00127 case idle:
00128 case downloading:
00129 case executing:
00130 case paused:
00131 return;
00132 case loaded:
00133 m_status = uploading;
00134 break;
00135 case uploading:
00136 case error:
00137 break;
00138 }
00139
00140 mavlink_mission_count_t cntMsg;
00141 cntMsg.target_system = recvMsg.sysid;
00142 cntMsg.target_component = recvMsg.compid;
00143 cntMsg.count = m_wpl.getListSize();
00144 m_wpl.readyToUpload();
00145
00146 mavlink_message_t sendMsg;
00147 mavlink_msg_mission_count_encode(getMySysid(),
00148 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &cntMsg);
00149 sendMsgToMaster(sendMsg);
00150
00151 if(NULL != m_missionRequestListHook)
00152 m_missionRequestListHook();
00153
00154 }
00155
00156
00157 void reactToMissionRequest(uint16_t gcsIdx,
00158 const mavlink_message_t &recvMsg) {
00159
00160 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00161 DJI2MAV_DEBUG("In Waypoint the compid is %u.",
00162 recvMsg.compid);
00163 }
00164
00165 DJI2MAV_DEBUG("In Waypoint mission request with status: "
00166 "%d.", (int)m_status);
00167
00168 switch(m_status) {
00169 case idle:
00170 case downloading:
00171 case loaded:
00172 case executing:
00173 case paused:
00174 return;
00175 case uploading:
00176 case error:
00177 break;
00178 }
00179
00180 mavlink_mission_request_t reqMsg;
00181 mavlink_msg_mission_request_decode(&recvMsg, &reqMsg);
00182
00183 mavlink_mission_item_t itemMsg;
00184 itemMsg.target_system = recvMsg.sysid;
00185 itemMsg.target_component = recvMsg.compid;
00186 itemMsg.seq = reqMsg.seq;
00187 if( m_wpl.isValidIdx(itemMsg.seq) ) {
00188
00189 m_wpl.getWaypointData(itemMsg.seq, itemMsg.command,
00190 itemMsg.param1, itemMsg.param2, itemMsg.param3,
00191 itemMsg.param4, itemMsg.x, itemMsg.y, itemMsg.z);
00192
00193 } else {
00194 DJI2MAV_ERROR("Invalid sequence %u of mission request in"
00195 "Waypoint!", reqMsg.seq);
00196 return;
00197 }
00198
00199 mavlink_message_t sendMsg;
00200 mavlink_msg_mission_item_encode(getMySysid(),
00201 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &itemMsg);
00202 sendMsgToMaster(sendMsg);
00203
00204 if(NULL != m_missionRequestHook)
00205 m_missionRequestHook(reqMsg.seq);
00206
00207 }
00208
00209
00210 void reactToMissionAck(uint16_t gcsIdx,
00211 const mavlink_message_t &recvMsg) {
00212
00213 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00214 DJI2MAV_DEBUG("In Wotpoint the compid is %u.",
00215 recvMsg.compid);
00216 }
00217
00218 DJI2MAV_DEBUG("In Waypoint mission ack with status: %d.",
00219 (int)m_status);
00220
00221 switch(m_status) {
00222 case idle:
00223 case downloading:
00224 case loaded:
00225 case executing:
00226 case paused:
00227 return;
00228 case uploading:
00229 m_status = loaded;
00230 break;
00231 case error:
00232 break;
00233 }
00234
00235 mavlink_mission_ack_t ackMsg;
00236 mavlink_msg_mission_ack_decode(&recvMsg, &ackMsg);
00237 DJI2MAV_DEBUG("In Waypoint mission ACK code: %d.", ackMsg.type);
00238 m_status = loaded;
00239 m_wpl.finishUpload();
00240 m_wpl.displayMission();
00241
00242 if(NULL != m_missionAckHook)
00243 m_missionAckHook();
00244
00245 }
00246
00247
00248 void reactToMissionCount(uint16_t gcsIdx,
00249 const mavlink_message_t &recvMsg) {
00250
00251 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00252 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00253 recvMsg.compid);
00254 }
00255
00256 DJI2MAV_DEBUG("In Waypoint mission count with status: %d.",
00257 (int)m_status);
00258
00259 switch(m_status) {
00260 case idle:
00261 case loaded:
00262 m_status = downloading;
00263 break;
00264 case downloading:
00265 case uploading:
00266 case executing:
00267 case paused:
00268 case error:
00269 return;
00270 }
00271
00272 mavlink_mission_count_t cntMsg;
00273 mavlink_msg_mission_count_decode(&recvMsg, &cntMsg);
00274 m_wpl.setListSize(cntMsg.count);
00275 m_wpl.readyToDownload();
00276
00277 mavlink_mission_request_t reqMsg;
00278 reqMsg.target_system = recvMsg.sysid;
00279 reqMsg.target_component = recvMsg.compid;
00280 reqMsg.seq = 0;
00281
00282 mavlink_message_t sendMsg;
00283 mavlink_msg_mission_request_encode(getMySysid(),
00284 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &reqMsg);
00285 sendMsgToMaster(sendMsg);
00286
00287 if(NULL != m_missionCountHook)
00288 m_missionCountHook(cntMsg.count);
00289
00290 }
00291
00292
00293 void reactToMissionItem(uint16_t gcsIdx,
00294 const mavlink_message_t &recvMsg) {
00295
00296 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00297 DJI2MAV_DEBUG("In Waypoint the compid is %u.",
00298 recvMsg.compid);
00299 }
00300
00301 DJI2MAV_DEBUG("In Hotpoint mission item with status: %d.",
00302 (int)m_status);
00303
00304 switch(m_status) {
00305 case loaded:
00306 case executing:
00307 case idle:
00308 case uploading:
00309 case paused:
00310 case error:
00311 return;
00312 case downloading:
00313 break;
00314 }
00315
00316 mavlink_mission_item_t itemMsg;
00317 mavlink_msg_mission_item_decode(&recvMsg, &itemMsg);
00318 if( m_wpl.isValidIdx(itemMsg.seq) ) {
00319
00320 m_wpl.setWaypointData(itemMsg.seq, itemMsg.command,
00321 itemMsg.param1, itemMsg.param2, itemMsg.param3,
00322 itemMsg.param4, itemMsg.x, itemMsg.y, itemMsg.z);
00323
00324 } else {
00325 DJI2MAV_ERROR("Invalid sequence %u of mission item in "
00326 "Waypoint!", itemMsg.seq);
00327 return;
00328 }
00329
00330 mavlink_message_t sendMsg;
00331 if( m_wpl.isDownloadFinished() ) {
00332 mavlink_msg_mission_ack_pack(getMySysid(),
00333 MAV_COMP_ID_MISSIONPLANNER, &sendMsg,
00334 recvMsg.sysid, recvMsg.compid,
00335 MAV_MISSION_ACCEPTED);
00336 sendMsgToMaster(sendMsg);
00337 m_status = loaded;
00338 m_wpl.displayMission();
00339 } else {
00340 mavlink_mission_request_t reqMsg;
00341 reqMsg.target_system = recvMsg.sysid;
00342 reqMsg.target_component = recvMsg.compid;
00343 reqMsg.seq = m_wpl.getTargetIdx();
00344 mavlink_msg_mission_request_encode(getMySysid(),
00345 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &reqMsg);
00346 sendMsgToMaster(sendMsg);
00347 DJI2MAV_TRACE("In Waypoint send request %u, %u, %u.",
00348 reqMsg.target_system, reqMsg.target_component,
00349 reqMsg.seq);
00350 }
00351
00352 if(NULL != m_missionItemHook)
00353 m_missionItemHook(itemMsg.seq);
00354
00355 }
00356
00357
00358 void reactToMissionClearAll(uint16_t gcsIdx,
00359 const mavlink_message_t &recvMsg) {
00360
00361 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00362 DJI2MAV_DEBUG("In Waypoint the compid is %u\n",
00363 recvMsg.compid);
00364 }
00365
00366 DJI2MAV_DEBUG("In Waypoint mission clear all with status: %d",
00367 (int)m_status);
00368
00369 switch(m_status) {
00370 case idle:
00371 case uploading:
00372 case downloading:
00373 case loaded:
00374 case error:
00375 m_status = idle;
00376 break;
00377 case executing:
00378 case paused:
00379 return;
00380 }
00381
00382 mavlink_mission_ack_t ackMsg;
00383 ackMsg.target_system = recvMsg.sysid;
00384 ackMsg.target_component = recvMsg.compid;
00385 ackMsg.type = MAV_MISSION_ACCEPTED;
00386
00387 mavlink_message_t sendMsg;
00388 mavlink_msg_mission_ack_encode(getMySysid(),
00389 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &ackMsg);
00390 sendMsgToMaster(sendMsg);
00391
00392 m_wpl.clearMission();
00393
00394 if(NULL != m_missionClearAllHook)
00395 m_missionClearAllHook();
00396
00397 }
00398
00399
00400 void reactToMissionSetCurrent(uint16_t gcsIdx,
00401 const mavlink_message_t &recvMsg) {
00402
00403 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00404 DJI2MAV_DEBUG("In Waypoint the compid is %u.",
00405 recvMsg.compid);
00406 }
00407
00408 DJI2MAV_DEBUG("In Waypoint mission set current with status: "
00409 "%d.", (int)m_status);
00410
00411 switch(m_status) {
00412 case idle:
00413 case uploading:
00414 case downloading:
00415 case error:
00416 return;
00417 case loaded:
00418 case paused:
00419 m_status = executing;
00420 break;
00421 case executing:
00422 break;
00423 }
00424
00425 mavlink_mission_set_current_t setCurrMsg;
00426 mavlink_msg_mission_set_current_decode(&recvMsg, &setCurrMsg);
00427 if( m_wpl.isValidIdx(setCurrMsg.seq) ) {
00428
00429 m_wpl.setTargetIdx(setCurrMsg.seq);
00430
00431 if(NULL != m_targetHook) {
00432 m_targetHook( m_wpl.getWaypointList(),
00433 setCurrMsg.seq,
00434 m_wpl.getListSize() );
00435
00436
00437 clearBuf();
00438 } else {
00439 DJI2MAV_WARN("In Waypoint no target hook is set.");
00440 }
00441
00442 mavlink_message_t sendMsg;
00443 mavlink_msg_mission_current_pack( getMySysid(),
00444 MAV_COMP_ID_MISSIONPLANNER, &sendMsg,
00445 m_wpl.getTargetIdx() );
00446 sendMsgToMaster(sendMsg);
00447
00448 m_status = loaded;
00449
00450 } else {
00451 m_status = paused;
00452 DJI2MAV_ERROR( "The sequence %u of set current message is "
00453 "invalid in Waypoint! The size of list is %u!",
00454 setCurrMsg.seq, m_wpl.getListSize() );
00455 }
00456
00457 if(NULL != m_missionSetCurrentHook)
00458 m_missionSetCurrentHook(setCurrMsg.seq);
00459
00460 }
00461
00462
00467 inline void setMissionRequestListHook( void (*func)() ) {
00468 m_missionRequestListHook = func;
00469 }
00470
00471
00472 inline void setMissionRequestHook( void (*func)(uint16_t) ) {
00473 m_missionRequestHook = func;
00474 }
00475
00476
00477 inline void setMissionAckHook( void (*func)() ) {
00478 m_missionAckHook = func;
00479 }
00480
00481
00482 inline void setMissionCountHook( void (*func)(uint16_t) ) {
00483 m_missionCountHook = func;
00484 }
00485
00486
00487 inline void setMissionItemHook( void (*func)(uint16_t) ) {
00488 m_missionItemHook = func;
00489 }
00490
00491
00492 inline void setMissionClearAllHook( void (*func)() ) {
00493 m_missionClearAllHook = func;
00494 }
00495
00496
00497 inline void setMissionSetCurrentHook( void (*func)(uint16_t) ) {
00498 m_missionSetCurrentHook = func;
00499 }
00500
00501
00502 inline void setTargetHook( void (*func)(const float[][7], uint16_t, uint16_t) ) {
00503 m_targetHook = func;
00504 }
00505
00506
00507 private:
00508 WaypointList m_wpl;
00509
00510 enum {
00511 idle,
00512 uploading,
00513 downloading,
00514 loaded,
00515 executing,
00516 paused,
00517 error
00518 } m_status;
00519
00520 void (*m_missionRequestListHook)();
00521 void (*m_missionRequestHook)(uint16_t);
00522 void (*m_missionAckHook)();
00523 void (*m_missionCountHook)(uint16_t);
00524 void (*m_missionItemHook)(uint16_t);
00525 void (*m_missionClearAllHook)();
00526 void (*m_missionSetCurrentHook)(uint16_t);
00527 void (*m_targetHook)(const float[][7], uint16_t, uint16_t);
00528
00529
00530 };
00531
00532 }
00533
00534
00535 #endif