Merge pull request #5 from AutoModality/BB-877

fix for onboard control status issue

Author: Aaron Roller

src/dji_sdk_obsoleted/modules/dji_sdk_node.cpp

@@ -458,6 +458,7 @@ DJISDKNode::initDataSubscribeFromFC(ros::NodeHandle& nh)
   {
     topicList50Hz.push_back(Telemetry::TOPIC_POSITION_VO);
     topicList50Hz.push_back(Telemetry::TOPIC_AVOID_DATA);
+    topicList50Hz.push_back(Telemetry::TOPIC_CONTROL_DEVICE);
     topicList50Hz.push_back(Telemetry::TOPIC_RC_WITH_FLAG_DATA);
     topicList50Hz.push_back(Telemetry::TOPIC_FLIGHT_ANOMALY);
 

src/dji_sdk_obsoleted/modules/dji_sdk_node_publisher.cpp

@@ -52,14 +52,6 @@ DJISDKNode::dataBroadcastCallback()
     rc_publisher.publish(rc_joy);
   }
 
-  //update device control info
-  if (data_enable_flag & DataBroadcast::DATA_ENABLE_FLAG::A3_HAS_DEVICE)
-  {
-    std_msgs::UInt8 status_device;
-    status_device.data = vehicle->broadcast->getSDKInfo().deviceStatus;
-    device_status_publisher.publish(status_device);
-  }
-
   tf::Matrix3x3 R_FRD2NED;
   tf::Quaternion q_FLU2ENU;
 
@@ -571,13 +563,24 @@ DJISDKNode::publish50HzData(Vehicle* vehicle, RecvContainer recvFrame,
 
   uint16_t data_enable_flag = vehicle->broadcast->getPassFlag();
 
+  
   //update device control info
-  if (data_enable_flag & DataBroadcast::DATA_ENABLE_FLAG::A3_HAS_DEVICE)
-  {
+  // if (data_enable_flag & DataBroadcast::DATA_ENABLE_FLAG::A3_HAS_DEVICE)
+  // {
+    Telemetry::TypeMap<Telemetry::TOPIC_CONTROL_DEVICE>::type sdk_info =
+      vehicle->subscribe->getValue<Telemetry::TOPIC_CONTROL_DEVICE>();
     std_msgs::UInt8 status_device;
-    status_device.data = vehicle->broadcast->getSDKInfo().deviceStatus;
+    // status_device.data = vehicle->broadcast->getSDKInfo().deviceStatus;
+    status_device.data = sdk_info.deviceStatus;
+    // TODO The underlying status changed, but we look for a value of 2. Fix this.
+    //https://github.com/dji-sdk/Onboard-SDK/issues/528
+    //Jeff's farewell Mary Poppins
+    if (status_device.data == 4){
+      status_device.data = 2;
+      ROS_INFO_THROTTLE(1,"**UNDER AM CONTROL**");
+    }
     p->device_status_publisher.publish(status_device);
-  }
+  // }
 }