Hotfix Release 3.2.2 Movement control, Vertical Position control bug…

…. User is not required to add home point altitude to Z desired position after this fix. Z desired position may be entered directly. Home point altitude is handled internally

CMakeLists.txt

@@ -4,7 +4,9 @@ project(onboardsdk)
 set(CMAKE_VERBOSE_MAKEFILE OFF)
 
 # Add module path for <name>.cmake conf files
-set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/contrib/)
+if(NOT CMAKE_MODULE_PATH)
+  set(CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/contrib/)
+endif()
 
 include(${CMAKE_MODULE_PATH}/DJIConfig.cmake)
 
@@ -17,8 +19,15 @@ option(GTEST "Build with Google Test framework" OFF)
 # Allow to build additional modules
 option(LIDAR_LOGGING "Build VLP16 Puck LiDAR logging" OFF)
 
-# Use External DJI Modules
+#### Use External DJI Modules ####
+
 option(USE_PRECISION_MISSIONS "Use DJI precision-missions library" OFF)
+# If you are using custom precision-missions library
+option(PRECISION_MISSIONS_LIBRARY "WARNING: for custom precision-missions Library only, point to a library location" PATH)
+option(PRECISION_MISSIONS_INCLUDE_DIR "WARNING: for custom precision-missions library only, point to headers location" PATH)
+# Dependencies include Eigen and csv
+option(PRECISION_MISSIONS_DEPENDENCIES_DIR "WARNING: for custom precision-missions library only, point to dependencies location" PATH)
+
 option(USE_COLLISION_AVOIDANCE "Use DJI collision avoidance library" OFF)
 option(USE_POINTCLOUD2LAS "Use DJI pointcloud to LAS converter library" OFF)
 

contrib/External_DJIModules.cmake

@@ -1,4 +1,7 @@
-if(USE_PRECISION_MISSIONS)
+# Check if custom binaries were given
+find_package(PrecisionMissions QUIET)
+
+if(NOT PrecisionMissions_FOUND AND USE_PRECISION_MISSIONS)
   add_definitions(-DUSE_PRECISION_MISSIONS)
   include(${CMAKE_MODULE_PATH}/External_PrecisionMissions.cmake)
 endif()

contrib/External_PrecisionMissions.cmake

@@ -23,6 +23,5 @@ ExternalProject_Get_Property(PrecisionMissions source_dir)
 
 # Make sure src/bin directory names do not collide
 # with other external projects
-set(precision_missions_source_dir ${source_dir}/precision-missions-${VERSION})
-include_directories("${precision_missions_source_dir}/inc")
+set(precision_missions_source_dir ${source_dir}/precision-missions-${VERSION} CACHE PATH "")
 

contrib/FindPrecisionMissions.cmake

