Hotfix Release for 3.2.1 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

dji_sdk_lib/include/dji_sdk_lib/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;

dji_sdk_lib/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();

dji_sdk_lib/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,39 @@ 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);
 
+  //! 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);
 }

dji_sdk_lib/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));
+  }
 }