Merge pull request #73 in OS/onboard-sdk from fix_heart_beat_issue to…

… develop * commit '2ea649a08271190ebe00ac830f9a2209c7f2619b': fix:fix the gimbal subscribe issues in gimbal sample fix:add angle data reporting in the gimbal sample fix:fix issues of release resources fix:remove useless files and add sleep between heartbeat sending
dji-sdk · Dec 20, 2019 · 1049dfc · 1049dfc
2 parents 2f879c7 + 2ea649a
commit 1049dfc
Show file tree

Hide file tree

Showing 8 changed files with 137 additions and 267 deletions.
diff --git a/osdk-core/api/src/dji_vehicle.cpp b/osdk-core/api/src/dji_vehicle.cpp
@@ -87,6 +87,7 @@ Vehicle::Vehicle(Linker* linker)
 #endif
 {
   ackErrorCode.data = OpenProtocolCMD::ErrorCode::CommonACK::NO_RESPONSE_ERROR;
+  sendHeartbeatToFCHandle = NULL;
 }
 
 bool
@@ -263,6 +264,8 @@ Vehicle::functionalSetUp()
 
 Vehicle::~Vehicle()
 {
+  OsdkOsal_TaskDestroy(sendHeartbeatToFCHandle);
+
   if (this->subscribe)
   {
     subscribe->reset(1);
@@ -324,13 +327,15 @@ Vehicle::~Vehicle()
   {
     delete this->flightController;
   }
-
+  if(this->legacyLinker)
+  {
+    delete this->legacyLinker;
+  }
 #ifdef ADVANCED_SENSING
   if (this->advancedSensing)
     delete this->advancedSensing;
 #endif
 
-  OsdkOsal_TaskDestroy(sendHeartbeatToFCHandle);
 }
 
 
@@ -783,12 +788,16 @@ Vehicle::processAdvancedSensingImgs(RecvContainer* receivedFrame)
 bool
 Vehicle::initOSDKHeartBeatThread() {
     /*! create task for OSDK heart beat */
-    E_OsdkStat osdkStat = OsdkOsal_TaskCreate(&sendHeartbeatToFCHandle,
-                                              (void *(*)( void *)) (sendHeartbeatToFCTask),
-                                              OSDK_TASK_STACK_SIZE_DEFAULT, this->linker);
-    if (osdkStat != OSDK_STAT_OK) {
+    if(!sendHeartbeatToFCHandle) {
+      E_OsdkStat osdkStat = OsdkOsal_TaskCreate(&sendHeartbeatToFCHandle,
+                                                (void *(*)(
+                                                    void *)) (sendHeartbeatToFCTask),
+                                                OSDK_TASK_STACK_SIZE_DEFAULT,
+                                                this->linker);
+      if (osdkStat != OSDK_STAT_OK) {
         DERROR("osdk heart beat task create error:%d", osdkStat);
         return false;
+      }
     }
 
     return true;
@@ -1212,6 +1221,9 @@ Vehicle::sendHeartbeatToFCTask(void *arg) {
               DJI::OSDK::Vehicle::sendHeartbeatToFCFunc(linker);
               preHeartBeatTimeStamp = curHeartBeatTimeStamp;
           }
+
+          /*! TODO: with out sleep 100ms, the time will get the same as last time. */
+          OsdkOsal_TaskSleepMs(100);
       }
     } else {
       DERROR("Osdk send heart beat to fc task run failed because of the invalid linker "

diff --git a/osdk-core/platform/STM32/inc/STM32F4DataGuard.h b/osdk-core/platform/STM32/inc/STM32F4DataGuard.h
diff --git a/osdk-core/platform/STM32/src/STM32F4DataGuard.cpp b/osdk-core/platform/STM32/src/STM32F4DataGuard.cpp
diff --git a/sample/core/src/gimbal_manager_async_sample.cpp b/sample/core/src/gimbal_manager_async_sample.cpp
@@ -29,13 +29,51 @@
 
 #include "gimbal_manager_async_sample.hpp"
 
+#define GIMBA_SUB_PACKAGE_INDEX 0
+
 using namespace DJI::OSDK;
 using namespace DJI::OSDK::Telemetry;
 
 GimbalManagerAsyncSample::GimbalManagerAsyncSample(Vehicle *vehiclePtr)
-    : vehicle(vehiclePtr) {}
+    : vehicle(vehiclePtr) {
+  /*! verify the subscribe function */
+  ACK::ErrorCode ack = vehiclePtr->subscribe->verify(1);
+  if (ACK::getError(ack) != ACK::SUCCESS) {
+    ACK::getErrorCodeMessage(ack, __func__);
+  }
+
+  /*! Package 0: Subscribe to TOPIC_GIMBAL_FULL_DATA at freq 50 Hz */
+  ACK::ErrorCode subscribeStatus;
+  int       pkgIndex        = GIMBA_SUB_PACKAGE_INDEX;
+  int       freq            = 50;
+  TopicName topicList50Hz[]  = { TOPIC_GIMBAL_FULL_DATA };
+  int       numTopic        = sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
+  bool      enableTimestamp = false;
 
-GimbalManagerAsyncSample::~GimbalManagerAsyncSample() {}
+  bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(
+      pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);
+  if (!(pkgStatus))
+  {
+    DERROR("init package for TOPIC_GIMBAL_FULL_DATA failed." );
+  }
+  subscribeStatus = vehicle->subscribe->startPackage(pkgIndex, 1);
+  if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
+  {
+    ACK::getErrorCodeMessage(subscribeStatus, __func__);
+    vehicle->subscribe->removePackage(pkgIndex, 1);
+    DERROR("subscribe TOPIC_GIMBAL_FULL_DATA failed." );
+  }
+}
+
+GimbalManagerAsyncSample::~GimbalManagerAsyncSample() {
+  ACK::ErrorCode subscribeStatus =
+      vehicle->subscribe->removePackage(GIMBA_SUB_PACKAGE_INDEX, 1);
+  if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
+  {
+    ACK::getErrorCodeMessage(subscribeStatus, __func__);
+    DERROR("remove subscribe package TOPIC_GIMBAL_FULL_DATA failed." );
+  }
+}
 
 void GimbalManagerAsyncSample::resetAsyncSample(PayloadIndexType index,
                                                 void (*UserCallBack)(