Merge pull request dji-sdk#439 from DJI-Colin/add_obtain_battery_inte…

…rface

Add obtain battery interface

CMakeLists.txt

@@ -105,6 +105,10 @@ add_message_files(
   MissionHotpointTask.msg
   FlightAnomaly.msg
   VOPosition.msg
+  BatteryState.msg
+  BatteryWholeInfo.msg
+  SmartBatteryState.msg
+  SmartBatteryDynamicInfo.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -130,6 +134,8 @@ add_service_files(
   CameraStartShootIntervalPhoto.srv
   CameraStopShootPhoto.srv
   CameraRecordVideoAction.srv
+  GetWholeBatteryInfo.srv
+  GetSingleBatteryDynamicInfo.srv
   MFIO.srv
   SetGoHomeAltitude.srv
   SetNewHomePoint.srv

include/dji_osdk_ros/dji_vehicle_node.h

@@ -96,6 +96,9 @@
 #include <dji_osdk_ros/MissionHpUpdateYawRate.h>
 #include <dji_osdk_ros/MissionHpResetYaw.h>
 #include <dji_osdk_ros/MissionHpUpdateRadius.h>
+//battery services
+#include <dji_osdk_ros/GetWholeBatteryInfo.h>
+#include <dji_osdk_ros/GetSingleBatteryDynamicInfo.h>
 
 //waypointV2.0 services
 #include <dji_osdk_ros/InitWaypointV2Setting.h>
@@ -207,6 +210,10 @@ namespace dji_osdk_ros
       ros::ServiceServer camera_control_start_shoot_interval_photo_server_;
       ros::ServiceServer camera_control_stop_shoot_photo_server_;
       ros::ServiceServer camera_control_record_video_action_server_;
+
+      /*! for battery */
+      ros::ServiceServer get_single_battery_dynamic_info_server_;
+      ros::ServiceServer get_whole_battery_info_server_;
       /*! for mfio */
       ros::ServiceServer mfio_control_server_;
       /*! for mobile device */
@@ -335,6 +342,9 @@ namespace dji_osdk_ros
       bool cameraStartShootIntervalPhotoCallback(CameraStartShootIntervalPhoto::Request& request, CameraStartShootIntervalPhoto::Response& response);
       bool cameraStopShootPhotoCallback(CameraStopShootPhoto::Request& request, CameraStopShootPhoto::Response& response);
       bool cameraRecordVideoActionCallback(CameraRecordVideoAction::Request& request, CameraRecordVideoAction::Response& response);
+      /*! for battery info */
+      bool getWholeBatteryInfoCallback(GetWholeBatteryInfo::Request& request,GetWholeBatteryInfo::Response& reponse);
+      bool getSingleBatteryDynamicInfoCallback(GetSingleBatteryDynamicInfo::Request& request, GetSingleBatteryDynamicInfo::Response& response);
       /*! for mfio conrol */
       bool mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response);
       /*! for mobile device */

include/dji_osdk_ros/vehicle_wrapper.h

@@ -105,6 +105,11 @@ namespace dji_osdk_ros
       bool moveByPositionOffset(ACK::ErrorCode& ack, int timeout, MoveOffset& p_offset);
       bool obtainReleaseCtrlAuthority(bool enableObtain, int timeout);
 
+      /*! Parts of Battery */
+      bool getBatteryWholeInfo(DJI::OSDK::BatteryWholeInfo& batteryWholeInfo);
+      bool getSingleBatteryDynamicInfo(const DJI::OSDK::DJIBattery::RequestSmartBatteryIndex batteryIndex,
+                                       DJI::OSDK::SmartBatteryDynamicInfo& batteryDynamicInfo);
+
       /*! Parts of mfio */
       uint8_t outputMFIO(uint8_t mode, uint8_t channel, uint32_t init_on_time_us, uint16_t freq, bool block, uint8_t gpio_value);
       uint32_t inputMFIO(uint8_t mode, uint8_t channel, bool block);

msg/BatteryState.msg

@@ -0,0 +1,6 @@
+uint8 voltageNotSafety # Generally caused by low temperature, the battery has electricity,
+                       # but the battery voltage is too low.
+uint8 veryLowVoltageAlarm
+uint8 LowVoltageAlarm
+uint8 seriousLowCapacityAlarm
+uint8 LowCapacityAlarm

msg/BatteryWholeInfo.msg

@@ -0,0 +1,19 @@
+uint16  remainFlyTime             # remain fly time(s)
+uint16  goHomeNeedTime            # Time required for the gohome flight (s)
+uint16  landNeedTime              # Time required for the land flight (s).max value:100*/
+uint16  goHomeNeedCapacity        # Capacity required for the gohome flight (%).max value:100*/
+uint16  landNeedCapacity          # Capacity required for the land flight (%).max value:100*/
+float32 safeFlyRadius             # Safe flight area radius (m)*/
+float32 capacityConsumeSpeed      # (mAh/sec)*/
+BatteryState batteryState
+uint8   goHomeCountDownState      # Countdown status of smart battery gohome
+                                   # 0/2:not in gohome state
+                                   # 1  :in gohome state
+                                   #
+uint8   gohomeCountDownvalue      # uint:s.max value:10
+uint16  voltage                   # mv
+uint8   batteryCapacityPercentage # uint:%.max value:100
+uint8   lowBatteryAlarmThreshold
+uint8   lowBatteryAlarmEnable
+uint8   seriousLowBatteryAlarmThreshold
+uint8   seriousLowBatteryAlarmEnable  

msg/SmartBatteryDynamicInfo.msg

@@ -0,0 +1,10 @@
+uint8  batteryIndex
+int32  currentVoltage          # uint:mV
+int32  currentElectric         # uint:mA
+uint32 fullCapacity            # uint:mAh
+uint32 remainedCapacity        # uint:mAh
+int16  batteryTemperature      # uint:0.1℃
+uint8  cellCount
+uint8  batteryCapacityPercent  # uint:%
+SmartBatteryState batteryState
+uint8  SOP                     # Relative power percentage

msg/SmartBatteryState.msg

@@ -0,0 +1,8 @@
+uint8 cellBreak                  # 0:normal;other:Undervoltage core index(0x01-0x1F)
+uint8 selfCheckError             # enum-type: DJISmartBatterySelfCheck
+uint8 batteryClosedReason        # enum-type: DJI_BETTERY_CLOSED_REASON
+uint8 batSOHState                # enum-type: DJISmartBatterySohState*/
+uint8 maxCycleLimit              # APP:cycle_limit*10*/
+uint8 batteryCommunicationAbnormal
+uint8 hasCellBreak
+uint8 heatState                  # enum-type: DJISmartBatteryHeatState

src/dji_osdk_ros/dji_vehicle_node.cpp

@@ -223,6 +223,12 @@ void VehicleNode::initService()
   camera_control_stop_shoot_photo_server_ = nh_.advertiseService("camera_stop_shoot_photo", &VehicleNode::cameraStopShootPhotoCallback, this);
   camera_control_record_video_action_server_ = nh_.advertiseService("camera_record_video_action", &VehicleNode::cameraRecordVideoActionCallback, this);
 
+  /* @brief
+   * get whole battery info server
+   * @platforms M210V2
+   */
+  get_whole_battery_info_server_ = nh_.advertiseService("get_whole_battery_info", &VehicleNode::getWholeBatteryInfoCallback, this);
+  get_single_battery_dynamic_info_server_ = nh_.advertiseService("get_single_battery_dynamic_info", &VehicleNode::getSingleBatteryDynamicInfoCallback, this);
   /*! @brief
    *  mfio control server
    *  @platforms null
@@ -1284,6 +1290,90 @@ bool VehicleNode::cameraRecordVideoActionCallback(CameraRecordVideoAction::Reque
   return response.result;
 }
 
+bool VehicleNode::getWholeBatteryInfoCallback(GetWholeBatteryInfo::Request& request,GetWholeBatteryInfo::Response& response)
+{
+  ROS_INFO_STREAM("get Whole Battery Info callback");
+  if (ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    return false;
+  }
+
+  DJI::OSDK::BatteryWholeInfo batteryWholeInfo;
+  if (ptr_wrapper_->getBatteryWholeInfo(batteryWholeInfo))
+  {
+    response.battery_whole_info.remainFlyTime  = batteryWholeInfo.remainFlyTime;
+    response.battery_whole_info.goHomeNeedTime = batteryWholeInfo.goHomeNeedTime ;
+    response.battery_whole_info.landNeedTime   = batteryWholeInfo.landNeedTime;
+    response.battery_whole_info.goHomeNeedCapacity = batteryWholeInfo.goHomeNeedCapacity;
+    response.battery_whole_info.landNeedCapacity = batteryWholeInfo.landNeedCapacity ;
+    response.battery_whole_info.safeFlyRadius = batteryWholeInfo.safeFlyRadius;
+    response.battery_whole_info.capacityConsumeSpeed = batteryWholeInfo.capacityConsumeSpeed;
+    response.battery_whole_info.goHomeCountDownState = batteryWholeInfo.goHomeCountDownState;
+    response.battery_whole_info.gohomeCountDownvalue = batteryWholeInfo.gohomeCountDownvalue;
+    response.battery_whole_info.voltage = batteryWholeInfo.voltage;
+    response.battery_whole_info.batteryCapacityPercentage = batteryWholeInfo.batteryCapacityPercentage;
+    response.battery_whole_info.lowBatteryAlarmThreshold = batteryWholeInfo.lowBatteryAlarmThreshold;
+    response.battery_whole_info.lowBatteryAlarmEnable = batteryWholeInfo.lowBatteryAlarmEnable;
+    response.battery_whole_info.seriousLowBatteryAlarmThreshold = batteryWholeInfo.seriousLowBatteryAlarmThreshold;
+    response.battery_whole_info.seriousLowBatteryAlarmEnable = batteryWholeInfo.seriousLowBatteryAlarmEnable;
+
+    response.battery_whole_info.batteryState.voltageNotSafety        = batteryWholeInfo.batteryState.voltageNotSafety;
+    response.battery_whole_info.batteryState.veryLowVoltageAlarm     = batteryWholeInfo.batteryState.veryLowVoltageAlarm;
+    response.battery_whole_info.batteryState.LowVoltageAlarm         = batteryWholeInfo.batteryState.LowVoltageAlarm;
+    response.battery_whole_info.batteryState.seriousLowCapacityAlarm = batteryWholeInfo.batteryState.seriousLowCapacityAlarm;
+    response.battery_whole_info.batteryState.LowCapacityAlarm        = batteryWholeInfo.batteryState.LowCapacityAlarm;
+  }
+  else
+  {
+    DSTATUS("get Battery Whole Info failed!");
+    return false;
+  }
+  return true;
+}
+
+bool VehicleNode::getSingleBatteryDynamicInfoCallback(GetSingleBatteryDynamicInfo::Request& request, 
+                                                      GetSingleBatteryDynamicInfo::Response& response)
+{
+  ROS_INFO_STREAM("get Single Battery Dynamic Info callback");
+  if(ptr_wrapper_ == nullptr)
+  {
+    ROS_ERROR_STREAM("Vehicle modules is nullptr");
+    return false;
+  }
+
+  DJI::OSDK::SmartBatteryDynamicInfo SmartBatteryDynamicInfo;
+  if (ptr_wrapper_->getSingleBatteryDynamicInfo(static_cast<DJI::OSDK::DJIBattery::RequestSmartBatteryIndex>(request.batteryIndex),
+                                                SmartBatteryDynamicInfo))
+  {
+    response.smartBatteryDynamicInfo.batteryIndex           = SmartBatteryDynamicInfo.batteryIndex;
+    response.smartBatteryDynamicInfo.currentVoltage         = SmartBatteryDynamicInfo.currentVoltage;
+    response.smartBatteryDynamicInfo.currentElectric        = SmartBatteryDynamicInfo.currentElectric;
+    response.smartBatteryDynamicInfo.fullCapacity           = SmartBatteryDynamicInfo.fullCapacity;
+    response.smartBatteryDynamicInfo.remainedCapacity       = SmartBatteryDynamicInfo.remainedCapacity;
+    response.smartBatteryDynamicInfo.batteryTemperature     = SmartBatteryDynamicInfo.batteryTemperature;
+    response.smartBatteryDynamicInfo.cellCount              = SmartBatteryDynamicInfo.cellCount;
+    response.smartBatteryDynamicInfo.batteryCapacityPercent = SmartBatteryDynamicInfo.batteryCapacityPercent;
+    response.smartBatteryDynamicInfo.SOP                    = SmartBatteryDynamicInfo.SOP;
+
+    response.smartBatteryDynamicInfo.batteryState.cellBreak                    = SmartBatteryDynamicInfo.batteryState.cellBreak;
+    response.smartBatteryDynamicInfo.batteryState.selfCheckError               = SmartBatteryDynamicInfo.batteryState.selfCheckError;
+    response.smartBatteryDynamicInfo.batteryState.batteryClosedReason          = SmartBatteryDynamicInfo.batteryState.batteryClosedReason;
+    response.smartBatteryDynamicInfo.batteryState.batSOHState                  = SmartBatteryDynamicInfo.batteryState.batSOHState;
+    response.smartBatteryDynamicInfo.batteryState.maxCycleLimit                = SmartBatteryDynamicInfo.batteryState.maxCycleLimit;
+    response.smartBatteryDynamicInfo.batteryState.batteryCommunicationAbnormal = SmartBatteryDynamicInfo.batteryState.batteryCommunicationAbnormal;
+    response.smartBatteryDynamicInfo.batteryState.hasCellBreak                 = SmartBatteryDynamicInfo.batteryState.hasCellBreak;
+    response.smartBatteryDynamicInfo.batteryState.heatState                    = SmartBatteryDynamicInfo.batteryState.heatState;
+  }
+  else
+  {
+    DSTATUS("get Single Battery Dynamic Info failed!");
+    return false;
+  }
+
+  return true;
+}
+
 bool VehicleNode::mfioCtrlCallback(MFIO::Request& request, MFIO::Response& response)
 {
   ROS_INFO_STREAM("MFIO Control callback");

src/dji_osdk_ros/modules/vehicle_wrapper.cpp

@@ -1896,6 +1896,32 @@ static T_OsdkOsalHandler osalHandler = {
     return true;
   }
 
+  bool VehicleWrapper::getBatteryWholeInfo(DJI::OSDK::BatteryWholeInfo& batteryWholeInfo)
+  {
+    if (!vehicle) {
+      DERROR("vehicle is a null value");
+      return false;
+    }
+    bool result = true;
+
+    result &= vehicle->djiBattery->subscribeBatteryWholeInfo(true);
+    OsdkOsal_TaskSleepMs(500);
+    result &= vehicle->djiBattery->getBatteryWholeInfo(batteryWholeInfo);
+
+    return result;
+  }
+
+  bool VehicleWrapper::getSingleBatteryDynamicInfo(const DJI::OSDK::DJIBattery::RequestSmartBatteryIndex batteryIndex,
+                                                   DJI::OSDK::SmartBatteryDynamicInfo& batteryDynamicInfo)
+  {
+    if (!vehicle) {
+      DERROR("vehicle is a null value");
+      return false;
+    }
+
+    return vehicle->djiBattery->getSingleBatteryDynamicInfo(batteryIndex, batteryDynamicInfo);
+  }
+
   uint8_t VehicleWrapper::outputMFIO(uint8_t mode, uint8_t channel, uint32_t init_on_time_us, uint16_t freq, bool block, uint8_t gpio_set_value)
   {
     int responseTimeout = 1;

src/dji_osdk_ros/samples/CMakeLists.txt

@@ -103,6 +103,12 @@ target_link_libraries(waypointV2_node
         )
 add_dependencies(waypointV2_node dji_osdk_ros_generate_messages_cpp)
 
+add_executable(battery_node battery_node.cpp)
+target_link_libraries(battery_node
+        ${PROJECT_NAME}
+        )
+add_dependencies(battery_node dji_osdk_ros_generate_messages_cpp)
+
 if (OPEN_CV_3_3_0_INSTALLED AND FOUND_OPENCV_VIZ)
 
     message(STATUS "Found OpenCV ${OpenCV_VERSION}, Viz3d, and advanced sensing module, stereo vision depth perception node will be compiled")