Merge pull request #71 in OS/onboard-sdk from adapting_to_advanced_se…

…nsing to develop * commit '05cb9c04263ae0d8c37bf53b917d4bb4606647d9': fix:fix compile issue fix:adapting advanced-sensing
dji-sdk · Dec 18, 2019 · 0fff9c8 · 0fff9c8
2 parents f3d3a39 + 05cb9c0
commit 0fff9c8
Show file tree

Hide file tree

Showing 40 changed files with 3,484 additions and 3 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -25,6 +25,11 @@ project(onboardsdk)
 
 set(CMAKE_VERBOSE_MAKEFILE OFF)
 
+set(DJIOSDK 0)
+set(DJIOSDK_MINOR_VERSION 1)
+set(DJIOSDK_PATCH_VERSION 0)
+set(DJIOSDK_VERSION
+  ${DJIOSDK_MAJOR_VERSION}.${DJIOSDK_MINOR_VERSION}.${DJIOSDK_PATCH_VERSION})
 add_definitions(-DDJIOSDK_MAJOR_VERSION=3)
 add_definitions(-DDJIOSDK_MINOR_VERSION=8)
 add_definitions(-DDJIOSDK_PATCH_VERSION=1)
@@ -49,6 +54,9 @@ endif()
 set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin)
 set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/libs)
 
+if (ADVANCED_SENSING)
+  add_definitions(-DADVANCED_SENSING)
+endif()
 add_subdirectory(osdk-core)
 
 if (${CMAKE_SYSTEM_NAME} MATCHES Linux)

diff --git a/osdk-core/CMakeLists.txt b/osdk-core/CMakeLists.txt
@@ -97,6 +97,46 @@ SET(OSDK_INTERFACE_LIBS pthread) # pthread is assumed available on linux
 SET(MODULE_BUILD_INTERFACE "")
 SET(MODULE_INSTALL_INTERFACE "")
 
