00001
00002
00003
00004
00005
00006
00007
00008
00009 #ifndef _DJI2MAV_MAVHOTPOINT_H_
00010 #define _DJI2MAV_MAVHOTPOINT_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 "hotpointData.h"
00020 #include "dji_sdk_dji2mav/log.h"
00021
00022 namespace dji2mav{
00023
00024 class MavHotpoint : public MavModule {
00025 public:
00033 MavHotpoint(MavHandler &handler, std::string name,
00034 uint16_t gcsNum, ...) : MavModule(handler, name, 4096) {
00035
00036 DJI2MAV_DEBUG("Going to construct Hotpoint 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 Hotpoint module.");
00061
00062 }
00063
00064
00065 ~MavHotpoint() {
00066 DJI2MAV_DEBUG("Going to destruct Hotpoint module...");
00067 DJI2MAV_DEBUG("...finish destructing Hotpoint module.");
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 Hotpoint the compid is %u.",
00120 recvMsg.compid);
00121 }
00122
00123 DJI2MAV_DEBUG("In Hotpoint 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 = 1;
00144
00145 mavlink_message_t sendMsg;
00146 mavlink_msg_mission_count_encode(getMySysid(),
00147 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &cntMsg);
00148 sendMsgToMaster(sendMsg);
00149
00150 if(NULL != m_missionRequestListHook)
00151 m_missionRequestListHook();
00152
00153 }
00154
00155
00156 void reactToMissionRequest(uint16_t gcsIdx,
00157 const mavlink_message_t &recvMsg) {
00158
00159 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00160 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00161 recvMsg.compid);
00162 }
00163
00164 DJI2MAV_DEBUG("In Hotpoint mission request with status: %d.",
00165 (int)m_status);
00166
00167 switch(m_status) {
00168 case idle:
00169 case downloading:
00170 case loaded:
00171 case executing:
00172 case paused:
00173 return;
00174 case uploading:
00175 case error:
00176 break;
00177 }
00178
00179 mavlink_mission_request_t reqMsg;
00180 mavlink_msg_mission_request_decode(&recvMsg, &reqMsg);
00181
00182 mavlink_mission_item_t itemMsg;
00183 if(reqMsg.seq == 0) {
00184 itemMsg.target_system = recvMsg.sysid;
00185 itemMsg.target_component = recvMsg.compid;
00186 itemMsg.seq = reqMsg.seq;
00187 m_hp.getHotpointData(itemMsg.seq, itemMsg.command,
00188 itemMsg.param1, itemMsg.param2, itemMsg.param3,
00189 itemMsg.param4, itemMsg.x, itemMsg.y, itemMsg.z);
00190 } else {
00191 DJI2MAV_ERROR("Invalid sequence %u of mission request in "
00192 "Hotpoint!", reqMsg.seq);
00193 return;
00194 }
00195
00196 mavlink_message_t sendMsg;
00197 mavlink_msg_mission_item_encode(getMySysid(),
00198 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &itemMsg);
00199 sendMsgToMaster(sendMsg);
00200
00201 if(NULL != m_missionRequestHook)
00202 m_missionRequestHook(reqMsg.seq);
00203
00204 }
00205
00206
00207 void reactToMissionAck(uint16_t gcsIdx,
00208 const mavlink_message_t &recvMsg) {
00209
00210 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00211 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00212 recvMsg.compid);
00213 }
00214
00215 DJI2MAV_DEBUG("In Hotpoint mission ack with status: %d.",
00216 (int)m_status);
00217
00218 switch(m_status) {
00219 case idle:
00220 case downloading:
00221 case loaded:
00222 case executing:
00223 case paused:
00224 return;
00225 case uploading:
00226 m_status = loaded;
00227 break;
00228 case error:
00229 break;
00230 }
00231
00232 mavlink_mission_ack_t ackMsg;
00233 mavlink_msg_mission_ack_decode(&recvMsg, &ackMsg);
00234 DJI2MAV_DEBUG("In Hotpoint mission ACK code: %d.", ackMsg.type);
00235 m_status = loaded;
00236 m_hp.display();
00237
00238 if(NULL != m_missionAckHook)
00239 m_missionAckHook();
00240
00241 }
00242
00243
00244 void reactToMissionCount(uint16_t gcsIdx,
00245 const mavlink_message_t &recvMsg) {
00246
00247 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00248 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00249 recvMsg.compid);
00250 }
00251
00252 DJI2MAV_DEBUG("In Hotpoint mission count with status: %d.",
00253 (int)m_status);
00254
00255 switch(m_status) {
00256 case idle:
00257 case loaded:
00258 m_status = downloading;
00259 break;
00260 case downloading:
00261 case uploading:
00262 case executing:
00263 case paused:
00264 case error:
00265 return;
00266 }
00267
00268 mavlink_mission_count_t cntMsg;
00269 mavlink_msg_mission_count_decode(&recvMsg, &cntMsg);
00270
00271 mavlink_mission_request_t reqMsg;
00272 reqMsg.target_system = recvMsg.sysid;
00273 reqMsg.target_component = recvMsg.compid;
00274 reqMsg.seq = 0;
00275
00276 mavlink_message_t sendMsg;
00277 mavlink_msg_mission_request_encode(getMySysid(),
00278 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &reqMsg);
00279 sendMsgToMaster(sendMsg);
00280
00281 if(NULL != m_missionCountHook)
00282 m_missionCountHook(cntMsg.count);
00283
00284 }
00285
00286
00287 void reactToMissionItem(uint16_t gcsIdx,
00288 const mavlink_message_t &recvMsg) {
00289
00290 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00291 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00292 recvMsg.compid);
00293 }
00294
00295 DJI2MAV_DEBUG("In Hotpoint mission item with status: %d.",
00296 (int)m_status);
00297
00298 switch(m_status) {
00299 case loaded:
00300 case executing:
00301 case idle:
00302 case uploading:
00303 case paused:
00304 case error:
00305 return;
00306 case downloading:
00307 break;
00308 }
00309
00310 mavlink_mission_item_t itemMsg;
00311 mavlink_msg_mission_item_decode(&recvMsg, &itemMsg);
00312 if(itemMsg.seq == 0) {
00313
00314 m_hp.setHotpointData(itemMsg.seq, itemMsg.command,
00315 itemMsg.param1, itemMsg.param2, itemMsg.param3,
00316 itemMsg.param4, itemMsg.x, itemMsg.y, itemMsg.z);
00317
00318 } else {
00319 DJI2MAV_ERROR("Invalid sequence %u of mission item in "
00320 "Hotpoint!", itemMsg.seq);
00321 return;
00322 }
00323
00324 mavlink_message_t sendMsg;
00325 mavlink_msg_mission_ack_pack(getMySysid(),
00326 MAV_COMP_ID_MISSIONPLANNER, &sendMsg,
00327 recvMsg.sysid, recvMsg.compid,
00328 MAV_MISSION_ACCEPTED);
00329 sendMsgToMaster(sendMsg);
00330 m_status = loaded;
00331 m_hp.display();
00332
00333 if(NULL != m_missionItemHook)
00334 m_missionItemHook(itemMsg.seq);
00335
00336 }
00337
00338
00339 void reactToMissionClearAll(uint16_t gcsIdx,
00340 const mavlink_message_t &recvMsg) {
00341
00342 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00343 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00344 recvMsg.compid);
00345 }
00346
00347 DJI2MAV_DEBUG("In Hotpoint mission clear all with status: %d.",
00348 (int)m_status);
00349
00350 switch(m_status) {
00351 case idle:
00352 case uploading:
00353 case downloading:
00354 case loaded:
00355 case error:
00356 m_status = idle;
00357 break;
00358 case executing:
00359 case paused:
00360 return;
00361 }
00362
00363 mavlink_mission_ack_t ackMsg;
00364 ackMsg.target_system = recvMsg.sysid;
00365 ackMsg.target_component = recvMsg.compid;
00366 ackMsg.type = MAV_MISSION_ACCEPTED;
00367
00368 mavlink_message_t sendMsg;
00369 mavlink_msg_mission_ack_encode(getMySysid(),
00370 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, &ackMsg);
00371 sendMsgToMaster(sendMsg);
00372
00373 m_hp.clear();
00374
00375 if(NULL != m_missionClearAllHook)
00376 m_missionClearAllHook();
00377
00378 }
00379
00380
00381 void reactToMissionSetCurrent(uint16_t gcsIdx,
00382 const mavlink_message_t &recvMsg) {
00383
00384 if(recvMsg.compid != MAV_COMP_ID_MISSIONPLANNER) {
00385 DJI2MAV_DEBUG("In Hotpoint the compid is %u.",
00386 recvMsg.compid);
00387 }
00388
00389 DJI2MAV_DEBUG("In Hotpoint mission set current with status: "
00390 "%d.", (int)m_status);
00391
00392 switch(m_status) {
00393 case idle:
00394 case uploading:
00395 case downloading:
00396 case error:
00397 return;
00398 case loaded:
00399 case paused:
00400 m_status = executing;
00401 break;
00402 case executing:
00403 break;
00404 }
00405
00406 mavlink_mission_set_current_t setCurrMsg;
00407 mavlink_msg_mission_set_current_decode(&recvMsg, &setCurrMsg);
00408 if(0 == setCurrMsg.seq) {
00409
00410 if(NULL != m_targetHook) {
00411 m_targetHook( m_hp.getHotpoint(), 7, m_hp.getCmd() );
00412 DJI2MAV_DEBUG("Finish running target hook.");
00413
00414
00415 clearBuf();
00416 } else {
00417 DJI2MAV_WARN("In Hotpoint no target hook is set.");
00418 }
00419
00420 mavlink_message_t sendMsg;
00421 mavlink_msg_mission_current_pack( getMySysid(),
00422 MAV_COMP_ID_MISSIONPLANNER, &sendMsg, 0 );
00423 sendMsgToMaster(sendMsg);
00424
00425 m_status = loaded;
00426
00427 } else {
00428 m_status = idle;
00429 DJI2MAV_ERROR("The sequence %u of set current message is "
00430 "invalid in Hotpoint!", setCurrMsg.seq);
00431 }
00432
00433 if(NULL != m_missionSetCurrentHook)
00434 m_missionSetCurrentHook(setCurrMsg.seq);
00435
00436 }
00437
00438
00439 void setMissionRequestListHook(void (*func)()) {
00440 m_missionRequestListHook = func;
00441 }
00442
00443
00444 void setMissionRequestHook(void (*func)(uint16_t)) {
00445 m_missionRequestHook = func;
00446 }
00447
00448
00449 void setMissionAckHook(void (*func)()) {
00450 m_missionAckHook = func;
00451 }
00452
00453
00454 void setMissionCountHook(void (*func)(uint16_t)) {
00455 m_missionCountHook = func;
00456 }
00457
00458
00459 void setMissionItemHook(void (*func)(uint16_t)) {
00460 m_missionItemHook = func;
00461 }
00462
00463
00464 void setMissionClearAllHook(void (*func)()) {
00465 m_missionClearAllHook = func;
00466 }
00467
00468
00469 void setMissionSetCurrentHook(void (*func)(uint16_t)) {
00470 m_missionSetCurrentHook = func;
00471 }
00472
00473
00474 void setTargetHook(void (*func)(const float[], uint16_t, uint16_t)) {
00475 m_targetHook = func;
00476 }
00477
00478
00479
00480 private:
00481 HotpointData m_hp;
00482
00483 enum {
00484 idle,
00485 uploading,
00486 downloading,
00487 loaded,
00488 executing,
00489 paused,
00490 error
00491 } m_status;
00492
00493 void (*m_missionRequestListHook)();
00494 void (*m_missionRequestHook)(uint16_t);
00495 void (*m_missionAckHook)();
00496 void (*m_missionCountHook)(uint16_t);
00497 void (*m_missionItemHook)(uint16_t);
00498 void (*m_missionClearAllHook)();
00499 void (*m_missionSetCurrentHook)(uint16_t);
00500 void (*m_targetHook)(const float[], uint16_t, uint16_t);
00501
00502
00503 };
00504
00505 }
00506
00507
00508 #endif