@@ -0,0 +1,30 @@
+# Find Precision Missions library
+find_path(PRECISION_MISSIONS_INCLUDE_DIR
+  OnboardSDK.h
+  HINTS ${PRECISION_MISSIONS_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+find_path(PRECISION_MISSIONS_DEPENDENCIES_DIR
+  Eigen/Dense
+  HINTS ${PRECISION_MISSIONS_DEPENDENCIES_DIR}
+  NO_DEFAULT_PATH)
+
+set(PRECISION_MISSIONS_LIBRARY_NAMES "libmissionplan.a")
+find_library(PRECISION_MISSIONS_LIBRARY
+  NAMES ${LIBLAS_NAMES}
+  HINTS ${PRECISION_MISSIONS_LIBRARY}
+  NO_DEFAULT_PATH)
+
+if(EXISTS ${PRECISION_MISSIONS_INCLUDE_DIR})
+  message(STATUS "Precision Missions include dir set to ${PRECISION_MISSIONS_INCLUDE_DIR}")
+endif()
+
+if(EXISTS ${PRECISION_MISSIONS_DEPENDENCIES_DIR})
+  message(STATUS "Precision Missions dependencies dir set to ${PRECISION_MISSIONS_DEPENDENCIES_DIR}")
+endif()
+
+if(EXISTS ${PRECISION_MISSIONS_LIBRARY})
+  message(STATUS "Precision Missions library set to ${PRECISION_MISSIONS_LIBRARY}")
+  set(PrecisionMissions_FOUND "YES")
+endif()
+

osdk-core/inc/DJI_API.h

@@ -634,7 +634,7 @@ class CoreAPI
   void setGlobalNavTestMobileCMD(bool userInput) {globalNavTestMobileCMD = userInput;}
   void setVRCTestMobileCMD(bool userInput) {VRCTestMobileCMD = userInput;}
 
-
+  float32_t homepointAltitude;
 
   private:
   BroadcastData broadcastData;

osdk-core/src/DJI_API.cpp

@@ -75,6 +75,9 @@ void CoreAPI::init(HardDriver *sDevice, CallBackHandler userRecvCallback, bool u
   versionData.fwVersion = 0; //! Default init value
   ack_activation = 0xFF;
 
+  //! This handles hotfix for Movement Control issue with Z position Control
+  homepointAltitude = 999999;
+
 
   //! @todo simplify code above
   serialDevice->lockMSG();

osdk-core/src/DJI_App.cpp

@@ -11,6 +11,7 @@
 
 #include <string.h>
 #include <stdio.h>
+#include <DJI_Flight.h>
 #include "DJI_App.h"
 #include "DJI_API.h"
 
@@ -75,6 +76,8 @@ void DJI::onboardSDK::CoreAPI::broadcast(Header *protocolHeader)
   enableFlag = (unsigned short *)pdata;
   broadcastData.dataFlag = *enableFlag;
   size_t len = MSG_ENABLE_FLAG_LEN;
+  static int currentState = 0;
+  static int prevState = 0;
 
   //! @warning Change to const (+change interface for passData) in next release
   uint16_t DATA_FLAG = 0x0001;
@@ -121,13 +124,40 @@ void DJI::onboardSDK::CoreAPI::broadcast(Header *protocolHeader)
   passData(*enableFlag, DATA_FLAG, &broadcastData.ctrlInfo, pdata,
       sizeof(CtrlInfoData) - ((versionData.fwVersion < MAKE_VERSION(3,1,0,0)) ? 1 : 0), len);
   serialDevice->freeMSG();
-
   /**
    * Set broadcast frame status
    * @todo Implement proper notification mechanism
    */
   setBroadcastFrameStatus(true);
+  static int counter=0;
 
+  //! State Machine for MSL Altitude bug in A3 and M600
+  //! Handles the case if users start OSDK after arming aircraft (STATUS_ON_GROUND)/after takeoff (STATUS_IN_AIR)
+  //! Transition from STATUS_MOTOR_STOPPED to STATUS_ON_GROUND can be seen with Takeoff command with 1hz flight status data
+  //! Transition from STATUS_ON_GROUND to STATUS_MOTOR_STOPPED can be seen with Landing command only for frequencies >= 50Hz
+  if (strcmp(getHwVersion(), "M100") != 0)
+  {//! Only runs if Flight status is available
+  if((*enableFlag) & (1<<11)) {
+    if (getBroadcastData().pos.health > 3) {
+      if (getFlightStatus() != currentState) {
+        prevState = currentState;
+        currentState = getFlightStatus();
+        if (prevState == Flight::STATUS_MOTOR_OFF && currentState == Flight::STATUS_GROUND_STANDBY) {
+          homepointAltitude = getBroadcastData().pos.altitude;
+        }
+        if (prevState == Flight::STATUS_TAKE_OFF && currentState == Flight::STATUS_GROUND_STANDBY) {
+          homepointAltitude = getBroadcastData().pos.altitude;
+        }
+        //! This case would exist if the user starts OSDK after take off.
+        else if (prevState == Flight::STATUS_MOTOR_OFF && currentState == Flight::STATUS_TAKE_OFF) {
+          homepointAltitude = 999999;
+        }
+      }
+    } else {
+      homepointAltitude = 999999;
+    }
+   }
+  }
   if (broadcastCallback.callback)
     broadcastCallback.callback(this, protocolHeader, broadcastCallback.userData);
 }

osdk-core/src/DJI_Flight.cpp

@@ -38,7 +38,6 @@ void Flight::task(TASK taskname, CallBack TaskCallback, UserData userData)
 {
   taskData.cmdData = taskname;
   taskData.cmdSequence++;
-
   api->send(2, encrypt, SET_CONTROL, CODE_TASK, (unsigned char *)&taskData, sizeof(taskData),
       100, 3, TaskCallback ? TaskCallback : Flight::taskCallback, userData);
 }
@@ -48,14 +47,12 @@ unsigned short Flight::task(TASK taskname, int timeout)
   taskData.cmdData = taskname;
   taskData.cmdSequence++;
 
-  api->send(2, encrypt, SET_CONTROL, CODE_TASK, (unsigned char *)&taskData, sizeof(taskData),
-      100, 3, 0, 0);
-
-  api->serialDevice->lockACK();
-  api->serialDevice->wait(timeout);
-  api->serialDevice->freeACK();
-
-  return api->missionACKUnion.simpleACK;
+    api->send(2, encrypt, SET_CONTROL, CODE_TASK, (unsigned char *) &taskData, sizeof(taskData),
+              100, 3, 0, 0);
+    api->serialDevice->lockACK();
+    api->serialDevice->wait(timeout);
+    api->serialDevice->freeACK();
+    return api->missionACKUnion.simpleACK;
 }
 
 void Flight::setArm(bool enable, CallBack ArmCallback, UserData userData)
@@ -96,9 +93,36 @@ void Flight::setMovementControl(uint8_t flag, float32_t x, float32_t y, float32_
   data.flag = flag;
   data.x = x;
   data.y = y;
-  data.z = z;
   data.yaw = yaw;
-  api->send(0, encrypt, SET_CONTROL, CODE_CONTROL, &data, sizeof(FlightData));
+  if(api->getFwVersion() > MAKE_VERSION(3,2,0,0) && api->getFwVersion() < MAKE_VERSION(3,2,15,39)) {
+    if (flag & (1 << 4)) {
+      if (api->getBroadcastData().pos.health > 3) {
+        if(api->homepointAltitude!= 999999) {
+          data.z = z + api->homepointAltitude;
+          api->send(0, encrypt, SET_CONTROL, CODE_CONTROL, &data, sizeof(FlightData));
+        }
+      } else {
+        API_LOG(api->getDriver(), STATUS_LOG, "Not enough GPS locks, cannot run Movement Control \n");
+      }
+    }
+  }
+  else if(api->getFwVersion() == MAKE_VERSION(3,2,100,0)) {
+    if (flag & (1 << 4)) {
+      if (api->getBroadcastData().pos.health > 3) {
+        if(api->homepointAltitude!= 999999) {
+          data.z = z + api->homepointAltitude;
+          api->send(0, encrypt, SET_CONTROL, CODE_CONTROL, &data, sizeof(FlightData));
+        }
+      } else {
+        API_LOG(api->getDriver(), STATUS_LOG, "Not enough GPS locks, cannot run Movement Control \n");
+      }
+    }
+  }
+  else
+  {
+    data.z = z;
+    api->send(0, encrypt, SET_CONTROL, CODE_CONTROL, &data, sizeof(FlightData));
+  }
 }
 
 
@@ -213,7 +237,7 @@ void Flight::armCallback(CoreAPI *api, Header *protocolHeader, UserData userData
   }
 }
 