+## Advanced Sensing
+if(ADVANCED_SENSING)
+  # Add a cmake file to find libusb
+  set(CMAKE_MODULE_PATH ${CURRENT_CMAKE_MODULE_PATH})
+
+  find_package(LibUSB REQUIRED)
+  find_package(AdvancedSensing QUIET)
+  if(NOT ADVANCED_SENSING_FOUND)
+    include(${CURRENT_CMAKE_MODULE_PATH}/External_AdvancedSensing.cmake)
+    add_dependencies(${PROJECT_NAME} advanced-sensing)
+  endif()
+
+  find_package(FFMPEG REQUIRED)
+
+  if(FFMPEG_FOUND)
+      message( STATUS "Found FFmpeg ${FFMPEG_VERSION} installed in the system.")
+      message( STATUS " - Includes: ${FFMPEG_INCLUDE_DIRS}")
+      message( STATUS " - Libraries: ${FFMPEG_LIBRARIES}")
+  else()
+    message("Cannot Find FFMPEG")
+  endif(FFMPEG_FOUND)
+
+  target_include_directories(${PROJECT_NAME} PUBLIC
+                             $<BUILD_INTERFACE:${ADVANCED_SENSING_INCLUDE_DIRS}>)
+  target_link_libraries(${PROJECT_NAME} PRIVATE ${ADVANCED_SENSING_LIBRARY})
+
+
+  target_include_directories(${PROJECT_NAME} PUBLIC ${LIBUSB_1_INCLUDE_DIRS})
+  target_link_libraries(${PROJECT_NAME} PUBLIC ${LIBUSB_1_LIBRARIES})
+
+  target_include_directories(${PROJECT_NAME} PUBLIC ${FFMPEG_INCLUDE_DIR})
+  target_link_libraries(${PROJECT_NAME} PUBLIC ${FFMPEG_LIBRARIES})
+
+  set(OSDK_INTERFACE_LIBS ${OSDK_INTERFACE_LIBS} ${LIBUSB_1_LIBRARIES} ${FFMPEG_LIBRARIES})
+  set(MODULE_BUILD_INTERFACE ${MODULE_BUILD_INTERFACE} ${ADVANCED_SENSING_LIBRARY})
+  set(MODULE_INSTALL_INTERFACE ${MODULE_INSTALL_INTERFACE} $<INSTALL_PREFIX>/lib/libadvanced-sensing.a)
+
+  target_compile_definitions(${PROJECT_NAME} PUBLIC -DADVANCED_SENSING)
+
+endif()
 ## Once all the modules are done, set the interface_link_libraries property
 set_property(TARGET ${PROJECT_NAME}
         PROPERTY INTERFACE_LINK_LIBRARIES
@@ -170,6 +210,21 @@ FILE(GLOB OSDK_LIB_HEADERS
         linker/core/utils/inc/*.h
         linker/core/root_task/*.h)
 
+# Append advanced sensing headers
+if(ADVANCED_SENSING)
+  install(FILES ${ADVANCED_SENSING_LIBRARY}
+        DESTINATION "${INSTALL_LIB_DIR}" COMPONENT shlib)
+
+  set(OSDK_LIB_HEADERS
+      ${OSDK_LIB_HEADERS}
+      ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_advanced_sensing.hpp
+      ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_advanced_sensing_protocol.hpp
+      ${ADVANCED_SENSING_INCLUDE_DIRS}/linux_usb_device.hpp
+      ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_camera_image.hpp
+      ${ADVANCED_SENSING_INCLUDE_DIRS}/dji_camera_stream.hpp
+      )
+endif()
+
 set_target_properties(${PROJECT_NAME} PROPERTIES
         PUBLIC_HEADER "${OSDK_LIB_HEADERS}")
 

diff --git a/osdk-core/api/inc/dji_ack.hpp b/osdk-core/api/inc/dji_ack.hpp
@@ -262,6 +262,57 @@ class ACK
     uint16_t reserved_1;
   } WayPointStatusPushData;
 
+  /*!
+   * @brief This constant variable defines number of pixels for QVGA images
+   */
+  static const int IMG_240P_SIZE = 240 * 320;
+  typedef uint8_t  Image[IMG_240P_SIZE];
+  /*!
+   * @brief sub-struct for stereo image with raw data and camera name
+   */
+  typedef struct ImageMeta
+  {
+      Image image;
+      char  name[12];
+  } ImageMeta; // pack(1)
+  /*!
+   * @brief This struct captures PushData when subscribe to QVGA images
+   */
+  typedef struct StereoImgData
+  {
+      uint32_t frame_index;
+      uint32_t time_stamp;
+      uint8_t  num_imgs;
+      /*
+       * There could be 50 different kinds of images coming from the drone,
+       * 5 camera pairs and 10 images types.
+       * Here we use an uint64_t to describe which image is coming
+       * from the USB line, each bit represents if there's data or not
+       * Please use AdvancedSensing::ReceivedImgDesc to match them
+       * For M210, we support up to 4 images at the same time
+       */
+      uint64_t img_desc;
+      // @note for M210, at most 4 imgs come at the same time.
+      ImageMeta img_vec[4];
+  } StereoImgData; // pack(1)
+  /*!
+   * @brief This constant variable defines number of pixels for VGA images
+   */
+  static const int IMG_VGA_SIZE = 640 * 480;
+  typedef uint8_t  VGAImage[IMG_VGA_SIZE];
+  /*!
+   * @brief This struct captures PushData when subscribe to VGA images
+   */
+  typedef struct StereoVGAImgData
+  {
+      uint32_t frame_index;
+      uint32_t time_stamp;
+      uint8_t  num_imgs;
+      uint8_t  direction;
+      // @note VGA imgs always come in pair
+      VGAImage img_vec[2];
+  } StereoVGAImgData; // pack(1)
+
   /*!
    * @brief This struct captures PushData when subscribe to UTC & FC time in hardware sync
    */
@@ -307,10 +358,9 @@ class ACK
 
     /*
      * Push Data from AdvancedSensing protocol
-
+     */
     StereoImgData           *stereoImgData;
     StereoVGAImgData        *stereoVGAImgData;
-     */
 
     /*
      * Push Data from GPS or RTK

diff --git a/osdk-core/api/inc/dji_vehicle.hpp b/osdk-core/api/inc/dji_vehicle.hpp
@@ -54,6 +54,9 @@
 #include "dji_gimbal_manager.hpp"
 #include "dji_flight_controller.hpp"
 #include "dji_psdk_manager.hpp"
+#ifdef ADVANCED_SENSING
+#include "dji_advanced_sensing.hpp"
+#endif
 
 namespace DJI
 {
@@ -106,6 +109,9 @@ class Vehicle
   FlightController*    flightController;
   PSDKManager*         psdkManager;
   GimbalManager*       gimbalManager;
+#ifdef ADVANCED_SENSING
+  AdvancedSensing* advancedSensing;
+#endif
 
   int functionalSetUp();
   ////////// Blocking calls ///////////
@@ -259,6 +265,20 @@ class Vehicle
   bool initPSDKManager();
   bool initGimbalManager();
   bool initOSDKHeartBeatThread();
+#ifdef ADVANCED_SENSING
+  bool initAdvancedSensing();
+#endif
+
+#ifdef ADVANCED_SENSING
+  /*! @brief This function takes a frame and calls the right handlers/functions
+   *         based on the nature of the frame (ack, blocking, etc.)
+   * @param receivedFrame: RecvContainer populated by the AdvancedSensing Protocol
+   * @return NULL
+   */
+  void processAdvancedSensingImgs(RecvContainer* receivedFrame);
+
+  bool advSensingErrorPrintOnce;
+#endif
 private:
   void setActivationStatus(bool is_activated);
   void initCMD_SetSupportMatrix();

diff --git a/osdk-core/api/src/dji_vehicle.cpp b/osdk-core/api/src/dji_vehicle.cpp
@@ -81,6 +81,10 @@ Vehicle::Vehicle(Linker* linker)
   , psdkManager(NULL)
   , flightController(NULL)
   , missionManager(NULL)
+#ifdef ADVANCED_SENSING
+  , advancedSensing(NULL)
+  , advSensingErrorPrintOnce(false)
+#endif
 {
   ackErrorCode.data = OpenProtocolCMD::ErrorCode::CommonACK::NO_RESPONSE_ERROR;
 }
@@ -205,6 +209,14 @@ Vehicle::init()
     return false;
   }
 
+#ifdef ADVANCED_SENSING
+  if (!initAdvancedSensing())
+  {
+    DERROR("Failed to initialize AdvancedSensing!\n");
+    return false;
+  }
+#endif
+
   return true;
 }
 
