00001 00012 #include "DJI_Follow.h" 00013 00014 using namespace DJI; 00015 using namespace DJI::onboardSDK; 00016 00017 Follow::Follow(CoreAPI *ControlAPI) 00018 { 00019 api = ControlAPI; 00020 resetData(); 00021 } 00022 00023 void Follow::resetData() 00024 { 00025 followData.mode = MODE_RELATIVE; 00026 followData.yaw = YAW_TOTARGET; 00027 followData.target.latitude = api->getBroadcastData().pos.latitude; 00028 followData.target.longitude = api->getBroadcastData().pos.longitude; 00029 followData.target.height = api->getBroadcastData().pos.altitude; 00030 followData.target.angle = 0; 00031 followData.sensitivity = 1; 00032 } 00033 00034 void Follow::start(FollowData *Data, CallBack callback, UserData userData) 00035 { 00036 if (Data) 00037 followData = *Data; 00038 else 00039 resetData(); 00040 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_START, &followData, sizeof(followData), 500, 2, 00041 callback ? callback : missionCallback, userData); 00042 } 00043 00044 MissionACK Follow::start(FollowData *Data, int timeout) 00045 { 00046 if (Data) 00047 followData = *Data; 00048 else 00049 resetData(); 00050 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_START, &followData, sizeof(followData), 500, 2, 0,0); 00051 00052 api->serialDevice->lockACK(); 00053 api->serialDevice->wait(timeout); 00054 api->serialDevice->freeACK(); 00055 00056 return api->missionACKUnion.missionACK; 00057 } 00058 00059 void Follow::stop(CallBack callback, UserData userData) 00060 { 00061 uint8_t zero = 0; 00062 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_STOP, &zero, sizeof(zero), 500, 2, 00063 callback ? callback : missionCallback, userData); 00064 } 00065 00066 MissionACK Follow::stop(int timeout) 00067 { 00068 uint8_t zero = 0; 00069 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_STOP, &zero, sizeof(zero), 500, 2, 0, 0); 00070 00071 api->serialDevice->lockACK(); 00072 api->serialDevice->wait(timeout); 00073 api->serialDevice->freeACK(); 00074 00075 return api->missionACKUnion.missionACK; 00076 } 00077 00078 void Follow::pause(bool isPause, CallBack callback, UserData userData) 00079 { 00080 uint8_t followData = isPause ? 0 : 1; 00081 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_SETPAUSE, &followData, sizeof(followData), 500, 2, 00082 callback ? callback : missionCallback, userData); 00083 } 00084 00085 MissionACK Follow::pause(bool isPause, int timeout) 00086 { 00087 uint8_t followData = isPause ? 0 : 1; 00088 api->send(2, encrypt, SET_MISSION, CODE_FOLLOW_SETPAUSE, &followData, sizeof(followData), 500, 2, 0, 0); 00089 00090 api->serialDevice->lockACK(); 00091 api->serialDevice->wait(timeout); 00092 api->serialDevice->freeACK(); 00093 00094 return api->missionACKUnion.missionACK; 00095 } 00096 00097 void Follow::updateTarget(FollowTarget target) 00098 { 00099 followData.target = target; 00100 api->send(0, encrypt, SET_MISSION, CODE_FOLLOW_TARGET, &(followData.target), sizeof(FollowTarget)); 00101 } 00102 00103 void Follow::updateTarget(float64_t latitude, float64_t longitude, uint16_t height, 00104 uint16_t angle) 00105 { 00106 followData.target.latitude = latitude; 00107 followData.target.longitude = longitude; 00108 followData.target.height = height; 00109 followData.target.angle = angle; 00110 api->send(0, encrypt, SET_MISSION, CODE_FOLLOW_TARGET, &(followData.target), sizeof(FollowTarget)); 00111 } 00112 00113 FollowData Follow::getData() const { return followData; } 00114 00115 void Follow::setData(const FollowData &value) { followData = value; } 00116 00117 void Follow::setMode(const Follow::MODE mode __UNUSED) 00118 { 00119 API_LOG(api->getDriver(), STATUS_LOG, "no available mode but default"); 00120 followData.mode = 0; 00121 } 00122 00123 void Follow::setTarget(FollowTarget target) { followData.target = target; } 00124 00125 void Follow::setYawType(const Follow::YAW_TYPE type) { followData.yaw = type; } 00126 00127 void Follow::setSensitivity(const Follow::SENSITIVITY sense __UNUSED) 00128 { 00129 API_LOG(api->getDriver(), STATUS_LOG, "no available mode but default"); 00130 followData.sensitivity = 1; 00131 }