fix report arm freedrive control mode

cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h

@@ -34,7 +34,7 @@ public:
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return ControlMode::Position; }
+    ControlMode getControlMode() const override;
     bool supportsActionQueueMotion() const noexcept override { return true; }
     Result listBaseFrame(std::vector<std::string>& frame_names) const override;
     Result listTCPFrame(std::vector<std::string>& frame_names) const override;

cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp

@@ -733,6 +733,39 @@ SafetyMode AuboArm::getSafetyMode() const
     return static_cast<SafetyMode>(hardware_safety_mode_.load());
 }
 
+ControlMode AuboArm::getControlMode() const
+{
+    if (!connected_.load()) {
+        return ControlMode::None;
+    }
+
+#if defined(CMVR_HAS_AUBO_SDK)
+    try {
+        const auto robot_names = sdk_->rpc_client->getRobotNames();
+        if (robot_names.empty()) {
+            return ControlMode::Position;
+        }
+        const auto robot_interface =
+            sdk_->rpc_client->getRobotInterface(robot_names.front());
+        if (!robot_interface) {
+            return ControlMode::Position;
+        }
+
+        const auto robot_state = robot_interface->getRobotState();
+        if (robot_state &&
+            robot_state->getRobotModeType() ==
+                arcs::common_interface::RobotModeType::BackDrive) {
+            return ControlMode::Freedrive;
+        }
+    } catch (const std::exception&) {
+        // Keep telemetry available when a controller version does not support
+        // one of the optional hand-guiding status queries.
+    }
+#endif
+
+    return ControlMode::Position;
+}
+
 bool AuboArm::isEmergencyStopped() const
 {
     return emergency_stopped_.load() || hardware_emergency_stopped_.load();

cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp

@@ -283,6 +283,34 @@ Result HuayanRobot::setSpeedScaling(const double scaling)
     return Result::success();
 }
 
+ControlMode HuayanRobot::getControlMode() const
+{
+    if (!isConnected()) {
+        return ControlMode::None;
+    }
+
+    int tri_stage_enabled = 0;
+    int tri_stage_mode = 1;
+    int force_control_state = 0;
+    int tri_stage_result = -1;
+    int force_state_result = -1;
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        tri_stage_result = HRIF_ReadTriStageSwitch(
+            box_id_, robot_id_, tri_stage_enabled, tri_stage_mode);
+        force_state_result = HRIF_ReadForceControlState(
+            box_id_, robot_id_, force_control_state);
+    }
+    const bool tri_stage_freedrive =
+        tri_stage_result == 0 && tri_stage_enabled != 0 && tri_stage_mode == 0;
+    const bool force_freedrive =
+        force_state_result == 0 && force_control_state == 3;
+    if (tri_stage_freedrive || force_freedrive) {
+        return ControlMode::Freedrive;
+    }
+    return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position;
+}
+
 bool HuayanRobot::isProtectiveStopped() const
 {
     const auto state = readHrState_();

cmvr-es/devices/arm/huayan_arm/huayan_arm.h

@@ -38,7 +38,7 @@ public:
     CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override;
     RobotMode getRobotMode() const override;
     SafetyMode getSafetyMode() const override;
-    ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; }
+    ControlMode getControlMode() const override;
     bool supportsActionQueueMotion() const noexcept override { return true; }
     Result listBaseFrame(std::vector<std::string>& frame_names) const override;
     Result listTCPFrame(std::vector<std::string>& frame_names) const override;