@@ -312,6 +324,12 @@ Vehicle::~Vehicle()
   {
     delete this->flightController;
   }
+
+#ifdef ADVANCED_SENSING
+  if (this->advancedSensing)
+    delete this->advancedSensing;
+#endif
+
   OsdkOsal_TaskDestroy(sendHeartbeatToFCHandle);
 }
 
@@ -693,6 +711,75 @@ Vehicle::initVirtualRC()
   return true;
 }
 
+#ifdef ADVANCED_SENSING
+bool
+Vehicle::initAdvancedSensing()
+{
+  if(this->advancedSensing)
+  {
+    DDEBUG("vehicle->advancedSensing already initalized!");
+    return true;
+  }
+
+  this->advancedSensing = new (std::nothrow) AdvancedSensing(this);
+  if (this->advancedSensing == 0)
+  {
+    DERROR("Failed to allocate memory for AdvancedSensing!\n");
+    return false;
+  }
+  this->advancedSensing->init();
+
+  return true;
+}
+#endif
+
+
+#ifdef ADVANCED_SENSING
+void
+Vehicle::processAdvancedSensingImgs(RecvContainer* receivedFrame)
+{
+  if (receivedFrame->recvInfo.cmd_id == AdvancedSensingProtocol::PROCESS_IMG_CMD_ID)
+  {
+    if (this->advancedSensing->stereoHandler.callback)
+    {
+      this->advancedSensing->stereoHandler.callback(
+          this, *receivedFrame, this->advancedSensing->stereoHandler.userData);
+    }
+    else
+    {
+      this->advancedSensing->unsubscribeStereoImages();
+      if(!advSensingErrorPrintOnce){
+        DERROR("No callback registered for 240p stereo images.\n"
+               "This usually happens when user subscribed to images and restart "
+               "the program without unsubscribing them.\n"
+               "Vehicle unsubscribed 240p stereo images automatically.\n");
+        advSensingErrorPrintOnce = true;
+      }
+    }
+  }
+  else if (receivedFrame->recvInfo.cmd_id ==
+           AdvancedSensingProtocol::PROCESS_VGA_CMD_ID)
+  {
+    if (this->advancedSensing->vgaHandler.callback)
+    {
+      this->advancedSensing->vgaHandler.callback(this, *receivedFrame,
+                                                 this->advancedSensing->vgaHandler.userData);
+    }
+    else
+    {
+      this->advancedSensing->unsubscribeVGAImages();
+      if(!advSensingErrorPrintOnce){
+        DERROR("No callback registered for VGA stereo images.\n"
+               "This usually happens when user subscribed to images and restart "
+               "the program without unsubscribing them.\n"
+               "Vehicle unsubscribed VGA stereo images automatically.\n");
+        advSensingErrorPrintOnce = true;
+      }
+    }
+  }
+}
+#endif
+
 bool
 Vehicle::initOSDKHeartBeatThread() {
     /*! create task for OSDK heart beat */

diff --git a/sample/platform/linux/CMakeLists.txt b/sample/platform/linux/CMakeLists.txt
@@ -73,4 +73,10 @@ add_subdirectory(telemetry)
 add_subdirectory(logging)
 add_subdirectory(time-sync)
 add_subdirectory(payload-3rd-party)
-add_subdirectory(payloads)
+add_subdirectory(payloads)
+
+if(ADVANCED_SENSING)
+    add_definitions(-DADVANCED_SENSING)
+    message(STATUS "AdvancedSensing sample is enabled")
+    add_subdirectory(advanced-sensing)
+endif()
diff --git a/sample/platform/linux/advanced-sensing/CMakeLists.txt b/sample/platform/linux/advanced-sensing/CMakeLists.txt
@@ -0,0 +1,62 @@
+# *  @Copyright (c) 2016-2017 DJI
+# *
+# * Permission is hereby granted, free of charge, to any person obtaining a copy
+# * of this software and associated documentation files (the "Software"), to deal
+# * in the Software without restriction, including without limitation the rights
+# * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+# * copies of the Software, and to permit persons to whom the Software is
+# * furnished to do so, subject to the following conditions:
+# *
+# * The above copyright notice and this permission notice shall be included in
+# * all copies or substantial portions of the Software.
+# *
+# * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+# * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+# * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+# * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+# * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+# * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+# * SOFTWARE.
+# *
+# *
+
+cmake_minimum_required(VERSION 2.8)
+project(djiosdk-advanced-sensing-sample)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -fPIC -pthread -g -O0")
+
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/osdk-core/cmake-modules/")
+
+find_package(LibUSB REQUIRED)
+include_directories(${LIBUSB_1_INCLUDE_DIRS})
+
+set(HELPER_FUNCTIONS_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../common)
+
+include_directories(${ONBOARDSDK_SOURCE}/api/inc)
+include_directories(${ONBOARDSDK_SOURCE}/utility/inc)
+include_directories(${ONBOARDSDK_SOURCE}/hal/inc)
+include_directories(${ONBOARDSDK_SOURCE}/protocol/inc)
+include_directories(${ONBOARDSDK_SOURCE}/platform/linux/inc)
+include_directories(${HELPER_FUNCTIONS_DIR})
+
+include_directories(${MODULES_HEADER_DIR})
+include_directories(${FLIGHT_MODULES_HEADER_DIR})
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../common)
+
+FILE(GLOB SOURCE_FILES *.hpp *.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/../common/dji_linux_environment.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/../common/dji_linux_helpers.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/../hal/*.c
+        ${CMAKE_CURRENT_SOURCE_DIR}/../osal/*.c
+        ${LINKER_HEADER_SRC}
+        )
+
+add_subdirectory(stereo_vision_single_thread_sample)
+add_subdirectory(stereo_vision_multi_thread_sample)
+add_subdirectory(camera_stream_poll_sample)
+add_subdirectory(camera_stream_callback_sample)
+add_subdirectory(stereo_vision_depth_perception_sample)
+
+if (TARGET_TRACKING_SAMPLE)
+  add_subdirectory(camera_stream_target_tracking_sample)
+endif()