-void Flight::taskCallback(CoreAPI *api, Header *protocolHeader, UserData userData __UNUSED)
+void Flight::taskCallback(CoreAPI *api, Header *protocolHeader, UserData userData)
 {
   unsigned short ack_data;
   if (protocolHeader->length - EXC_DATA_SIZE <= 2)

osdk-wrapper/CMakeLists.txt

@@ -21,18 +21,26 @@ FILE(GLOB SOURCE_FILES inc/*.h src/*.cpp)
 add_library(${PROJECT_NAME} ${SOURCE_FILES})
 
 if (USE_PRECISION_MISSIONS AND (${DISTRO_VERSION} STREQUAL "1604"))
+  # Precision-Missions library requiers RapidJSON
   add_dependencies(${PROJECT_NAME} rapidjson)
   include_directories("${rapidjson_source_dir}/include")
 
-  add_dependencies(${PROJECT_NAME} PrecisionMissions)
-  include_directories("${precision_missions_source_dir}/inc")
-
-  # Local Mission Planning libraries
-  include_directories("${precision_missions_source_dir}/dependencies")
-  include_directories("${precision_missions_source_dir}/dependencies/csv")
-
-  set(MISSIONPLAN_LIBRARY 
-   "${precision_missions_source_dir}/lib/${TARGET_ARCH}/${DISTRO_VERSION}/libmissionplan.a")
+  # If you are not using cutom binaries, CMake will download
+  # pre-duilt binaries and place them in the CMake build directory
+  if(NOT EXISTS ${PRECISION_MISSIONS_INCLUDE_DIR})
+    add_dependencies(${PROJECT_NAME} PrecisionMissions)
+    set(PRECISION_MISSIONS_INCLUDE_DIR "${precision_missions_source_dir}/inc" CACHE INTERNAL "" FORCE)
+    set(PRECISION_MISSIONS_DEPENDENCIES_DIR "${precision_missions_source_dir}/dependencies" CACHE INTERNAL "" FORCE)
+ endif()
+
+  if(NOT EXISTS ${PRECISION_MISSIONS_LIBRARY})
+     set(PRECISION_MISSIONS_LIBRARY
+     "${precision_missions_source_dir}/lib/${TARGET_ARCH}/${DISTRO_VERSION}/libmissionplan.a")
+  endif()
+
+  include_directories("${PRECISION_MISSIONS_INCLUDE_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}/csv")
 endif()
 
 if (USE_COLLISION_AVOIDANCE)
@@ -43,16 +51,15 @@ if (USE_POINTCLOUD2LAS)
   add_dependencies(${PROJECT_NAME} dji-ros-pointcloud2las)
 endif()
 
-
 set (WRAPPER_LINK_LIBRARIES djiosdk-core djiosdk-platform)
 
 if (LIDAR_LOGGING)
   set(WRAPPER_LINK_LIBRARIES ${WRAPPER_LINK_LIBRARIES} vlp16lidar-driver)
-  #target_link_libraries(${PROJECT_NAME}  djiosdk-core vlp16lidar-driver djiosdk-platform ${MISSIONPLAN})
+  #target_link_libraries(${PROJECT_NAME}  djiosdk-core vlp16lidar-driver djiosdk-platform ${PRECISION_MISSIONS_LIBRARY})
 endif()
 
 if (USE_PRECISION_MISSIONS)
-  set(WRAPPER_LINK_LIBRARIES ${WRAPPER_LINK_LIBRARIES} ${MISSIONPLAN_LIBRARY})
+  set(WRAPPER_LINK_LIBRARIES ${WRAPPER_LINK_LIBRARIES} ${PRECISION_MISSIONS_LIBRARY})
 endif ()
 
 target_link_libraries(${PROJECT_NAME}  ${WRAPPER_LINK_LIBRARIES})

sample/Linux/Blocking/CMakeLists.txt

@@ -18,24 +18,22 @@ if (LIDAR_LOGGING)
   include_directories(../../../sensor-integration/velodyne_vlp16puck_lite_API_example/vlp16lidar-driver/inc)
 endif ()
 
-if (USE_PRECISION_MISSIONS)
-  include_directories("${precision_missions_source_dir}/inc")
-
-  # Local Mission Planning libraries
-  include_directories("${precision_missions_source_dir}/dependencies")
-  include_directories("${precision_missions_source_dir}/dependencies/csv")
-endif()
-
 FILE(GLOB SOURCE_FILES inc/*.h src/*.cpp)
 add_executable(${PROJECT_NAME} ${SOURCE_FILES})
 
 add_dependencies(${PROJECT_NAME} djiosdk-wrapper)
 
+if(USE_PRECISION_MISSIONS)
+  include_directories("${PRECISION_MISSIONS_INCLUDE_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}/csv")
+endif()
+
 set(BLOCKING_LINK_LIBRARIES djiosdk-core djiosdk-platform djiosdk-wrapper)
 
 if(LIDAR_LOGGING)
   set(BLOCKING_LINK_LIBRARIES ${BLOCKING_LINK_LIBRARIES} vlp16lidar-driver)
-  #target_link_libraries(${PROJECT_NAME} vlp16lidar-driver djiosdk-core djiosdk-platform djiosdk-wrapper ${MISSIONPLAN})
+  #target_link_libraries(${PROJECT_NAME} vlp16lidar-driver djiosdk-core djiosdk-platform djiosdk-wrapper ${PRECISION_MISSIONS_LIBRARY})
 endif()
 
 target_link_libraries(${PROJECT_NAME} ${BLOCKING_LINK_LIBRARIES})

sample/Linux/Non-Blocking/CMakeLists.txt

@@ -19,11 +19,9 @@ if (LIDAR_LOGGING)
 endif ()
 
 if (USE_PRECISION_MISSIONS)
-  include_directories("${precision_missions_source_dir}/inc")
-
-  # Local Mission Planning libraries
-  include_directories("${precision_missions_source_dir}/dependencies")
-  include_directories("${precision_missions_source_dir}/dependencies/csv")
+  include_directories("${PRECISION_MISSIONS_INCLUDE_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}")
+  include_directories("${PRECISION_MISSIONS_DEPENDENCIES_DIR}/csv")
 endif()
 
 FILE(GLOB SOURCE_FILES inc/*.h src/*.cpp)