diff --git a/.gitignore b/.gitignore index 63a1b5590..2cb9687bc 100644 --- a/.gitignore +++ b/.gitignore @@ -49,15 +49,4 @@ build/ CMakeFiles/ CMakeCache.txt cmake_install.cmake -<<<<<<< HEAD -<<<<<<< HEAD -Makefile -======= -Makefile - -osdk-core/test -contrib ->>>>>>> refs/heads/3.3 -======= -Makefile ->>>>>>> develop +Makefile \ No newline at end of file diff --git a/README.md b/README.md index d261bfd78..ac502e1bf 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ A new major version of DJI Onboard SDK (v3.3) was released on 15 Jun 2017. This | | 1.7.0.0 | 3.2.15.37 | OSDK 3.2 | | | | | | | | | **M600/M600 Pro** | *1.0.1.60* | *3.2.41.5* | *OSDK 3.3 (Current)* | Firmware being investigated for potential in-flight instability | -| | **1.0.1.20** | **3.2.15.62** | **OSDK 3.2** | OSDK 3.3 support for this firmware coming soon | +| | **1.0.1.20** | **3.2.15.62** | **OSDK 3.3 (Current)** | | | | 1.0.0.80 | 3.2.15.00 | OSDK 3.2 | | | | | | | | | **M100** | 1.3.1.0 | 3.1.10.0 | **OSDK 3.3 (Current)** | | diff --git a/osdk-core/api/inc/dji_ack.hpp b/osdk-core/api/inc/dji_ack.hpp index 9589d166e..b5ccfb83a 100644 --- a/osdk-core/api/inc/dji_ack.hpp +++ b/osdk-core/api/inc/dji_ack.hpp @@ -293,7 +293,7 @@ class ACK static const std::map createMFIOErrorCodeMap(); static const std::map createSetArmErrorCodeMap(); static const std::map - createM100TaskErrorCodeMap(); + createLegacyTaskErrorCodeMap(); }; // class ACK } // namespace OSDK diff --git a/osdk-core/api/inc/dji_broadcast.hpp b/osdk-core/api/inc/dji_broadcast.hpp index be8203031..8a55acbfd 100644 --- a/osdk-core/api/inc/dji_broadcast.hpp +++ b/osdk-core/api/inc/dji_broadcast.hpp @@ -317,17 +317,23 @@ class DataBroadcast private: /*! - * @brief Extract broadcast data for A3/N3 - * @param recvFrame: the raw data payload + * @brief Extract broadcast data for A3/N3/M600 + * @param recvFrame: pointer to the raw data payload */ void unpackData(RecvContainer* recvFrame); /*! * @brief Extract broadcast data for M100 - * @param recvFrame: the raw data payload + * @param recvFrame: pointer to the raw data payload */ void unpackM100Data(RecvContainer* pRecvFrame); + /*! + * @brief Extract broadcast data for M600 FW 3.2.41.5 + * @param recvFrame: pointer to the raw data payload + */ + void unpackOldM600Data(RecvContainer* recvFrame); + inline void unpackOne(FLAG flag, void* data, uint8_t*& buf, size_t size); public: @@ -355,13 +361,14 @@ class DataBroadcast Telemetry::Battery battery ; Telemetry::SDKInfo info ; /* - * @note Broadcast data for Matrice 100 that is fundamentally - * different from the A3/N3 + * @note Broadcast data for Matrice 100/600 older firmware that is fundamentally + * different from the A3/N3/M600 newer firmware */ - Telemetry::M100TimeStamp m100TimeStamp; - Telemetry::M100Velocity m100Velocity; - Telemetry::M100Status m100FlightStatus; - Telemetry::M100Battery m100Battery; + Telemetry::LegacyTimeStamp legacyTimeStamp; + Telemetry::LegacyVelocity legacyVelocity; + Telemetry::LegacyStatus legacyStatus; + Telemetry::LegacyBattery legacyBattery; + Telemetry::LegacyGPSInfo legacyGPSInfo; // clang-format on private: Vehicle* vehicle; diff --git a/osdk-core/api/inc/dji_control.hpp b/osdk-core/api/inc/dji_control.hpp index 97d430cbd..4eda5701e 100644 --- a/osdk-core/api/inc/dji_control.hpp +++ b/osdk-core/api/inc/dji_control.hpp @@ -40,12 +40,12 @@ class Control /* * @note Matrice 100 flight commands */ - typedef struct M100CMD + typedef struct LegacyCMD { const static int goHome = 1; const static int takeOff = 4; const static int landing = 6; - } M100CMD; + } LegacyCMD; /* * @note OSDK release 3.3 @@ -238,11 +238,11 @@ class Control } AdvancedCtrlData; // pack(1) // CMD data supported in Matrice 100 - typedef struct M100CMDData + typedef struct LegacyCMDData { uint8_t sequence; uint8_t cmd; - } M100CMDData; // pack (1) + } LegacyCMDData; // pack (1) #pragma pack() /*! @note @@ -439,7 +439,7 @@ class Control /* * Task CMD data to send to the flight controller (supported in Matrice 100) */ - M100CMDData m100CMDData; + LegacyCMDData legacyCMDData; }; // class Control } // OSDK diff --git a/osdk-core/api/inc/dji_error.hpp b/osdk-core/api/inc/dji_error.hpp index 4d55c8391..9c95a41f1 100644 --- a/osdk-core/api/inc/dji_error.hpp +++ b/osdk-core/api/inc/dji_error.hpp @@ -335,11 +335,11 @@ class ErrorCode /* * Task ACKs Supported in firmware version < 3.3 */ - typedef struct M100Task + typedef struct LegacyTask { const static uint16_t SUCCESS; const static uint16_t FAIL; - } M100Task; + } LegacyTask; /*! * @brief CMDID: SetArm supported in products with diff --git a/osdk-core/api/inc/dji_telemetry.hpp b/osdk-core/api/inc/dji_telemetry.hpp index 4004fc358..418359aeb 100644 --- a/osdk-core/api/inc/dji_telemetry.hpp +++ b/osdk-core/api/inc/dji_telemetry.hpp @@ -500,19 +500,19 @@ typedef struct HardSyncData /*! * @brief Matrice 100 Timestamp data, available in Broadcast telemetry (only for M100) */ -typedef struct M100TimeStamp +typedef struct LegacyTimeStamp { uint32_t time; uint32_t nanoTime; uint8_t syncFlag; -} M100TimeStamp; // pack(1) +} LegacyTimeStamp; // pack(1) /*! * @brief Matrice 100 Velocity struct, returned in Broadcast telemetry (only for M100) * @note The velocity may be in body or ground frame * based on settings in DJI Assistant 2's SDK page. */ -typedef struct M100Velocity +typedef struct LegacyVelocity { float32_t x; float32_t y; @@ -523,18 +523,32 @@ typedef struct M100Velocity uint8_t health : 1; uint8_t sensorID : 4; uint8_t reserve : 3; -} M100Velocity; // pack(1) +} LegacyVelocity; // pack(1) typedef uint16_t EnableFlag; // pack(1) /*! * @brief Return type for flight status data broadcast (only for M100). Returns VehicleStatus::M100FlightStatus. */ -typedef uint8_t M100Status; // pack(1) +typedef uint8_t LegacyStatus; // pack(1) /*! * @brief Return type for battery data broadcast (only for M100). Returns percentage. */ -typedef uint8_t M100Battery; // pack(1) +typedef uint8_t LegacyBattery; // pack(1) + +/*! + * @brief struct for GPSInfo of data broadcast + * + * @note only work outside of simulation + */ +typedef struct LegacyGPSInfo +{ + PositionTimeStamp time; + int32_t longitude; /*!< 1/1.0e7deg */ + int32_t latitude; /*!< 1/1.0e7deg */ + int32_t HFSL; /*!< height above mean sea level (mm) */ + Vector3f velocityNED; /*!< cm/s */ +} LegacyGPSInfo; // pack(1) #pragma pack() diff --git a/osdk-core/api/inc/dji_vehicle.hpp b/osdk-core/api/inc/dji_vehicle.hpp index 826ae3c54..22375c3fe 100644 --- a/osdk-core/api/inc/dji_vehicle.hpp +++ b/osdk-core/api/inc/dji_vehicle.hpp @@ -203,8 +203,12 @@ class Vehicle Version::FirmWare getFwVersion() const; char* getHwVersion() const; char* getHwSerialNum() const; + bool isLegacyM600(); + bool isM100(); - void setKey(const char* key); + + + void setKey(const char* key); void setStopCond(bool stopCond); bool getStopCond(); CircularBuffer* circularBuffer; //! @note not used yet diff --git a/osdk-core/api/src/dji_ack.cpp b/osdk-core/api/src/dji_ack.cpp index f84646f28..cc5f655e5 100644 --- a/osdk-core/api/src/dji_ack.cpp +++ b/osdk-core/api/src/dji_ack.cpp @@ -259,18 +259,18 @@ ACK::createTaskErrorCodeMap() /* * Supported in Matrice 100 */ -const std::pair M100TaskData[] = { - std::make_pair(OpenProtocol::ErrorCode::ControlACK::M100Task::SUCCESS, +const std::pair LegacyTaskData[] = { + std::make_pair(OpenProtocol::ErrorCode::ControlACK::LegacyTask::SUCCESS, (const char*)"CONTROLLER_SUCCESS\n"), - std::make_pair(OpenProtocol::ErrorCode::ControlACK::M100Task::FAIL, + std::make_pair(OpenProtocol::ErrorCode::ControlACK::LegacyTask::FAIL, (const char*)"CONTROLLER_FAIL\n") }; const std::map -ACK::createM100TaskErrorCodeMap() +ACK::createLegacyTaskErrorCodeMap() { const std::map errorCodeMap( - M100TaskData, M100TaskData + sizeof M100TaskData / sizeof M100TaskData[0]); + LegacyTaskData, LegacyTaskData + sizeof LegacyTaskData / sizeof LegacyTaskData[0]); return errorCodeMap; } @@ -501,7 +501,7 @@ ACK::getError(ACK::ErrorCode ack) else if (memcmp(cmd, OpenProtocol::CMDSet::Control::setArm, sizeof(cmd)) == 0) { /* - * SetArm command supported in Matrice 100 + * SetArm command supported in Matrice 100/ M600 old firmware */ return (ack.data == OpenProtocol::ErrorCode::ControlACK::SetArm::SUCCESS) ? ACK::SUCCESS @@ -514,7 +514,15 @@ ACK::getError(ACK::ErrorCode ack) } else if (memcmp(cmd, OpenProtocol::CMDSet::Control::task, sizeof(cmd)) == 0) { - if (ack.info.version != Version::M100_31) + if (ack.info.version == Version::FW(3,2,15,62)) + { + //! ACKs supported in Matrice 600 old firmware + return (ack.data == + OpenProtocol::ErrorCode::ControlACK::LegacyTask::SUCCESS) + ? ACK::SUCCESS + : ACK::FAIL; + } + else if (ack.info.version != Version::M100_31) { return (ack.data == OpenProtocol::ErrorCode::ControlACK::Task::SUCCESS) ? ACK::SUCCESS @@ -524,7 +532,7 @@ ACK::getError(ACK::ErrorCode ack) { //! ACKs supported in Matrice 100 return (ack.data == - OpenProtocol::ErrorCode::ControlACK::M100Task::SUCCESS) + OpenProtocol::ErrorCode::ControlACK::LegacyTask::SUCCESS) ? ACK::SUCCESS : ACK::FAIL; } @@ -746,13 +754,17 @@ ACK::getCMDIDTaskMSG(ACK::ErrorCode ack) { std::map taskErrorCodeMap; - if (ack.info.version != Version::M100_31) + if (ack.info.version == Version::FW(3,2,15,62)) + { + taskErrorCodeMap = static_cast>(ACK::createLegacyTaskErrorCodeMap()); + } + else if (ack.info.version == Version::M100_31) { - taskErrorCodeMap = ACK::createTaskErrorCodeMap(); + taskErrorCodeMap = static_cast>(ACK::createLegacyTaskErrorCodeMap()); } else { - taskErrorCodeMap = ACK::createM100TaskErrorCodeMap(); + taskErrorCodeMap = static_cast>(ACK::createTaskErrorCodeMap()); } auto msg = taskErrorCodeMap.find(ack.data); diff --git a/osdk-core/api/src/dji_broadcast.cpp b/osdk-core/api/src/dji_broadcast.cpp index 74dafaa96..20c49d47e 100644 --- a/osdk-core/api/src/dji_broadcast.cpp +++ b/osdk-core/api/src/dji_broadcast.cpp @@ -21,7 +21,11 @@ DataBroadcast::unpackCallback(Vehicle* vehicle, RecvContainer recvFrame, { DataBroadcast* broadcastPtr = (DataBroadcast*)data; - if (broadcastPtr->getVehicle()->getFwVersion() != Version::M100_31) + if (broadcastPtr->getVehicle()->isLegacyM600()) + { + broadcastPtr->unpackOldM600Data(&recvFrame); + } + else if (broadcastPtr->getVehicle()->getFwVersion() != Version::M100_31) { broadcastPtr->unpackData(&recvFrame); } @@ -63,15 +67,21 @@ DataBroadcast::getTimeStamp() { Telemetry::TimeStamp data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = timeStamp; + // Supported Broadcast data in Matrice 600 old firmware + data.time_ms = legacyTimeStamp.time; + data.time_ns = legacyTimeStamp.nanoTime; } - else + else if(vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.time_ms = m100TimeStamp.time; - data.time_ns = m100TimeStamp.nanoTime; + data.time_ms = legacyTimeStamp.time; + data.time_ns = legacyTimeStamp.nanoTime; + } + else + { + data = timeStamp; } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; @@ -82,14 +92,19 @@ DataBroadcast::getSyncStamp() { Telemetry::SyncStamp data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = syncStamp; + // Supported Broadcast data in Matrice 600 old firmware + data.flag = legacyTimeStamp.syncFlag; } - else + else if(vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.flag = m100TimeStamp.syncFlag; + data.flag = legacyTimeStamp.syncFlag; + } + else + { + data = syncStamp; } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; @@ -120,16 +135,23 @@ DataBroadcast::getVelocity() { Telemetry::Vector3f data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = v; + // Supported Broadcast data in Matrice 600 old firmware + data.x = legacyVelocity.x; + data.y = legacyVelocity.y; + data.z = legacyVelocity.z; } - else + else if(vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.x = m100Velocity.x; - data.y = m100Velocity.y; - data.z = m100Velocity.z; + data.x = legacyVelocity.x; + data.y = legacyVelocity.y; + data.z = legacyVelocity.z; + } + else + { + data = v; } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; @@ -140,17 +162,23 @@ DataBroadcast::getVelocityInfo() { Telemetry::VelocityInfo data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = vi; + // Supported Broadcast data in Matrice 600 old firmware + data.health = legacyVelocity.health; + data.reserve = legacyVelocity.reserve; } - else + else if(vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.health = m100Velocity.health; - data.reserve = m100Velocity.reserve; + data.health = legacyVelocity.health; + data.reserve = legacyVelocity.reserve; // TODO add sensorID (only M100) } + else + { + data = vi; + } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; } @@ -192,7 +220,20 @@ DataBroadcast::getGPSInfo() { Telemetry::GPSInfo data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - data = gps; + if (vehicle->isLegacyM600()) + { + // Supported Broadcast data in Matrice 600 old firmware + data.latitude = legacyGPSInfo.latitude; + data.longitude = legacyGPSInfo.longitude; + data.HFSL = legacyGPSInfo.HFSL; + data.velocityNED = legacyGPSInfo.velocityNED; + data.time = legacyGPSInfo.time; + //GPS details not supported. + } + else + { + data = gps; + } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; } @@ -243,14 +284,19 @@ DataBroadcast::getStatus() { Telemetry::Status data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = status; + // Broadcast data on M600 old firmware. Only flight status is available. + data.flight = legacyStatus; } - else + else if(vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.flight = m100FlightStatus; + data.flight = legacyStatus; + } + else + { + data = status; } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; @@ -261,14 +307,19 @@ DataBroadcast::getBatteryInfo() { Telemetry::Battery data; vehicle->protocolLayer->getThreadHandle()->lockMSG(); - if(vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - data = battery; + // Only capacity is supported on old M600 FW + data.percentage = legacyBattery; } - else + else if (vehicle->isM100()) { // Supported Broadcast data in Matrice 100 - data.capacity = m100Battery; + data.percentage = legacyBattery; + } + else + { + data = battery; } vehicle->protocolLayer->getThreadHandle()->freeMSG(); return data; @@ -383,18 +434,44 @@ DataBroadcast::unpackM100Data(RecvContainer* pRecvFrame) passFlag = *(uint16_t*)pdata; pdata += sizeof(uint16_t); // clang-format off - unpackOne(FLAG_TIME ,&m100TimeStamp ,pdata,sizeof(m100TimeStamp )); - unpackOne(FLAG_QUATERNION ,&q ,pdata,sizeof(q )); - unpackOne(FLAG_ACCELERATION,&a ,pdata,sizeof(a )); - unpackOne(FLAG_VELOCITY ,&m100Velocity ,pdata,sizeof(m100Velocity )); - unpackOne(FLAG_ANGULAR_RATE,&w ,pdata,sizeof(w )); - unpackOne(FLAG_POSITION ,&gp ,pdata,sizeof(gp )); - unpackOne(FLAG_M100_MAG ,&mag ,pdata,sizeof(mag )); - unpackOne(FLAG_M100_RC ,&rc ,pdata,sizeof(rc )); - unpackOne(FLAG_M100_GIMBAL ,&gimbal ,pdata,sizeof(gimbal )); - unpackOne(FLAG_M100_STATUS ,&m100FlightStatus,pdata,sizeof(m100FlightStatus)); - unpackOne(FLAG_M100_BATTERY,&m100Battery ,pdata,sizeof(m100Battery )); - unpackOne(FLAG_M100_DEVICE ,&info ,pdata,sizeof(info )); + unpackOne(FLAG_TIME ,&legacyTimeStamp ,pdata,sizeof(legacyTimeStamp )); + unpackOne(FLAG_QUATERNION ,&q ,pdata,sizeof(q )); + unpackOne(FLAG_ACCELERATION,&a ,pdata,sizeof(a )); + unpackOne(FLAG_VELOCITY ,&legacyVelocity ,pdata,sizeof(legacyVelocity )); + unpackOne(FLAG_ANGULAR_RATE,&w ,pdata,sizeof(w )); + unpackOne(FLAG_POSITION ,&gp ,pdata,sizeof(gp )); + unpackOne(FLAG_M100_MAG ,&mag ,pdata,sizeof(mag )); + unpackOne(FLAG_M100_RC ,&rc ,pdata,sizeof(rc )); + unpackOne(FLAG_M100_GIMBAL ,&gimbal ,pdata,sizeof(gimbal )); + unpackOne(FLAG_M100_STATUS ,&legacyStatus ,pdata,sizeof(legacyStatus )); + unpackOne(FLAG_M100_BATTERY,&legacyBattery ,pdata,sizeof(legacyBattery )); + unpackOne(FLAG_M100_DEVICE ,&info ,pdata,sizeof(info )); + // clang-format on + vehicle->protocolLayer->getThreadHandle()->freeMSG(); +} + +void +DataBroadcast::unpackOldM600Data(RecvContainer* pRecvFrame) +{ + uint8_t* pdata = pRecvFrame->recvData.raw_ack_array; + vehicle->protocolLayer->getThreadHandle()->lockMSG(); + passFlag = *(uint16_t*)pdata; + pdata += sizeof(uint16_t); + // clang-format off + unpackOne(FLAG_TIME ,&legacyTimeStamp ,pdata,sizeof(legacyTimeStamp )); + unpackOne(FLAG_QUATERNION ,&q ,pdata,sizeof(q )); + unpackOne(FLAG_ACCELERATION,&a ,pdata,sizeof(a )); + unpackOne(FLAG_VELOCITY ,&legacyVelocity ,pdata,sizeof(legacyVelocity )); + unpackOne(FLAG_ANGULAR_RATE,&w ,pdata,sizeof(w )); + unpackOne(FLAG_POSITION ,&gp ,pdata,sizeof(gp )); + unpackOne(FLAG_GPSINFO ,&legacyGPSInfo ,pdata,sizeof(legacyGPSInfo )); + unpackOne(FLAG_RTKINFO ,&rtk ,pdata,sizeof(rtk )); + unpackOne(FLAG_MAG ,&mag ,pdata,sizeof(mag )); + unpackOne(FLAG_RC ,&rc ,pdata,sizeof(rc )); + unpackOne(FLAG_GIMBAL ,&gimbal ,pdata,sizeof(gimbal )); + unpackOne(FLAG_STATUS ,&legacyStatus ,pdata,sizeof(legacyStatus )); + unpackOne(FLAG_BATTERY ,&legacyBattery ,pdata,sizeof(legacyBattery )); + unpackOne(FLAG_DEVICE ,&info ,pdata,sizeof(info )); // clang-format on vehicle->protocolLayer->getThreadHandle()->freeMSG(); } @@ -413,7 +490,7 @@ DataBroadcast::unpackOne(DataBroadcast::FLAG flag, void* data, uint8_t*& buf, void DataBroadcast::setVersionDefaults(uint8_t* frequencyBuffer) { - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100()) { setFreqDefaults(frequencyBuffer); } @@ -473,7 +550,7 @@ DataBroadcast::setFreqDefaultsM100_31(uint8_t* freq) void DataBroadcast::setFreqDefaults(uint8_t* freq) { - /* Channels definition for A3/N3 + /* Channels definition for A3/N3/M600 * 0 - Timestamp * 1 - Attitude Quaterniouns * 2 - Acceleration diff --git a/osdk-core/api/src/dji_control.cpp b/osdk-core/api/src/dji_control.cpp index f845a3a49..9a6014882 100644 --- a/osdk-core/api/src/dji_control.cpp +++ b/osdk-core/api/src/dji_control.cpp @@ -41,21 +41,30 @@ Control::action(const int cmd, VehicleCallBack callback, UserData userData) vehicle->nbUserData[cbIndex] = NULL; } - if (vehicle->getFwVersion() != Version::M100_31) + // Check which version of firmware we are dealing with + if (vehicle->isLegacyM600()) + { + legacyCMDData.cmd = cmd; + legacyCMDData.sequence++; + vehicle->protocolLayer->send( + 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, + (uint8_t*)&legacyCMDData, sizeof(legacyCMDData), 500, 2, true, cbIndex); + } + else if (vehicle->isM100()) + { + legacyCMDData.cmd = cmd; + legacyCMDData.sequence++; + vehicle->protocolLayer->send( + 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, + (uint8_t*)&legacyCMDData, sizeof(legacyCMDData), 500, 2, true, cbIndex); + } + else { uint8_t data = cmd; vehicle->protocolLayer->send(2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, &data, sizeof(data), 500, 2, true, cbIndex); } - else - { - m100CMDData.cmd = cmd; - m100CMDData.sequence++; - vehicle->protocolLayer->send( - 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, - (uint8_t*)&m100CMDData, sizeof(m100CMDData), 500, 2, true, cbIndex); - } } ACK::ErrorCode @@ -63,21 +72,29 @@ Control::action(const int cmd, int timeout) { ACK::ErrorCode ack; - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) + { + legacyCMDData.cmd = cmd; + legacyCMDData.sequence++; + vehicle->protocolLayer->send( + 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, + (uint8_t*)&legacyCMDData, sizeof(legacyCMDData), 500, 2, false, 2); + } + else if (vehicle->isM100()) + { + legacyCMDData.cmd = cmd; + legacyCMDData.sequence++; + vehicle->protocolLayer->send( + 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, + (uint8_t*)&legacyCMDData, sizeof(legacyCMDData), 100, 3, false, 2); + } + else { uint8_t data = cmd; vehicle->protocolLayer->send(2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, &data, sizeof(data), 500, 2, false, 2); } - else - { - m100CMDData.cmd = cmd; - m100CMDData.sequence++; - vehicle->protocolLayer->send( - 2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::task, - (uint8_t*)&m100CMDData, sizeof(m100CMDData), 100, 3, false, 2); - } ack = *((ACK::ErrorCode*)vehicle->waitForACK( OpenProtocol::CMDSet::Control::task, timeout)); @@ -126,130 +143,170 @@ Control::setArm(bool armSetting, int timeout) ACK::ErrorCode Control::armMotors(int wait_timeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - return this->action(FlightCommand::startMotor, wait_timeout); + return this->setArm(true, wait_timeout); } - else + else if (vehicle->isM100()) { return this->setArm(true, wait_timeout); } + else + { + return this->action(FlightCommand::startMotor, wait_timeout); + } } void Control::armMotors(VehicleCallBack callback, UserData userData) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - this->action(FlightCommand::startMotor, callback, userData); + this->setArm(true, callback, userData); } - else + else if (vehicle->isM100()) { this->setArm(true, callback, userData); } + else + { + this->action(FlightCommand::startMotor, callback, userData); + } } ACK::ErrorCode Control::disArmMotors(int wait_timeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - return this->action(FlightCommand::stopMotor, wait_timeout); + return this->setArm(false, wait_timeout); } - else + else if (vehicle->isM100()) { return this->setArm(false, wait_timeout); } + else + { + return this->action(FlightCommand::stopMotor, wait_timeout); + } } void Control::disArmMotors(VehicleCallBack callback, UserData userData) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - this->action(FlightCommand::stopMotor, callback, userData); + this->setArm(false, callback, userData); } - else + else if (vehicle->isM100()) { this->setArm(false, callback, userData); } + else + { + this->action(FlightCommand::stopMotor, callback, userData); + } } ACK::ErrorCode Control::takeoff(int wait_timeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - return this->action(FlightCommand::takeOff, wait_timeout); + return this->action(FlightCommand::LegacyCMD::takeOff, wait_timeout); + } + else if (vehicle->isM100()) + { + return this->action(FlightCommand::LegacyCMD::takeOff, wait_timeout); } else { - return this->action(FlightCommand::M100CMD::takeOff, wait_timeout); + return this->action(FlightCommand::takeOff, wait_timeout); } } void Control::takeoff(VehicleCallBack callback, UserData userData) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - this->action(FlightCommand::takeOff, callback, userData); + this->action(FlightCommand::LegacyCMD::takeOff, callback, userData); + } + else if (vehicle->isM100()) + { + this->action(FlightCommand::LegacyCMD::takeOff, callback, userData); } else { - this->action(FlightCommand::M100CMD::takeOff, callback, userData); + this->action(FlightCommand::takeOff, callback, userData); } } ACK::ErrorCode Control::goHome(int wait_timeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - return this->action(FlightCommand::goHome, wait_timeout); + return this->action(FlightCommand::LegacyCMD::goHome, wait_timeout); + } + else if (vehicle->isM100()) + { + return this->action(FlightCommand::LegacyCMD::goHome, wait_timeout); } else { - return this->action(FlightCommand::M100CMD::goHome, wait_timeout); + return this->action(FlightCommand::goHome, wait_timeout); } } void Control::goHome(VehicleCallBack callback, UserData userData) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - this->action(FlightCommand::goHome, callback, userData); + this->action(FlightCommand::LegacyCMD::goHome, callback, userData); + } + else if (vehicle->isM100()) + { + this->action(FlightCommand::LegacyCMD::goHome, callback, userData); } else { - this->action(FlightCommand::M100CMD::goHome, callback, userData); + this->action(FlightCommand::goHome, callback, userData); } } ACK::ErrorCode Control::land(int wait_timeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - return this->action(FlightCommand::landing, wait_timeout); + return this->action(FlightCommand::LegacyCMD::landing, wait_timeout); + } + else if (vehicle->isM100()) + { + return this->action(FlightCommand::LegacyCMD::landing, wait_timeout); } else { - return this->action(FlightCommand::M100CMD::landing, wait_timeout); + return this->action(FlightCommand::landing, wait_timeout); } } void Control::land(VehicleCallBack callback, UserData userData) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->isLegacyM600()) { - this->action(FlightCommand::landing, callback, userData); + this->action(FlightCommand::LegacyCMD::landing, callback, userData); + } + else if (vehicle->isM100()) + { + this->action(FlightCommand::LegacyCMD::landing, callback, userData); } else { - this->action(FlightCommand::M100CMD::landing, callback, userData); + this->action(FlightCommand::landing, callback, userData); } } @@ -264,7 +321,7 @@ Control::flightCtrl(CtrlData data) void Control::flightCtrl(AdvancedCtrlData data) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->getFwVersion() > extendedVersionBase) { vehicle->protocolLayer->send( 0, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::control, @@ -272,14 +329,13 @@ Control::flightCtrl(AdvancedCtrlData data) } else { - if (vehicle->getHwVersion() == "M100") + if (strcmp(vehicle->getHwVersion(), "M100") == 0) { DERROR("Advanced flight control not supported on Matrice 100!\n"); } else { - DERROR("Advanced flight control not supported. Upgrade firmware to 3.3 " - "or higher!\n"); + DERROR("This advanced Flight Control feature is only supported on newer version of firmware.\n"); } } } @@ -324,7 +380,7 @@ void Control::angularRateAndVertPosCtrl(float32_t rollRate, float32_t pitchRate, float32_t yawRate, float32_t z) { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->getFwVersion() > extendedVersionBase) { //! @note 218 is the flag value of this mode uint8_t ctrl_flag = (VERTICAL_POSITION | HORIZONTAL_ANGULAR_RATE | @@ -335,14 +391,13 @@ Control::angularRateAndVertPosCtrl(float32_t rollRate, float32_t pitchRate, } else { - if (vehicle->getHwVersion() == "M100") + if (strcmp(vehicle->getHwVersion(), "M100") == 0) { DERROR("angularRateAndVertPosCtrl not supported on Matrice 100!\n"); } else { - DERROR("angularRateAndVertPosCtrl not supported. Upgrade firmware to 3.3 " - "or higher!\n"); + DERROR("angularRateAndVertPosCtrl is only supported on newer firmware.\n"); } } } @@ -350,7 +405,7 @@ Control::angularRateAndVertPosCtrl(float32_t rollRate, float32_t pitchRate, void Control::emergencyBrake() { - if (vehicle->getFwVersion() != Version::M100_31) + if (vehicle->getFwVersion() > extendedVersionBase) { //! @note 75 is the flag value of this mode AdvancedCtrlData data(72, 0, 0, 0, 0, 0, 0); @@ -359,14 +414,14 @@ Control::emergencyBrake() } else { - if (vehicle->getHwVersion() == "M100") + if (strcmp(vehicle->getHwVersion(), "M100") == 0) { DERROR("emergencyBrake not supported on Matrice 100!\n"); } else { DERROR( - "emergencyBrake not supported. Upgrade firmware to 3.3 or higher!\n"); + "emergencyBrake is only supported on newer firmware for your product.\n"); } } } diff --git a/osdk-core/api/src/dji_error.cpp b/osdk-core/api/src/dji_error.cpp index 0dfa64688..896fa4fb1 100644 --- a/osdk-core/api/src/dji_error.cpp +++ b/osdk-core/api/src/dji_error.cpp @@ -176,8 +176,8 @@ const uint16_t DJI::OSDK::ErrorCode::ControlACK::Task::GIMBAL_MOUNTED = 0x0012; const uint16_t DJI::OSDK::ErrorCode::ControlACK::Task::BAD_SENSOR = 0x0013; const uint16_t DJI::OSDK::ErrorCode::ControlACK::Task::ALREADY_PACKED = 0x0014; const uint16_t DJI::OSDK::ErrorCode::ControlACK::Task::NO_PACKED = 0x0015; -const uint16_t DJI::OSDK::ErrorCode::ControlACK::M100Task::SUCCESS = 0x0002; -const uint16_t DJI::OSDK::ErrorCode::ControlACK::M100Task::FAIL = 0x0001; +const uint16_t DJI::OSDK::ErrorCode::ControlACK::LegacyTask::SUCCESS = 0x0002; +const uint16_t DJI::OSDK::ErrorCode::ControlACK::LegacyTask::FAIL = 0x0001; const uint16_t DJI::OSDK::ErrorCode::ControlACK::Task::PACKED_MODE_NOT_SUPPORTED = 0x0016; const uint16_t DJI::OSDK::ErrorCode::ControlACK::SetArm::AIRCRAFT_IN_AIR_ERROR = 0x0003; const uint16_t DJI::OSDK::ErrorCode::ControlACK::SetArm::ALREADY_ARMED_ERROR = 0x0002; diff --git a/osdk-core/api/src/dji_vehicle.cpp b/osdk-core/api/src/dji_vehicle.cpp index 61d3c8ff9..7cb14149a 100644 --- a/osdk-core/api/src/dji_vehicle.cpp +++ b/osdk-core/api/src/dji_vehicle.cpp @@ -71,7 +71,7 @@ Vehicle::mandatorySetUp() { // We only need a buffer of recvContainers if we are using threads this->nbCallbackRecvContainer = new RecvContainer[200]; - this->circularBuffer = new CircularBuffer(); + this->circularBuffer = new CircularBuffer(); } /* @@ -102,7 +102,6 @@ Vehicle::mandatorySetUp() } } - void Vehicle::functionalSetUp() { @@ -111,8 +110,8 @@ Vehicle::functionalSetUp() DERROR("Failed to initialize Version! Please exit.\n"); return; } - else if(this->getFwVersion() < extendedVersionBase && - this->getFwVersion() != Version::M100_31) + else if (this->getFwVersion() < extendedVersionBase && + this->getFwVersion() != Version::M100_31 && !(this->isLegacyM600())) { DERROR("Upgrade firmware using Assistant software!\n"); return; @@ -279,7 +278,6 @@ Vehicle::initOpenProtocol() return true; } - bool Vehicle::initPlatformSupport() { @@ -296,8 +294,10 @@ Vehicle::initPlatformSupport() QThread* qReadThread = new QThread; readThreadPtr->setQThreadPtr(qReadThread); readThreadPtr->moveToThread(qReadThread); - QObject::connect(qReadThread, SIGNAL(started()), readThreadPtr, SLOT(run())); - QObject::connect(qReadThread, SIGNAL(finished()), qReadThread, SLOT(deleteLater())); + QObject::connect(qReadThread, SIGNAL(started()), readThreadPtr, + SLOT(run())); + QObject::connect(qReadThread, SIGNAL(finished()), qReadThread, + SLOT(deleteLater())); qReadThread->start(); this->readThread = readThreadPtr; } @@ -313,7 +313,8 @@ Vehicle::initPlatformSupport() cbThreadPtr->setQThreadPtr(qCbThread); cbThreadPtr->moveToThread(qCbThread); QObject::connect(qCbThread, SIGNAL(started()), cbThreadPtr, SLOT(run())); - QObject::connect(qCbThread, SIGNAL(finished()), qCbThread, SLOT(deleteLater())); + QObject::connect(qCbThread, SIGNAL(finished()), qCbThread, + SLOT(deleteLater())); qCbThread->start(); this->callbackThread = cbThreadPtr; } @@ -666,27 +667,36 @@ bool Vehicle::initGimbal() { // Gimbal information via subscription - Telemetry::TypeMap::type - subscriptionGimbal; + Telemetry::TypeMap::type subscriptionGimbal; - if(this->getFwVersion() != Version::M100_31) + if (isLegacyM600()) + { + this->gimbal = new (std::nothrow) Gimbal(this); + if (this->gimbal == 0) + { + DERROR("Failed to allocate memory for Gimbal!\n"); + return false; + } + return true; + } + else if (this->getFwVersion() != Version::M100_31) { ACK::ErrorCode ack = this->subscribe->verify(wait_timeout); - if(ACK::getError(ack)) + if (ACK::getError(ack)) { DERROR("Failed to verify subscription!\n"); return false; } Telemetry::TopicName topicList0[] = { Telemetry::TOPIC_GIMBAL_STATUS }; - int nTopic0 = sizeof(topicList0) / sizeof(topicList0[0]); + int nTopic0 = sizeof(topicList0) / sizeof(topicList0[0]); bool result = this->subscribe->initPackageFromTopicList(0, nTopic0, topicList0, 0, 50); if (result) { ack = this->subscribe->startPackage(0, wait_timeout); - if(ACK::getError(ack)) + if (ACK::getError(ack)) { DERROR("Failed to start subscription package!\n"); } @@ -697,7 +707,7 @@ Vehicle::initGimbal() return false; } - // Wait for telemetry data +// Wait for telemetry data #ifdef QT QThread::msleep(200); #elif STM32 @@ -706,7 +716,7 @@ Vehicle::initGimbal() sleep(2); #endif subscriptionGimbal = - this->subscribe->getValue(); + this->subscribe->getValue(); this->subscribe->removePackage(0, wait_timeout); #ifdef QT @@ -718,8 +728,8 @@ Vehicle::initGimbal() #endif } - if((this->getFwVersion() != Version::M100_31 && - subscriptionGimbal.mountStatus == GIMBAL_MOUNTED) || + if ((this->getFwVersion() != Version::M100_31 && + subscriptionGimbal.mountStatus == GIMBAL_MOUNTED) || this->getFwVersion() == Version::M100_31) { this->gimbal = new (std::nothrow) Gimbal(this); @@ -838,6 +848,16 @@ Vehicle::isCmdSetSupported(const uint8_t cmdSet) return false; } } + else if (isLegacyM600()) + { + // CMDs not supported in Matrice 600 old firmware + if (cmdSet == OpenProtocol::CMDSet::hardwareSync || + cmdSet == OpenProtocol::CMDSet::mfio || + cmdSet == OpenProtocol::CMDSet::subscribe) + { + return false; + } + } } } return true; @@ -861,7 +881,7 @@ Vehicle::processReceivedData(RecvContainer receivedFrame) if (threadSupported) { this->nbCallbackRecvContainer[receivedFrame.dispatchInfo.callbackID] = - receivedFrame; + receivedFrame; protocolLayer->getThreadHandle()->lockNonBlockCBAck(); this->circularBuffer->cbPush( this->circularBuffer, this->nbVehicleCallBackHandler, @@ -870,9 +890,7 @@ Vehicle::processReceivedData(RecvContainer receivedFrame) } else this->nbVehicleCallBackHandler.callback( - this, - receivedFrame, - this->nbVehicleCallBackHandler.userData); + this, receivedFrame, this->nbVehicleCallBackHandler.userData); } else @@ -982,7 +1000,7 @@ Vehicle::getDroneVersion(VehicleCallBack callback, UserData userData) OpenProtocol::ErrorCode::CommonACK::NO_RESPONSE_ERROR; versionData.version_crc = 0x0; versionData.version_name[0] = 0; - versionData.fwVersion = 0; + versionData.fwVersion = 0; uint32_t cmd_timeout = 100; // unit is ms uint32_t retry_time = 3; @@ -1176,8 +1194,7 @@ Vehicle::controlAuthorityCallback(Vehicle* vehiclePtr, RecvContainer recvFrame, } } -/*****************************Set State - * Data**************************************/ +/*****************************Set State Data**************************************/ void Vehicle::setVersion(const Version::FirmWare& value) @@ -1246,11 +1263,11 @@ Vehicle::ACKHandler(void* eventData) hotpointStartACK.maxRadius = ackData->recvData.hpStartACK.maxRadius; } else if (memcmp(cmd, OpenProtocol::CMDSet::Mission::hotpointDownload, - sizeof(cmd)) == 0) + sizeof(cmd)) == 0) { - hotpointReadACK.ack.info = ackData->recvInfo; - hotpointReadACK.ack.data = ackData->recvData.hpReadACK.ack; - hotpointReadACK.data = ackData->recvData.hpReadACK.data; + hotpointReadACK.ack.info = ackData->recvInfo; + hotpointReadACK.ack.data = ackData->recvData.hpReadACK.ack; + hotpointReadACK.data = ackData->recvData.hpReadACK.data; } else { @@ -1452,7 +1469,7 @@ Vehicle::waitForACK(const uint8_t (&cmd)[OpenProtocol::MAX_CMD_ARRAY_SIZE], pACK = static_cast(&this->hotpointStartACK); } else if (memcmp(cmd, OpenProtocol::CMDSet::Mission::hotpointDownload, - sizeof(cmd)) == 0) + sizeof(cmd)) == 0) { pACK = static_cast(&this->hotpointReadACK); } @@ -1499,17 +1516,17 @@ ACK::ErrorCode Vehicle::obtainCtrlAuthority(int timeout) { ACK::ErrorCode ack; - uint8_t data = 1; + uint8_t data = 1; protocolLayer->send(2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::setControl, &data, 1, 500, 2, false, 0); ack = *(ACK::ErrorCode*)waitForACK(OpenProtocol::CMDSet::Control::setControl, - timeout); + timeout); if (ack.data == OpenProtocol::ErrorCode::ControlACK::SetControl:: - OBTAIN_CONTROL_IN_PROGRESS) + OBTAIN_CONTROL_IN_PROGRESS) { ack = this->obtainCtrlAuthority(timeout); } @@ -1541,17 +1558,17 @@ ACK::ErrorCode Vehicle::releaseCtrlAuthority(int timeout) { ACK::ErrorCode ack; - uint8_t data = 0; + uint8_t data = 0; protocolLayer->send(2, DJI::OSDK::encrypt, OpenProtocol::CMDSet::Control::setControl, &data, 1, 500, 2, false, 1); ack = *(ACK::ErrorCode*)waitForACK(OpenProtocol::CMDSet::Control::setControl, - timeout); + timeout); if (ack.data == OpenProtocol::ErrorCode::ControlACK::SetControl:: - RELEASE_CONTROL_IN_PROGRESS) + RELEASE_CONTROL_IN_PROGRESS) { ack = this->releaseCtrlAuthority(timeout); } @@ -1592,3 +1609,37 @@ Vehicle::getHwSerialNum() const { return (char*)versionData.hw_serial_num; } + +bool +Vehicle::isLegacyM600() +{ + //! Check for the special M600 backwards compatibility + if (versionData.fwVersion == Version::FW(3, 2, 15, 62)) + { + if (strncmp(versionData.hwVersion, "PM820V3", 7) == 0) + { + return true; + } + else + { + return false; + } + } +} + +bool +Vehicle::isM100() +{ + //! Check for the special M600 backwards compatibility + if (versionData.fwVersion == Version::FW(3, 1, 10, 0)) + { + if (strncmp(versionData.hwVersion, "M100", 4) == 0) + { + return true; + } + else + { + return false; + } + } +} \ No newline at end of file diff --git a/sample/Qt/djiosdk-qt-sample/qtosdk.cpp b/sample/Qt/djiosdk-qt-sample/qtosdk.cpp index 81a8e6f1c..4cca37a3d 100644 --- a/sample/Qt/djiosdk-qt-sample/qtosdk.cpp +++ b/sample/Qt/djiosdk-qt-sample/qtosdk.cpp @@ -47,6 +47,13 @@ qtOsdk::readAppIDKey() void qtOsdk::refreshPort() { + QString currentPort; + + if (ui->portSelection->currentText() != "Connect Serial") + { + currentPort = ui->portSelection->currentText(); + } + ui->portSelection->clear(); QList ports = QSerialPortInfo::availablePorts(); QStringList list; @@ -55,9 +62,19 @@ qtOsdk::refreshPort() list.append(ports[i].portName()); } if (ports.length()== 0) - list.append("Connect Serial"); + list.append("Connect Serial"); ui->portSelection->addItems(list); + + if (ports.length() != 0) + { + ui->portSelection->setCurrentIndex(ui->portSelection->findText(currentPort)); + } + + if (currentPort.isEmpty()) + { + ui->portSelection->setCurrentIndex(0); + } } void diff --git a/sample/linux/camera-gimbal/camera_gimbal_sample.cpp b/sample/linux/camera-gimbal/camera_gimbal_sample.cpp index d52c44ac6..68bd4ab7f 100644 --- a/sample/linux/camera-gimbal/camera_gimbal_sample.cpp +++ b/sample/linux/camera-gimbal/camera_gimbal_sample.cpp @@ -67,7 +67,7 @@ gimbalCameraControl(Vehicle* vehicle) * Subscribe to gimbal data not supported in MAtrice 100 */ - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { // Telemetry: Verify the subscription ACK::ErrorCode subscribeStatus; @@ -110,7 +110,7 @@ gimbalCameraControl(Vehicle* vehicle) ", and the accuracy depends on your magnetometer calibration.\n\n"; // Get Gimbal initial values - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { initialAngle.roll = vehicle->subscribe->getValue().y; initialAngle.pitch = vehicle->subscribe->getValue().x; @@ -134,7 +134,7 @@ gimbalCameraControl(Vehicle* vehicle) "incremental control:\n"; // Get current gimbal data to calc precision error in post processing - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentAngle.roll = vehicle->subscribe->getValue().y; currentAngle.pitch = vehicle->subscribe->getValue().x; @@ -150,7 +150,7 @@ gimbalCameraControl(Vehicle* vehicle) gimbal = GimbalContainer(0, 200, 1800, 20, 0, initialAngle, currentAngle); doSetGimbalAngle(vehicle, &gimbal); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentAngle.roll = vehicle->subscribe->getValue().y; currentAngle.pitch = vehicle->subscribe->getValue().x; @@ -176,7 +176,7 @@ gimbalCameraControl(Vehicle* vehicle) gimbal = GimbalContainer(0, -500, 0, 20, 1, initialAngle); doSetGimbalAngle(vehicle, &gimbal); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentAngle.roll = vehicle->subscribe->getValue().y; currentAngle.pitch = vehicle->subscribe->getValue().x; @@ -217,7 +217,7 @@ gimbalCameraControl(Vehicle* vehicle) usleep(incrementMs * 1000); } - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentAngle.roll = vehicle->subscribe->getValue().y; currentAngle.pitch = vehicle->subscribe->getValue().x; @@ -237,7 +237,7 @@ gimbalCameraControl(Vehicle* vehicle) gimbal = GimbalContainer(0, 0, 0, 20, 1, initialAngle); doSetGimbalAngle(vehicle, &gimbal); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentAngle.roll = vehicle->subscribe->getValue().y; currentAngle.pitch = vehicle->subscribe->getValue().x; @@ -258,7 +258,7 @@ gimbalCameraControl(Vehicle* vehicle) std::cout << "Check DJI GO App or SD card for a new video.\n"; // Cleanup and exit gimbal sample - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, responseTimeout); diff --git a/sample/linux/common/dji_linux_helpers.cpp b/sample/linux/common/dji_linux_helpers.cpp index 2e42109da..a5a94731f 100644 --- a/sample/linux/common/dji_linux_helpers.cpp +++ b/sample/linux/common/dji_linux_helpers.cpp @@ -66,8 +66,9 @@ setupOSDK(int argc, char** argv) // Check if drone version is okay if (vehicle->getFwVersion() < extendedVersionBase && - vehicle->getFwVersion() != Version::M100_31) + (!vehicle->isM100()) && !vehicle->isLegacyM600()) { + delete(vehicle); return NULL; } diff --git a/sample/linux/flight-control/flight_control_sample.cpp b/sample/linux/flight-control/flight_control_sample.cpp index ea9a67b9f..5065ac6ad 100644 --- a/sample/linux/flight-control/flight_control_sample.cpp +++ b/sample/linux/flight-control/flight_control_sample.cpp @@ -84,7 +84,7 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) char func[50]; int pkgIndex; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { // Telemetry: Verify the subscription ACK::ErrorCode subscribeStatus; @@ -96,7 +96,7 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) } // Telemetry: Subscribe to flight status and mode at freq 10 Hz - pkgIndex = 0; + pkgIndex = 0; int freq = 10; TopicName topicList10Hz[] = { TOPIC_STATUS_FLIGHT, TOPIC_STATUS_DISPLAYMODE }; @@ -131,7 +131,7 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) int motorsNotStarted = 0; int timeoutCycles = 20; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (vehicle->subscribe->getValue() != VehicleStatus::FlightStatus::ON_GROUND && @@ -147,9 +147,9 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) { std::cout << "Takeoff failed. Motors are not spinning." << std::endl; // Cleanup - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { - vehicle->subscribe->removePackage(0, timeout); + vehicle->subscribe->removePackage(0, timeout); } return false; } @@ -158,9 +158,24 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) std::cout << "Motors spinning...\n"; } } - else + else if (vehicle->isLegacyM600()) { - while ((vehicle->broadcast->getStatus().flight != + while ((vehicle->broadcast->getStatus().flight < + DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND) && + motorsNotStarted < timeoutCycles) + { + motorsNotStarted++; + usleep(100000); + } + + if (motorsNotStarted < timeoutCycles) + { + std::cout << "Successful TakeOff!" << std::endl; + } + } + else // M100 + { + while ((vehicle->broadcast->getStatus().flight < DJI::OSDK::VehicleStatus::M100FlightStatus::TAKEOFF) && motorsNotStarted < timeoutCycles) { @@ -168,7 +183,7 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) usleep(100000); } - if(motorsNotStarted == timeoutCycles) + if (motorsNotStarted < timeoutCycles) { std::cout << "Successful TakeOff!" << std::endl; } @@ -178,7 +193,7 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) int stillOnGround = 0; timeoutCycles = 110; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (vehicle->subscribe->getValue() != VehicleStatus::FlightStatus::IN_AIR && @@ -194,22 +209,37 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) if (stillOnGround == timeoutCycles) { - std::cout << "Takeoff failed. Aircraft is still on the ground, but the " - "motors are spinning." - << std::endl; - // Cleanup - if (vehicle->getFwVersion() != Version::M100_31) - { - vehicle->subscribe->removePackage(0, timeout); - } - return false; + std::cout << "Takeoff failed. Aircraft is still on the ground, but the " + "motors are spinning." + << std::endl; + // Cleanup + if (!vehicle->isM100() && !vehicle->isLegacyM600()) + { + vehicle->subscribe->removePackage(0, timeout); + } + return false; } else { - std::cout << "Ascending...\n"; + std::cout << "Ascending...\n"; } } - else + else if (vehicle->isLegacyM600()) + { + while ((vehicle->broadcast->getStatus().flight < + DJI::OSDK::VehicleStatus::FlightStatus::IN_AIR) && + stillOnGround < timeoutCycles) + { + stillOnGround++; + usleep(100000); + } + + if (stillOnGround < timeoutCycles) + { + std::cout << "Aircraft in air!" << std::endl; + } + } + else // M100 { while ((vehicle->broadcast->getStatus().flight != DJI::OSDK::VehicleStatus::M100FlightStatus::IN_AIR_STANDBY) && @@ -219,14 +249,14 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) usleep(100000); } - if(stillOnGround == timeoutCycles) + if (stillOnGround < timeoutCycles) { std::cout << "Aircraft in air!" << std::endl; } } // Final check: Finished takeoff - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (vehicle->subscribe->getValue() == VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || @@ -236,44 +266,45 @@ monitoredTakeoff(Vehicle* vehicle, int timeout) sleep(1); } - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { if (vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_P_GPS || - vehicle->subscribe->getValue() != - VehicleStatus::DisplayMode::MODE_ATTITUDE) + VehicleStatus::DisplayMode::MODE_P_GPS || + vehicle->subscribe->getValue() != + VehicleStatus::DisplayMode::MODE_ATTITUDE) { - std::cout << "Successful takeoff!\n"; + std::cout << "Successful takeoff!\n"; } else { - std::cout - << "Takeoff finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; - vehicle->subscribe->removePackage(0, timeout); - return false; + std::cout + << "Takeoff finished, but the aircraft is in an unexpected mode. " + "Please connect DJI GO.\n"; + vehicle->subscribe->removePackage(0, timeout); + return false; } } } else { - float32_t delta; + float32_t delta; Telemetry::GlobalPosition currentHeight; - Telemetry::GlobalPosition deltaHeight = vehicle->broadcast->getGlobalPosition(); + Telemetry::GlobalPosition deltaHeight = + vehicle->broadcast->getGlobalPosition(); do { sleep(3); currentHeight = vehicle->broadcast->getGlobalPosition(); delta = fabs(currentHeight.altitude - deltaHeight.altitude); - deltaHeight.altitude = currentHeight.altitude; + deltaHeight.altitude = currentHeight.altitude; } while (delta >= 0.009); std::cout << "Aircraft hovering at " << currentHeight.altitude << "m!\n"; } // Cleanup - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, timeout); if (ACK::getError(ack)) @@ -315,7 +346,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, //@todo: remove this once the getErrorCode function signature changes char func[50]; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { // Telemetry: Verify the subscription ACK::ErrorCode subscribeStatus; @@ -366,7 +397,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, // Convert position offset from first position to local coordinates Telemetry::Vector3f localOffset; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { currentSubscriptionGPS = vehicle->subscribe->getValue(); originSubscriptionGPS = currentSubscriptionGPS; @@ -400,7 +431,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, Telemetry::Quaternion broadcastQ; double yawInRad; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { subscriptionQ = vehicle->subscribe->getValue(); yawInRad = toEulerAngle((static_cast(&subscriptionQ))).z / DEG2RAD; @@ -422,7 +453,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, // while x and y are in relative float zDeadband = 0.12; - if(vehicle->getFwVersion() == Version::M100_31) + if (vehicle->isM100() || vehicle->isLegacyM600()) { zDeadband = 0.12 * 10; } @@ -448,7 +479,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, else yCmd = 0; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { zCmd = currentSubscriptionGPS.altitude + zOffsetDesired; } @@ -468,7 +499,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, elapsedTimeInMs += cycleTimeInMs; //! Get current position in required coordinates and units - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { subscriptionQ = vehicle->subscribe->getValue(); yawInRad = toEulerAngle((static_cast(&subscriptionQ))).z; @@ -498,18 +529,17 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, if (std::abs(yOffsetRemaining) < speedFactor) yCmd = yOffsetRemaining; - if(vehicle->getFwVersion() == Version::M100_31 && - std::abs(xOffsetRemaining) < posThresholdInM && - std::abs(yOffsetRemaining) < posThresholdInM && - std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) + if (vehicle->isM100() && std::abs(xOffsetRemaining) < posThresholdInM && + std::abs(yOffsetRemaining) < posThresholdInM && + std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) { //! 1. We are within bounds; start incrementing our in-bound counter withinBoundsCounter += cycleTimeInMs; } - else if(std::abs(xOffsetRemaining) < posThresholdInM && - std::abs(yOffsetRemaining) < posThresholdInM && - std::abs(zOffsetRemaining) < zDeadband && - std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) + else if (std::abs(xOffsetRemaining) < posThresholdInM && + std::abs(yOffsetRemaining) < posThresholdInM && + std::abs(zOffsetRemaining) < zDeadband && + std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad) { //! 1. We are within bounds; start incrementing our in-bound counter withinBoundsCounter += cycleTimeInMs; @@ -537,7 +567,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, //! Set velocity to zero, to prevent any residual velocity from position //! command - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (brakeCounter < withinControlBoundsTimeReqmt) { @@ -550,7 +580,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, if (elapsedTimeInMs >= timeoutInMilSec) { std::cout << "Task timeout!\n"; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, responseTimeout); @@ -563,7 +593,7 @@ moveByPositionOffset(Vehicle* vehicle, float xOffsetDesired, return ACK::FAIL; } - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, responseTimeout); @@ -591,7 +621,7 @@ monitoredLanding(Vehicle* vehicle, int timeout) char func[50]; int pkgIndex; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { // Telemetry: Verify the subscription ACK::ErrorCode subscribeStatus; @@ -638,7 +668,7 @@ monitoredLanding(Vehicle* vehicle, int timeout) int landingNotStarted = 0; int timeoutCycles = 20; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (vehicle->subscribe->getValue() != VehicleStatus::DisplayMode::MODE_AUTO_LANDING && @@ -648,7 +678,7 @@ monitoredLanding(Vehicle* vehicle, int timeout) usleep(100000); } } - else + else if (vehicle->isM100()) { while (vehicle->broadcast->getStatus().flight != DJI::OSDK::VehicleStatus::M100FlightStatus::LANDING && @@ -677,7 +707,7 @@ monitoredLanding(Vehicle* vehicle, int timeout) } // Second check: Finished landing - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { while (vehicle->subscribe->getValue() == VehicleStatus::DisplayMode::MODE_AUTO_LANDING && @@ -708,7 +738,34 @@ monitoredLanding(Vehicle* vehicle, int timeout) return false; } } - else + else if (vehicle->isLegacyM600()) + { + while (vehicle->broadcast->getStatus().flight == + DJI::OSDK::VehicleStatus::FlightStatus::STOPED) + { + sleep(1); + } + + Telemetry::GlobalPosition gp; + do + { + sleep(2); + gp = vehicle->broadcast->getGlobalPosition(); + } while (gp.altitude != 0); + + if (gp.altitude != 0) + { + std::cout + << "Landing finished, but the aircraft is in an unexpected mode. " + "Please connect DJI GO.\n"; + return false; + } + else + { + std::cout << "Successful landing!\n"; + } + } + else // M100 { while (vehicle->broadcast->getStatus().flight == DJI::OSDK::VehicleStatus::M100FlightStatus::FINISHING_LANDING) @@ -723,11 +780,11 @@ monitoredLanding(Vehicle* vehicle, int timeout) gp = vehicle->broadcast->getGlobalPosition(); } while (gp.altitude != 0); - if(gp.altitude != 0) + if (gp.altitude != 0) { std::cout - << "Landing finished, but the aircraft is in an unexpected mode. " - "Please connect DJI GO.\n"; + << "Landing finished, but the aircraft is in an unexpected mode. " + "Please connect DJI GO.\n"; return false; } else @@ -737,7 +794,7 @@ monitoredLanding(Vehicle* vehicle, int timeout) } // Cleanup - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, timeout); if (ACK::getError(ack)) @@ -768,7 +825,7 @@ localOffsetFromGpsOffset(Vehicle* vehicle, Telemetry::Vector3f& deltaNed, double deltaLon; double deltaLat; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { subscriptionTarget = (Telemetry::GPSFused*)target; subscriptionOrigin = (Telemetry::GPSFused*)origin; diff --git a/sample/linux/missions/mission_sample.cpp b/sample/linux/missions/mission_sample.cpp index 8ead7a5da..e15d575e0 100644 --- a/sample/linux/missions/mission_sample.cpp +++ b/sample/linux/missions/mission_sample.cpp @@ -133,7 +133,7 @@ teardownSubscription(DJI::OSDK::Vehicle* vehicle, const int pkgIndex, bool runWaypointMission(Vehicle* vehicle, uint8_t numWaypoints, int responseTimeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { if (!setUpSubscription(vehicle, responseTimeout)) { @@ -186,7 +186,7 @@ runWaypointMission(Vehicle* vehicle, uint8_t numWaypoints, int responseTimeout) // Cleanup before return. The mission isn't done yet, but it doesn't need any // more input from our side. - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { return teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout); @@ -242,7 +242,7 @@ createWaypoints(DJI::OSDK::Vehicle* vehicle, int numWaypoints, // Global position retrieved via broadcast Telemetry::GlobalPosition broadcastGPosition; - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { subscribeGPosition = vehicle->subscribe->getValue(); start_wp.latitude = subscribeGPosition.latitude; @@ -324,7 +324,7 @@ uploadWaypoints(Vehicle* vehicle, bool runHotpointMission(Vehicle* vehicle, int initialRadius, int responseTimeout) { - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { if (!setUpSubscription(vehicle, responseTimeout)) { @@ -344,7 +344,7 @@ runHotpointMission(Vehicle* vehicle, int initialRadius, int responseTimeout) NULL); vehicle->missionManager->printInfo(); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { subscribeGPosition = vehicle->subscribe->getValue(); vehicle->missionManager->hpMission->setHotPoint( @@ -363,7 +363,7 @@ runHotpointMission(Vehicle* vehicle, int initialRadius, int responseTimeout) { ACK::getErrorCodeMessage(takeoffAck, __func__); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout); } @@ -381,7 +381,7 @@ runHotpointMission(Vehicle* vehicle, int initialRadius, int responseTimeout) if (ACK::getError(startAck)) { ACK::getErrorCodeMessage(startAck, __func__); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout); } @@ -442,7 +442,7 @@ runHotpointMission(Vehicle* vehicle, int initialRadius, int responseTimeout) // Clean up ACK::getErrorCodeMessage(startAck, __func__); - if (vehicle->getFwVersion() != Version::M100_31) + if (!vehicle->isM100() && !vehicle->isLegacyM600()) { teardownSubscription(vehicle, DEFAULT_PACKAGE_INDEX, responseTimeout); } diff --git a/sample/linux/mobile/mobile_sample.cpp b/sample/linux/mobile/mobile_sample.cpp index d3571bd2b..8b07996e7 100644 --- a/sample/linux/mobile/mobile_sample.cpp +++ b/sample/linux/mobile/mobile_sample.cpp @@ -51,26 +51,10 @@ parseFromMobileCallback(Vehicle* vehicle, RecvContainer recvFrame, vehicle->releaseCtrlAuthority(controlAuthorityMobileCallback); break; case 5: - if(vehicle->getFwVersion() != Version::M100_31) - { - vehicle->control->action(Control::FlightCommand::startMotor, - actionMobileCallback); - } - else - { - vehicle->control->armMotors(actionMobileCallback); - } + vehicle->control->armMotors(actionMobileCallback); break; case 6: - if(vehicle->getFwVersion() != Version::M100_31) - { - vehicle->control->action(Control::FlightCommand::stopMotor, - actionMobileCallback); - } - else - { - vehicle->control->disArmMotors(actionMobileCallback); - } + vehicle->control->disArmMotors(actionMobileCallback); break; default: break; @@ -159,10 +143,13 @@ actionMobileCallback(Vehicle* vehiclePtr, RecvContainer recvFrame, ack.info = recvFrame.recvInfo; - if (vehiclePtr->getFwVersion() != Version::M100_31) - ack.data = recvFrame.recvData.commandACK; - else + if (vehiclePtr->isLegacyM600()) + ack.data = recvFrame.recvData.ack; + else if (vehiclePtr->isM100()) ack.data = recvFrame.recvData.ack; + else + ack.data = recvFrame.recvData.commandACK; + // Display ACK message ACK::getErrorCodeMessage(ack, __func__); @@ -172,7 +159,7 @@ actionMobileCallback(Vehicle* vehiclePtr, RecvContainer recvFrame, recvFrame.recvInfo.cmd_id }; // startMotor supported in FW version >= 3.3 - // setArm supported only on Matrice 100 + // setArm supported only on Matrice 100 && M600 old FW if (recvFrame.recvInfo.buf[2] == Control::FlightCommand::startMotor || (memcmp(cmd, OpenProtocol::CMDSet::Control::setArm, sizeof(cmd)) && recvFrame.recvInfo.buf[2] == true)) diff --git a/sample/linux/telemetry/telemetry_sample.cpp b/sample/linux/telemetry/telemetry_sample.cpp index 6c05a4d37..eb4d60cf0 100644 --- a/sample/linux/telemetry/telemetry_sample.cpp +++ b/sample/linux/telemetry/telemetry_sample.cpp @@ -40,7 +40,7 @@ main(int argc, char** argv) switch (inputChar) { case 'a': - if (vehicle->getFwVersion() == Version::M100_31) + if (vehicle->isM100() || vehicle->isLegacyM600()) { getBroadcastData(vehicle); }