• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*
2 * Copyright (c) 2025 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 *     http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15 
16 #include "mc_motion_manager.h"
17 
18 #include <algorithm>
19 #include <stdexcept>
20 #include <iostream>
21 
22 #include "mechbody_controller_log.h"
23 #include "mc_subscription_center.h"
24 #include "mechbody_controller_service.h"
25 #include "mc_camera_tracking_controller.h"
26 #include "mc_connect_manager.h"
27 #include "mechbody_controller_utils.h"
28 
29 namespace OHOS {
30 namespace MechBodyController {
31 namespace {
32 const std::string TAG = "MotionManager";
33 const std::string EVENT_THREAD_NAME = "MotionManager_EventHandler";
34 constexpr float DEGREE_CIRCLED_MAX = 6.28;
35 constexpr float DEGREE_CIRCLED_MIN = -6.28;
36 constexpr float DEFAULT_DURATION = 500;
37 static constexpr int32_t TRANS_TIME = 1000;
38 constexpr int32_t WHEEL_FILTER_COUNT = 2;
39 constexpr int32_t WHEEL_CORRECT_COUNT = 3;
40 constexpr float WHEEL_ZOOM_THRESHOLD = 10.0f;
41 constexpr int32_t WHEEL_ZOOM_01X_EVENT_NUM = 1;
42 constexpr int32_t WHEEL_ZOOM_1X_EVENT_NUM = 10;
43 constexpr float YAW_DISCREPANCY = 0.1f;
44 constexpr float MILLISECONDS_TO_SECONDS = 1000.0f;
45 constexpr float NO_LIMIT_MAX = 3.1415927f;
46 constexpr float NO_LIMIT_MIN = -3.1415926f;
47 constexpr int TASK_COMPLETED = 2;
48 constexpr int32_t RESPONSE_TIMEOUT = 10000;
49 constexpr float YAW_OFFSET = 0.2f;
50 constexpr int32_t CMD_SEND_INTERVAL = 100;
51 
52 const std::map<CameraKeyEvent, int32_t> MAP_KEY_EVENT_VALUE = {
53     {CameraKeyEvent::START_FILMING, MMI::KeyEvent::KEYCODE_VOLUME_UP},
54     {CameraKeyEvent::SWITCH_CAMERA, MMI::KeyEvent::KEYCODE_VIDEO_NEXT},
55     {CameraKeyEvent::ZOOM_IN, MMI::KeyEvent::KEYCODE_ZOOMIN},
56     {CameraKeyEvent::ZOOM_OUT, MMI::KeyEvent::KEYCODE_ZOOMOUT},
57     {CameraKeyEvent::SWITCH_PHOTO_FILM, MMI::KeyEvent::KEYCODE_VCR2}
58 };
59 
60 const std::vector<RotateParam> NOD_ACTIONS = {
61     {{0.0f, 0.0f, -0.02f}, 30, false},
62     {{0.0f, 0.0f, 0.2f}, 80, false},
63     {{0.0f, 0.0f, 0.0f}, 100, false}
64 };
65 }
66 
CheckRotatePointParam(EulerAngles rotateDegree)67 bool CheckRotatePointParam(EulerAngles rotateDegree)
68 {
69     if (rotateDegree.yaw > DEGREE_CIRCLED_MAX || rotateDegree.yaw < DEGREE_CIRCLED_MIN) {
70         HILOGE("The 'yaw' value is out of the specified range. %{public}f.", rotateDegree.yaw);
71         return false;
72     } else if (rotateDegree.roll > DEGREE_CIRCLED_MAX || rotateDegree.roll < DEGREE_CIRCLED_MIN) {
73         HILOGE("The 'roll' value is out of the specified range. %{public}f.", rotateDegree.roll);
74         return false;
75     } else if (rotateDegree.pitch > DEGREE_CIRCLED_MAX || rotateDegree.pitch < DEGREE_CIRCLED_MIN) {
76         HILOGE("The 'pitch' value is out of the specified range. %{public}f.", rotateDegree.pitch);
77         return false;
78     }
79     return true;
80 }
81 
MechAttitudeNotify(const std::shared_ptr<RegisterMechPositionInfoCmd> & cmd)82 void MotionManager::MechAttitudeNotify(const std::shared_ptr<RegisterMechPositionInfoCmd> &cmd)
83 {
84     HILOGD("Received gimbal attitude change event.");
85     CHECK_POINTER_RETURN(cmd, "cmd");
86     EulerAngles position = cmd->GetPosition();
87     RotationAxesStatus status = status_;
88     {
89         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
90         if (deviceStatus_->eulerAngles != position) {
91             HILOGI("New position: %{public}s", position.ToString().c_str());
92             deviceStatus_->eulerAngles = position;
93         }
94     }
95     HILOGD("notify end");
96 }
97 
MMIKeyEvent(CameraKeyEvent eventType)98 void MotionManager::MMIKeyEvent(CameraKeyEvent eventType)
99 {
100     HILOGI("event: %{public}d.", eventType);
101     auto it = MAP_KEY_EVENT_VALUE.find(eventType);
102     if (it == MAP_KEY_EVENT_VALUE.end()) {
103         HILOGW("The camera key event not found.");
104         return;
105     }
106     int32_t keyCode = it->second;
107     auto keyDown = CreateKeyEvent(keyCode, MMI::KeyEvent::KEY_ACTION_DOWN);
108     auto keyUp = CreateKeyEvent(keyCode, MMI::KeyEvent::KEY_ACTION_UP);
109     if (keyDown == nullptr || keyUp == nullptr) {
110         HILOGW("Failed to create back key event");
111         return;
112     }
113     MMI::InputManager::GetInstance()->SimulateInputEvent(keyDown);
114     MMI::InputManager::GetInstance()->SimulateInputEvent(keyUp);
115 }
116 
CreateKeyEvent(int32_t keyCode,int32_t keyAction)117 std::shared_ptr<MMI::KeyEvent> MotionManager::CreateKeyEvent(int32_t keyCode, int32_t keyAction)
118 {
119     std::shared_ptr<MMI::KeyEvent> keyEvent = MMI::KeyEvent::Create();
120     if (keyEvent == nullptr) {
121         HILOGE("Failed to create key event.");
122         return nullptr;
123     }
124     MMI::KeyEvent::KeyItem keyItem;
125     keyItem.SetKeyCode(keyCode);
126     keyItem.SetPressed(keyAction == MMI::KeyEvent::KEY_ACTION_DOWN);
127     keyItem.SetDownTime(GetSysClockTime());
128     keyEvent->SetKeyCode(keyCode);
129     keyEvent->SetKeyAction(keyAction);
130     keyEvent->AddPressedKeyItems(keyItem);
131     return keyEvent;
132 }
133 
GetSysClockTime()134 int64_t MotionManager::GetSysClockTime()
135 {
136     struct timespec ts = {0, 0};
137     if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) {
138         return 0;
139     }
140     return (ts.tv_sec * TRANS_TIME * TRANS_TIME) + (ts.tv_nsec / TRANS_TIME);
141 }
142 
MechButtonEventNotify(const std::shared_ptr<RegisterMechCameraKeyEventCmd> & cmd)143 void MotionManager::MechButtonEventNotify(const std::shared_ptr<RegisterMechCameraKeyEventCmd> &cmd)
144 {
145     CHECK_POINTER_RETURN(cmd, "cmd");
146     CameraKeyEvent eventType = cmd->GetEvent();
147     HILOGI("Received gimbal key press event. eventNo: %{public}d.", eventType);
148 
149     std::shared_ptr<CameraInfo> cameraInfo = McCameraTrackingController::GetInstance().GetCurrentCameraInfo();
150     CHECK_POINTER_RETURN(cameraInfo, "cameraInfo");
151     if (!cameraInfo->isCameraOn) {
152         HILOGI("No camera working, ignore.");
153         return;
154     }
155 
156     std::shared_ptr<MMI::KeyEvent> event = MMI::KeyEvent::Create();
157     CHECK_POINTER_RETURN(cmd, "KeyEvent");
158     switch (eventType) {
159         case CameraKeyEvent::START_FILMING :
160             HILOGI("ButtonEvent START_FILMING.");
161             MMIKeyEvent(CameraKeyEvent::START_FILMING);
162             break;
163         case CameraKeyEvent::SWITCH_CAMERA :
164             HILOGI("ButtonEvent SWITCH_CAMERA.");
165             MMIKeyEvent(CameraKeyEvent::SWITCH_CAMERA);
166             break;
167         case CameraKeyEvent::ZOOM_IN :
168             HILOGI("ButtonEvent ZOOM_IN.");
169             MMIKeyEvent(CameraKeyEvent::ZOOM_IN);
170             break;
171         case CameraKeyEvent::ZOOM_OUT :
172             HILOGI("ButtonEvent ZOOM_OUT.");
173             MMIKeyEvent(CameraKeyEvent::ZOOM_OUT);
174             break;
175         case CameraKeyEvent::SWITCH_PHOTO_FILM :
176             HILOGI("ButtonEvent SWITCH_PHOTO_FILM.");
177             MMIKeyEvent(CameraKeyEvent::SWITCH_PHOTO_FILM);
178             break;
179         default:
180             HILOGW("ButtonEvent undefined action");
181             break;
182     }
183 }
184 
MechParamNotify(const std::shared_ptr<RegisterMechStateInfoCmd> & cmd)185 void MotionManager::MechParamNotify(const std::shared_ptr<RegisterMechStateInfoCmd> &cmd)
186 {
187     HILOGD("Received gimbal param change event.");
188     CHECK_POINTER_RETURN(cmd, "cmd");
189     MechStateInfo info = cmd->GetInfo();
190     RotationAxesStatus status{};
191     {
192         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
193         if (deviceStatus_->stateInfo == info) {
194             return;
195         }
196         HILOGI("mech info change, isPhoneOn %{public}d", info.isPhoneOn);
197         deviceStatus_->stateInfo = info;
198         if (MechConnectManager::GetInstance().GetMechState(mechId_) != info.isPhoneOn) {
199             HandleMechPlacementChange(info.isPhoneOn);
200         }
201         // roll is not enable now, it will be available in the future
202         switch (info.mechMode) {
203             case MechMode::FOLLOW:
204                 status.yawEnabled = true;
205                 status.rollEnabled = false;
206                 status.pitchEnabled = true;
207                 break;
208             case MechMode::PITCH_FREE:
209                 status.yawEnabled = true;
210                 status.rollEnabled = false;
211                 status.pitchEnabled = false;
212                 break;
213             case MechMode::FPV:
214             case MechMode::ROLL:
215                 status.yawEnabled = true;
216                 status.rollEnabled = false;
217                 status.pitchEnabled = true;
218                 break;
219             case MechMode::FREE:
220                 status.yawEnabled = false;
221                 status.rollEnabled = false;
222                 status.pitchEnabled = false;
223                 break;
224             default:
225                 break;
226         }
227         if (deviceStatus_->rotationAxesStatus == status) {
228             return;
229         }
230         deviceStatus_->rotationAxesStatus = status;
231     }
232     HILOGI("notify");
233     MechBodyControllerService::GetInstance().OnRotationAxesStatusChange(mechId_, status);
234 }
235 
HandleMechPlacementChange(bool isPhoneOn)236 void MotionManager::HandleMechPlacementChange(bool isPhoneOn)
237 {
238     HILOGI("start, isPhoneOn = %{public}d.", isPhoneOn);
239     if (!isPhoneOn) {
240         auto hidCmd = factory.CreateSetMechHidPreemptiveCmd(true);
241         CHECK_POINTER_RETURN(hidCmd, "hidCmd is empty.");
242         auto callback = [this, hidCmd, isPhoneOn]() {
243             uint8_t result = hidCmd->GetResult();
244             HILOGI("SetMechHidPreemptiveCmd result: %{public}u.", result);
245             MechConnectManager::GetInstance().NotifyMechState(mechId_, isPhoneOn);
246         };
247         hidCmd->SetResponseCallback(callback);
248         CHECK_POINTER_RETURN(sendAdapter_, "sendAdapter_");
249         sendAdapter_->SendCommand(hidCmd);
250         MechConnectManager::GetInstance().NotifyMechState(mechId_, isPhoneOn);
251     } else {
252         MechConnectManager::GetInstance().NotifyMechState(mechId_, isPhoneOn);
253     }
254     HILOGI("end.");
255 }
256 
MechExecutionResultNotify(const std::shared_ptr<RegisterMechControlResultCmd> & cmd)257 void MotionManager::MechExecutionResultNotify(const std::shared_ptr<RegisterMechControlResultCmd> &cmd)
258 {
259     HILOGD("start.");
260     if (cmd == nullptr) {
261         HILOGW("Result cmd is empty.");
262         return;
263     }
264     uint16_t result = cmd->GetControlResult();
265     uint16_t taskId = cmd->GetTaskId();
266     HILOGD("Notify callback id: %{public}d", taskId);
267     {
268         std::unique_lock <std::mutex> lock(seqCallbackMutex_);
269         if (seqCallbacks_.empty()) {
270             HILOGI("The callback is empty.");
271             return;
272         }
273         auto it = seqCallbacks_.find(taskId);
274         if (it == seqCallbacks_.end()) {
275             HILOGI("The callback is not found.");
276             return;
277         }
278         MechNapiCommandCallbackInfo callbackInfo = it->second;
279         HILOGI("callback info: %{public}s", callbackInfo.ToString().c_str());
280         if (result != TASK_COMPLETED) {
281             HILOGW("Task is running.");
282             return;
283         }
284         std::string taskName = EVENT_THREAD_NAME + std::to_string(taskId);
285         if (eventHandler_ != nullptr) {
286             eventHandler_->RemoveTask(taskName);
287         }
288         if (callbackInfo.willLimitChange) {
289             MechBodyControllerService::GetInstance().OnRotationAxesStatusChange(mechId_, status_);
290         }
291         if (IsLimited()) {
292             HILOGI("Task is limit.");
293             MechBodyControllerService::GetInstance()
294                 .NotifyOperationResult(callbackInfo.tokenId, callbackInfo.napiCmdId, ExecResult::LIMITED);
295         } else {
296             HILOGD("Task completed.");
297             MechBodyControllerService::GetInstance().NotifyOperationResult(callbackInfo.tokenId,
298                 callbackInfo.napiCmdId, ExecResult::COMPLETED);
299         }
300     }
301 }
302 
MechWheelZoomNotify(const std::shared_ptr<RegisterMechWheelDataCmd> & cmd)303 void MotionManager::MechWheelZoomNotify(const std::shared_ptr<RegisterMechWheelDataCmd> &cmd)
304 {
305     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
306         HILOGE("Access is not allowed if the phone is not placed on.");
307         return;
308     }
309 
310     CHECK_POINTER_RETURN(cmd, "cmd");
311     WheelData data = cmd->GetWheelData();
312     auto func = [this, data]() {
313         std::shared_ptr<CameraInfo> cameraInfo = McCameraTrackingController::GetInstance().GetCurrentCameraInfo();
314         if (cameraInfo == nullptr || !cameraInfo->isCameraOn) {
315             HILOGE("MechWheelZoomNotify current camera info is nullptr or no camera working, ignore");
316             return;
317         }
318 
319         HILOGI("MechWheelZoomNotify wheel data: degree %{public}d, speed %{public}d, speed_ratio %{public}u",
320             data.degree, data.speed, data.speed_ratio);
321         if (data.degree == 0) {
322             HILOGI("MechWheelZoomNotify degree is zero");
323             return;
324         }
325         if (wheelFilterCnt_ < WHEEL_FILTER_COUNT) {
326             HILOGI("MechWheelZoomNotify wheelFilterCnt_ is %{public}d.", wheelFilterCnt_);
327             wheelFilterCnt_++;
328             return;
329         }
330         wheelFilterCnt_ = 0;
331 
332         float currentZoom = cameraInfo->zoomFactor;
333         int32_t keyEventNum = WHEEL_ZOOM_01X_EVENT_NUM;
334         if (currentZoom > WHEEL_ZOOM_THRESHOLD + FLOAT_EPSILON) {
335             int32_t left = static_cast<int32_t>(currentZoom * DECIMAL_SCALE) % DECIMAL_SCALE;
336             HILOGI("MechWheelZoomNotify left is %{public}d, wheelCorretCnt_ %{public}d.", left, wheelCorretCnt_);
337             if (left == 0) {
338                 keyEventNum = WHEEL_ZOOM_1X_EVENT_NUM;
339                 wheelCorretCnt_ = 0;
340             } else if (wheelCorretCnt_ < WHEEL_CORRECT_COUNT) {
341                 keyEventNum = WHEEL_ZOOM_1X_EVENT_NUM;
342                 wheelCorretCnt_++;
343             } else {
344                 keyEventNum = WHEEL_ZOOM_1X_EVENT_NUM - left;
345                 wheelCorretCnt_ = 0;
346             }
347         }
348 
349         for (int32_t i = 0; i < keyEventNum; i++) {
350             data.speed > 0 ? MMIKeyEvent(CameraKeyEvent::ZOOM_OUT) : MMIKeyEvent(CameraKeyEvent::ZOOM_IN);
351         }
352     };
353     CHECK_POINTER_RETURN(eventHandler_, "eventHandler_");
354     eventHandler_->PostTask(func);
355     return;
356 }
357 
MechTrackingStatusNotify(const std::shared_ptr<RegisterMechTrackingEnableCmd> & cmd)358 void MotionManager::MechTrackingStatusNotify(const std::shared_ptr<RegisterMechTrackingEnableCmd> &cmd)
359 {
360     CHECK_POINTER_RETURN(cmd, "cmd");
361     bool isEnabled = cmd->GetIsEnabled();
362     HILOGI("Received mech tracking status notify, isEnabled: %{public}d.", isEnabled);
363     TrackingEvent trackingEvent;
364     std::shared_ptr<SetMechCameraTrackingEnableCmd> tkCmd = nullptr;
365     {
366         std::unique_lock<std::mutex> lock(deviceStatusMutex_);
367         CHECK_POINTER_RETURN(deviceStatus_, "deviceStatus_ is empty.");
368         if (!isEnabled) {
369             trackingEvent = TrackingEvent::CAMERA_TRACKING_USER_DISABLED;
370             deviceStatus_->isEnabled = false;
371             tkCmd = factory.CreateSetMechCameraTrackingEnableCmd(MechTrackingStatus::MECH_TK_DISABLE);
372         } else {
373             trackingEvent = TrackingEvent::CAMERA_TRACKING_USER_ENABLED;
374             deviceStatus_->isEnabled = true;
375             tkCmd = factory.CreateSetMechCameraTrackingEnableCmd(MechTrackingStatus::MECH_TK_ENABLE_NO_TARGET);
376         }
377     }
378     CHECK_POINTER_RETURN(sendAdapter_, "sendAdapter_ is empty.");
379     sendAdapter_->SendCommand(tkCmd);
380     McControllerManager::GetInstance().OnTrackingEvent(mechId_, trackingEvent);
381 }
382 
CreateResponseTaskId()383 uint8_t MotionManager::CreateResponseTaskId()
384 {
385     HILOGI("called");
386     std::unique_lock<std::shared_mutex> responseReadLock(taskIdMutex_);
387     if (lastTaskId_ >= UINT8_MAX) {
388         HILOGE("the taskId is full, reorder the taskId.");
389         lastTaskId_ = 0;
390         return 0;
391     }
392     uint8_t taskId = ++lastTaskId_;
393     HILOGI("end, new taskId is %{public}d", taskId);
394     return taskId;
395 }
396 
IsLimited()397 bool MotionManager::IsLimited()
398 {
399     HILOGI("isLimited.");
400     {
401         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
402         // The roll axis data is inaccurate and should not be used as the current judgment criterion.
403         if (deviceStatus_->rotationAxesStatus.pitchLimited == RotationAxisLimited::NOT_LIMITED &&
404             deviceStatus_->rotationAxesStatus.yawLimited == RotationAxisLimited::NOT_LIMITED) {
405             return false;
406         }
407     }
408     return true;
409 }
410 
IsLimited(const float & negMax,const float & posMax,const float & position)411 bool MotionManager::IsLimited(const float &negMax, const float &posMax, const float &position)
412 {
413     if (negMax < 0 && posMax < 0 && position < negMax && position > posMax) {
414         return true;
415     }
416     if (negMax > 0 && posMax > 0 && position < negMax && position > posMax) {
417         return true;
418     }
419     if (negMax < 0 && posMax > 0) {
420         if (position > 0 && position > posMax) {
421             return true;
422         }
423         if (position < 0 && position < negMax) {
424             return true;
425         }
426     }
427     return false;
428 }
429 
SetTimeout()430 void SetTimeout()
431 {
432     HILOGI("MotionManager start.");
433 }
434 
MotionManager(const std::shared_ptr<TransportSendAdapter> sendAdapter,int32_t mechId)435 MotionManager::MotionManager(const std::shared_ptr<TransportSendAdapter> sendAdapter, int32_t mechId)
436     : sendAdapter_(sendAdapter), mechId_(mechId)
437 {
438     HILOGI("MotionManager start.");
439 
440     deviceStatus_ = std::make_shared<DeviceStatus>();
441     deviceStatus_->rotateSpeedLimit.speedMin.yawSpeed = DEGREE_CIRCLED_MIN;
442     deviceStatus_->rotateSpeedLimit.speedMin.rollSpeed = DEGREE_CIRCLED_MIN;
443     deviceStatus_->rotateSpeedLimit.speedMin.pitchSpeed = DEGREE_CIRCLED_MIN;
444     deviceStatus_->rotateSpeedLimit.speedMax.yawSpeed = DEGREE_CIRCLED_MAX;
445     deviceStatus_->rotateSpeedLimit.speedMax.rollSpeed = DEGREE_CIRCLED_MIN;
446     deviceStatus_->rotateSpeedLimit.speedMax.pitchSpeed = DEGREE_CIRCLED_MIN;
447 
448     std::shared_ptr<GetMechCapabilityInfoCmd> limitCmd = factory
449             .CreateGetMechCapabilityInfoCmd();
450     CHECK_POINTER_RETURN(limitCmd, "CapabilityInfoCmd is empty.");
451     auto limitCallback = [this, limitCmd]() {
452         auto rawParams = limitCmd->GetParams();
453         auto params = rawParams;
454         {
455             std::unique_lock <std::mutex> lock(this->deviceStatusMutex_);
456             this->deviceStatus_->rotationLimit = rawParams;
457             FormatLimit(params);
458             this->deviceStatus_->rotationLimitWithOffset = params;
459         }
460         JudgingYawLimit(params);
461     };
462 
463     limitCmd->SetResponseCallback(limitCallback);
464     limitCmd->SetTimeoutCallback(SetTimeout);
465     CHECK_POINTER_RETURN(sendAdapter_, "sendAdapter_");
466     sendAdapter_->SendCommand(limitCmd);
467 
468     auto hidCmd = factory.CreateSetMechHidPreemptiveCmd(false);
469     CHECK_POINTER_RETURN(hidCmd, "hidCmd is empty.");
470     sendAdapter_->SendCommand(hidCmd, CMD_SEND_INTERVAL);
471 
472     auto tkCmd = factory.CreateSetMechCameraTrackingEnableCmd(MechTrackingStatus::MECH_TK_ENABLE_NO_TARGET);
473     CHECK_POINTER_RETURN(tkCmd, "tkCmd is empty.");
474     sendAdapter_->SendCommand(tkCmd, CMD_SEND_INTERVAL * BIT_OFFSET_2);
475 
476     PerformPresetAction(PresetAction::NOD, CMD_SEND_INTERVAL * BIT_OFFSET_3);
477 
478     eventThread_ = std::thread(&MotionManager::StartEvent, this);
479     std::unique_lock<std::mutex> lock(eventMutex_);
480     eventCon_.wait(lock, [this] {
481         return eventHandler_ != nullptr;
482     });
483     HILOGI("MotionManager end.");
484 }
485 
FormatLimit(RotateDegreeLimit & params)486 void MotionManager::FormatLimit(RotateDegreeLimit &params)
487 {
488     bool flag = params.posMax.yaw > NO_LIMIT_MAX;
489     while (flag) {
490         params.posMax.yaw -= DEGREE_CIRCLED_MAX;
491         flag = params.posMax.yaw > NO_LIMIT_MAX;
492     }
493     flag = params.posMax.yaw < NO_LIMIT_MIN;
494     while (flag) {
495         params.posMax.yaw += DEGREE_CIRCLED_MAX;
496         flag = params.posMax.yaw < NO_LIMIT_MIN;
497     }
498     flag = params.negMax.yaw > NO_LIMIT_MAX;
499     while (flag) {
500         params.negMax.yaw -= DEGREE_CIRCLED_MAX;
501         flag = params.negMax.yaw > NO_LIMIT_MAX;
502     }
503     flag = params.negMax.yaw < NO_LIMIT_MIN;
504     while (flag) {
505         params.negMax.yaw += DEGREE_CIRCLED_MAX;
506         flag = params.negMax.yaw < NO_LIMIT_MIN;
507     }
508 }
509 
RegisterEventListener()510 void MotionManager::RegisterEventListener()
511 {
512     HILOGI("RegisterEventListener start.");
513     if (mechEventListener_ == nullptr) {
514         HILOGI("RegisterReceiveListener create.");
515         mechEventListener_ = std::make_shared<MechEventListenerImpl>(shared_from_this());
516     }
517     std::shared_ptr<CommandBase> executionResultCmd = factory.CreateRegisterMechControlResultCmd();
518     CHECK_POINTER_RETURN(executionResultCmd, "ControlResultCmd is empty.");
519     {
520         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
521         notifyListenerType_.push_back(executionResultCmd->GetCmdType());
522     }
523     SubscriptionCenter::GetInstance().Subscribe(executionResultCmd->GetCmdType(), mechEventListener_);
524 
525     std::shared_ptr<CommandBase> attitudeCmd = factory.CreateRegisterMechPositionInfoCmd();
526     CHECK_POINTER_RETURN(attitudeCmd, "PositionInfoCmd is empty.");
527     {
528         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
529         notifyListenerType_.push_back(attitudeCmd->GetCmdType());
530     }
531     SubscriptionCenter::GetInstance().Subscribe(attitudeCmd->GetCmdType(), mechEventListener_);
532 
533     std::shared_ptr<CommandBase> buttonEventCmd = factory.CreateRegisterMechCameraKeyEventCmd();
534     CHECK_POINTER_RETURN(buttonEventCmd, "CameraKeyEventCmd is empty.");
535     {
536         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
537         notifyListenerType_.push_back(buttonEventCmd->GetCmdType());
538     }
539     SubscriptionCenter::GetInstance().Subscribe(buttonEventCmd->GetCmdType(), mechEventListener_);
540 
541     std::shared_ptr<CommandBase> paramCmd = factory.CreateRegisterMechStateInfoCmd();
542     CHECK_POINTER_RETURN(paramCmd, "StateInfoCmd is empty.");
543     {
544         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
545         notifyListenerType_.push_back(paramCmd->GetCmdType());
546     }
547     SubscriptionCenter::GetInstance().Subscribe(paramCmd->GetCmdType(), mechEventListener_);
548 
549     std::shared_ptr<CommandBase> wheelCmd = factory.CreateRegisterMechWheelDataCmd();
550     CHECK_POINTER_RETURN(wheelCmd, "WheelDataCmd is empty.");
551     {
552         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
553         notifyListenerType_.push_back(wheelCmd->GetCmdType());
554     }
555     SubscriptionCenter::GetInstance().Subscribe(wheelCmd->GetCmdType(), mechEventListener_);
556 
557     std::shared_ptr<CommandBase> tkEnableCmd = factory.CreateRegisterMechTrackingEnableCmd();
558     CHECK_POINTER_RETURN(tkEnableCmd, "tkEnableCmd is empty.");
559     {
560         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
561         notifyListenerType_.push_back(tkEnableCmd->GetCmdType());
562     }
563     SubscriptionCenter::GetInstance().Subscribe(tkEnableCmd->GetCmdType(), mechEventListener_);
564     HILOGI("RegisterEventListener end.");
565 }
566 
StartEvent()567 void MotionManager::StartEvent()
568 {
569     HILOGI("StartEvent start");
570     prctl(PR_SET_NAME, EVENT_THREAD_NAME.c_str());
571     auto runner = AppExecFwk::EventRunner::Create(false);
572     {
573         std::lock_guard<std::mutex> lock(eventMutex_);
574         if (runner != nullptr) {
575             HILOGE("Runner create success");
576             eventHandler_ = std::make_shared<OHOS::AppExecFwk::EventHandler>(runner);
577             eventCon_.notify_one();
578         } else {
579             HILOGE("Runner create failed");
580         }
581     }
582     if (eventHandler_) {
583         HILOGI("eventHandler_ create success");
584         CHECK_POINTER_RETURN(runner, "runner");
585         runner->Run();
586     } else {
587         HILOGI("eventHandler_ create failed");
588     }
589     HILOGI("StartEvent end");
590 }
591 
~MotionManager()592 MotionManager::~MotionManager()
593 {
594     HILOGI("~MotionManager start.");
595     if (eventHandler_ != nullptr) {
596         eventHandler_->GetEventRunner()->Stop();
597         if (eventThread_.joinable()) {
598             eventThread_.join();
599         }
600         eventHandler_ = nullptr;
601     } else {
602         HILOGE("eventHandler_ is nullptr");
603     }
604     HILOGI("~MotionManager end.");
605 }
606 
UnRegisterNotifyEvent()607 void MotionManager::UnRegisterNotifyEvent()
608 {
609     if (notifyListenerType_.empty()) {
610         HILOGE("The notifyListenerType_ is empty.");
611         return;
612     }
613     {
614         std::lock_guard<std::mutex> lock(notifyListenerMutex_);
615         for (int32_t notifyListenerType : notifyListenerType_) {
616             HILOGD("Start UnSubscribe. Type: %{public}d.", notifyListenerType);
617             SubscriptionCenter::GetInstance().UnSubscribe(notifyListenerType, mechEventListener_);
618             HILOGD("UnSubscribe over. Type: %{public}d.", notifyListenerType);
619         }
620     }
621     mechEventListener_ = nullptr;
622 }
623 
Rotate(std::shared_ptr<RotateParam> rotateParam,uint32_t & tokenId,std::string & napiCmdId)624 int32_t MotionManager::Rotate(std::shared_ptr<RotateParam> rotateParam,
625     uint32_t &tokenId, std::string &napiCmdId)
626 {
627     HILOGI("Rotate start.");
628     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
629         HILOGE("Access is not allowed if the phone is not placed on mech.");
630         return DEVICE_NOT_PLACED_ON_MECH;
631     }
632     CHECK_POINTER_RETURN_VALUE(rotateParam, INVALID_PARAMETERS_ERR, "rotateParam");
633     if (!CheckRotatePointParam(rotateParam->degree)) {
634         HILOGE("The angle exceeds the limit.");
635         MechBodyControllerService::GetInstance().NotifyOperationResult(tokenId, napiCmdId, ExecResult::SYSTEM_ERROR);
636         return INVALID_PARAMETERS_ERR;
637     }
638     MechNapiCommandCallbackInfo callbackInfo = {tokenId, napiCmdId};
639     HandelRotateParam(rotateParam, callbackInfo.willLimitChange);
640     RotateParam param = *rotateParam;
641     if (rotateParam->isRelative) {
642         HILOGI("Rotate isRelative.");
643         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
644         AbsolutelyEulerAnglesJudgingLimitLocked(param.degree);
645     }
646     uint8_t taskId = CreateResponseTaskId();
647     param.taskId = taskId;
648     std::shared_ptr<CommandBase> rotationCmd = factory.CreateSetMechRotationCmd(param);
649     CHECK_POINTER_RETURN_VALUE(rotationCmd, INVALID_PARAMETERS_ERR, "RotationCmd is empty.");
650 
651     {
652         std::unique_lock<std::mutex> lock(seqCallbackMutex_);
653         HILOGD("Insert callback id: %{public}d", taskId);
654         seqCallbacks_.insert(std::make_pair(taskId, callbackInfo));
655     }
656     sendAdapter_->SendCommand(rotationCmd);
657     auto func = [callbackInfo]() {
658         HILOGI("notify rotate timeout. tokenid: %{public}s; napicmdid: %{public}s",
659             GetAnonymUint32(callbackInfo.tokenId).c_str(), callbackInfo.napiCmdId.c_str());
660         MechBodyControllerService::GetInstance().NotifyOperationResult(callbackInfo.tokenId,
661             callbackInfo.napiCmdId, ExecResult::TIMEOUT);
662     };
663 
664     std::string taskName = EVENT_THREAD_NAME + std::to_string(taskId);
665     if (eventHandler_ != nullptr && (rotateParam->duration + RESPONSE_TIMEOUT) > 0) {
666         eventHandler_->PostTask(func, taskName, rotateParam->duration + RESPONSE_TIMEOUT);
667     }
668     return ERR_OK;
669 }
670 
671 
HandelRotateParam(std::shared_ptr<RotateParam> & rotateParam,bool & willLimitChange)672 void MotionManager::HandelRotateParam(std::shared_ptr<RotateParam> &rotateParam, bool &willLimitChange)
673 {
674     if (deviceStatus_ == nullptr) {
675         HILOGW("currentPosition is nullptr");
676         return;
677     }
678     if (rotateParam == nullptr) {
679         HILOGW("rotateParam is nullptr");
680         return;
681     }
682     EulerAngles currentPosition = deviceStatus_->eulerAngles;
683     auto limit = deviceStatus_->rotationLimitWithOffset;
684     RotationAxesStatus status = status_;
685     HILOGI("Limit is: %{public}s; YAW_OFFSET: %{public}f",
686         deviceStatus_->rotationLimitWithOffset.ToString().c_str(), YAW_OFFSET);
687     HILOGI("current position: %{public}s", currentPosition.ToString().c_str());
688     HILOGI("rotateParam before: %{public}s", rotateParam->ToString().c_str());
689 
690     float yawResult = currentPosition.yaw + rotateParam->degree.yaw;
691     float rollResult = currentPosition.roll + rotateParam->degree.roll;
692     float pitchResult = currentPosition.pitch + rotateParam->degree.pitch;
693     HILOGI("yawResult = %{public}f; rollResult = %{public}f; pitchResult = %{public}f;",
694         yawResult, rollResult, pitchResult);
695     CheckYawDegree(rotateParam, limit, yawResult);
696 
697     CheckRollDegree(rotateParam, limit, rollResult);
698 
699     CheckPitchDegree(rotateParam, limit, pitchResult);
700 
701     HILOGI("rotateParam after: %{public}s", rotateParam->ToString().c_str());
702     if (status.yawLimited != status_.yawLimited || status.rollLimited != status_.rollLimited ||
703         status.pitchLimited != status_.pitchLimited) {
704         HILOGI("limit state changed.");
705         willLimitChange = true;
706     }
707 }
708 
CheckYawDegree(std::shared_ptr<RotateParam> & rotateParam,const RotateDegreeLimit & limit,float yawResult)709 void MotionManager::CheckYawDegree(std::shared_ptr<RotateParam> &rotateParam, const RotateDegreeLimit &limit,
710     float yawResult)
711 {
712     if (rotateParam == nullptr) {
713         HILOGW("rotateParam is nullptr");
714         return;
715     }
716     yawResult += rotateParam->degree.yaw > 0 ? YAW_OFFSET : -YAW_OFFSET;
717     bool flag = yawResult < NO_LIMIT_MIN;
718     while (flag) {
719         yawResult += DEGREE_CIRCLED_MAX;
720         flag = yawResult < NO_LIMIT_MIN;
721     }
722     flag = yawResult > NO_LIMIT_MAX;
723     while (flag) {
724         yawResult -= DEGREE_CIRCLED_MAX;
725         flag = yawResult > NO_LIMIT_MAX;
726     }
727     if (IsLimited(limit.negMax.yaw, limit.posMax.yaw, yawResult)) {
728         if (rotateParam->degree.yaw > 0) {
729             rotateParam->degree.yaw -= (yawResult - limit.posMax.yaw);
730             status_.yawLimited = RotationAxisLimited::POS_LIMITED;
731         } else {
732             rotateParam->degree.yaw -= (yawResult - limit.negMax.yaw);
733             status_.yawLimited = RotationAxisLimited::NEG_LIMITED;
734         }
735     } else {
736         status_.yawLimited = RotationAxisLimited::NOT_LIMITED;
737     }
738 }
739 
CheckRollDegree(std::shared_ptr<RotateParam> & rotateParam,const RotateDegreeLimit & limit,float rollResult)740 void MotionManager::CheckRollDegree(std::shared_ptr<RotateParam> &rotateParam, const RotateDegreeLimit &limit,
741     float rollResult)
742 {
743     if (rotateParam == nullptr) {
744         HILOGW("rotateParam is nullptr");
745         return;
746     }
747     bool flag = rollResult < NO_LIMIT_MIN;
748     while (flag) {
749         rollResult += DEGREE_CIRCLED_MAX;
750         flag = rollResult < NO_LIMIT_MIN;
751     }
752     flag = rollResult > NO_LIMIT_MIN;
753     while (flag) {
754         rollResult -= DEGREE_CIRCLED_MAX;
755         flag = rollResult > NO_LIMIT_MIN;
756     }
757     if (IsLimited(limit.negMax.roll, limit.posMax.roll, rollResult)) {
758         if (rotateParam->degree.roll > 0) {
759             rotateParam->degree.roll -= (rollResult - limit.posMax.roll);
760             status_.rollLimited = RotationAxisLimited::POS_LIMITED;
761         } else {
762             rotateParam->degree.roll -= (rollResult - limit.negMax.roll);
763             status_.rollLimited = RotationAxisLimited::NEG_LIMITED;
764         }
765     } else {
766         status_.rollLimited = RotationAxisLimited::NOT_LIMITED;
767     }
768 }
769 
CheckPitchDegree(std::shared_ptr<RotateParam> & rotateParam,const RotateDegreeLimit & limit,float pitchResult)770 void MotionManager::CheckPitchDegree(std::shared_ptr<RotateParam> &rotateParam, const RotateDegreeLimit &limit,
771     float pitchResult)
772 {
773     if (rotateParam == nullptr) {
774         HILOGW("rotateParam is nullptr");
775         return;
776     }
777     bool flag = pitchResult < NO_LIMIT_MIN;
778     while (flag) {
779         pitchResult += DEGREE_CIRCLED_MAX;
780         flag = pitchResult < NO_LIMIT_MIN;
781     }
782     flag = pitchResult > NO_LIMIT_MIN;
783     while (flag) {
784         pitchResult -= DEGREE_CIRCLED_MAX;
785         flag = pitchResult > NO_LIMIT_MIN;
786     }
787     if (IsLimited(limit.negMax.pitch, limit.posMax.pitch, pitchResult)) {
788         if (rotateParam->degree.pitch > 0) {
789             rotateParam->degree.pitch -= (pitchResult - limit.posMax.pitch);
790             status_.pitchLimited = RotationAxisLimited::POS_LIMITED;
791         } else {
792             rotateParam->degree.pitch -= (pitchResult - limit.negMax.pitch);
793             status_.pitchLimited = RotationAxisLimited::NEG_LIMITED;
794         }
795     } else {
796         status_.pitchLimited = RotationAxisLimited::NOT_LIMITED;
797     }
798 }
799 
RotateBySpeed(std::shared_ptr<RotateBySpeedParam> rotateSpeedParam,uint32_t & tokenId,std::string & napiCmdId)800 int32_t MotionManager::RotateBySpeed(std::shared_ptr<RotateBySpeedParam> rotateSpeedParam,
801     uint32_t &tokenId, std::string &napiCmdId)
802 {
803     HILOGI("Rotate by speed start.");
804     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
805         HILOGE("Access is not allowed if the phone is not placed on mech.");
806         return DEVICE_NOT_PLACED_ON_MECH;
807     }
808     CHECK_POINTER_RETURN_VALUE(rotateSpeedParam, INVALID_PARAMETERS_ERR, "rotateSpeedParam");
809     MechNapiCommandCallbackInfo callbackInfo = {tokenId, napiCmdId};
810     HandelRotateSpeedParam(rotateSpeedParam, callbackInfo.willLimitChange);
811     std::shared_ptr<CommandBase> rotationBySpeedCmd =
812             factory.CreateSetMechRotationBySpeedCmd(*rotateSpeedParam);
813     CHECK_POINTER_RETURN_VALUE(rotationBySpeedCmd, INVALID_PARAMETERS_ERR, "RotationBySpeedCmd is empty.");
814     sendAdapter_->SendCommand(rotationBySpeedCmd);
815 
816     auto func = [this, &callbackInfo]() {
817         HILOGI("RotateBySpeed completed. tokenid: %{public}s; napicmdid: %{public}s",
818                GetAnonymUint32(callbackInfo.tokenId).c_str(), callbackInfo.napiCmdId.c_str());
819         MechBodyControllerService::GetInstance().NotifyOperationResult(callbackInfo.tokenId,
820             callbackInfo.napiCmdId, ExecResult::COMPLETED);
821         std::shared_ptr<CommandBase> motionCmd = factory.CreateSetMechStopCmd();
822         CHECK_POINTER_RETURN(motionCmd, "StopCmd is empty.");
823         sendAdapter_->SendCommand(motionCmd);
824     };
825     uint8_t taskId = CreateResponseTaskId();
826     std::string taskName = EVENT_THREAD_NAME + std::to_string(taskId);
827     if (eventHandler_ != nullptr && rotateSpeedParam->duration < DEFAULT_DURATION) {
828         eventHandler_->PostTask(func, taskName, rotateSpeedParam->duration);
829     }
830     HILOGI("Rotate by speed end.");
831     return ERR_OK;
832 }
833 
HandelRotateSpeedParam(std::shared_ptr<RotateBySpeedParam> & rotateBySpeedParam,bool & willLimitChange)834 void MotionManager::HandelRotateSpeedParam(std::shared_ptr<RotateBySpeedParam> &rotateBySpeedParam,
835     bool &willLimitChange)
836 {
837     if (deviceStatus_ == nullptr) {
838         HILOGW("currentPosition is nullptr");
839         return;
840     }
841     if (rotateBySpeedParam == nullptr) {
842         HILOGW("rotateBySpeedParam is nullptr");
843         return;
844     }
845     EulerAngles currentPosition = deviceStatus_->eulerAngles;
846     auto limit = deviceStatus_->rotationLimitWithOffset;
847     RotationAxesStatus status = status_;
848     HILOGI("Limit is: %{public}s; YAW_OFFSET: %{public}f",
849         deviceStatus_->rotationLimitWithOffset.ToString().c_str(), YAW_OFFSET);
850     HILOGI("current position: %{public}s", currentPosition.ToString().c_str());
851     HILOGI("rotateBySpeedParam before: %{public}s", rotateBySpeedParam->ToString().c_str());
852     HILOGI("status before: %{public}s", status.ToString().c_str());
853 
854     float duration = rotateBySpeedParam->duration;
855     float yawResult =
856         currentPosition.yaw + rotateBySpeedParam->speed.yawSpeed * duration / MILLISECONDS_TO_SECONDS;
857     float rollResult =
858         currentPosition.roll - rotateBySpeedParam->speed.rollSpeed * duration / MILLISECONDS_TO_SECONDS;
859     float pitchResult =
860         currentPosition.pitch + rotateBySpeedParam->speed.pitchSpeed * duration / MILLISECONDS_TO_SECONDS;
861     HILOGI("yawResult = %{public}f; rollResult = %{public}f; pitchResult = %{public}f;", yawResult, rollResult,
862         pitchResult);
863 
864     CheckYawSpeed(rotateBySpeedParam, limit, yawResult);
865 
866     CheckRollSpeed(rotateBySpeedParam, limit, rollResult);
867 
868     CheckPitchSpeed(rotateBySpeedParam, limit, pitchResult);
869 
870     HILOGI("status after: %{public}s", status.ToString().c_str());
871     HILOGI("rotateBySpeedParam after: %{public}s", rotateBySpeedParam->ToString().c_str());
872     if (status.yawLimited != status_.yawLimited || status.rollLimited != status_.rollLimited ||
873         status.pitchLimited != status_.pitchLimited) {
874         HILOGI("limit state changed.");
875         MechBodyControllerService::GetInstance().OnRotationAxesStatusChange(mechId_, status_);
876     }
877 }
878 
CheckYawSpeed(const std::shared_ptr<RotateBySpeedParam> & rotateBySpeedParam,const RotateDegreeLimit & limit,float yawResult)879 void MotionManager::CheckYawSpeed(const std::shared_ptr<RotateBySpeedParam> &rotateBySpeedParam,
880     const RotateDegreeLimit &limit, float yawResult)
881 {
882     if (rotateBySpeedParam == nullptr) {
883         HILOGW("rotateBySpeedParam is nullptr");
884         return;
885     }
886     yawResult += rotateBySpeedParam->speed.yawSpeed > 0 ? YAW_OFFSET : -YAW_OFFSET;
887     bool flag = yawResult < NO_LIMIT_MIN;
888     while (flag) {
889         yawResult += DEGREE_CIRCLED_MAX;
890         flag = yawResult < NO_LIMIT_MIN;
891     }
892     flag = yawResult > NO_LIMIT_MAX;
893     while (flag) {
894         yawResult -= DEGREE_CIRCLED_MAX;
895         flag = yawResult > NO_LIMIT_MAX;
896     }
897 
898     if (IsLimited(limit.negMax.yaw, limit.posMax.yaw, yawResult)) {
899         if (status_.yawLimited == RotationAxisLimited::POS_LIMITED  ||
900             status_.yawLimited == RotationAxisLimited::NEG_LIMITED) {
901             return;
902         }
903         if (rotateBySpeedParam->speed.yawSpeed > 0) {
904             status_.yawLimited = RotationAxisLimited::POS_LIMITED;
905         } else {
906             status_.yawLimited = RotationAxisLimited::NEG_LIMITED;
907         }
908     } else {
909         status_.yawLimited = RotationAxisLimited::NOT_LIMITED;
910     }
911 }
912 
CheckRollSpeed(const std::shared_ptr<RotateBySpeedParam> & rotateBySpeedParam,const RotateDegreeLimit & limit,float rollResult)913 void MotionManager::CheckRollSpeed(const std::shared_ptr<RotateBySpeedParam> &rotateBySpeedParam,
914     const RotateDegreeLimit &limit, float rollResult)
915 {
916     if (rotateBySpeedParam == nullptr) {
917         HILOGW("rotateBySpeedParam is nullptr");
918         return;
919     }
920     bool flag = rollResult < NO_LIMIT_MIN;
921     while (flag) {
922         rollResult += DEGREE_CIRCLED_MAX;
923         flag = rollResult < NO_LIMIT_MIN;
924     }
925     flag = rollResult > NO_LIMIT_MAX;
926     while (flag) {
927         rollResult -= DEGREE_CIRCLED_MAX;
928         flag = rollResult > NO_LIMIT_MAX;
929     }
930     if (IsLimited(limit.negMax.roll, limit.posMax.roll, rollResult)) {
931         if (rotateBySpeedParam->speed.rollSpeed > 0) {
932             status_.rollLimited = RotationAxisLimited::POS_LIMITED;
933         } else {
934             status_.rollLimited = RotationAxisLimited::NEG_LIMITED;
935         }
936     } else {
937         status_.rollLimited = RotationAxisLimited::NOT_LIMITED;
938     }
939 }
940 
CheckPitchSpeed(const std::shared_ptr<RotateBySpeedParam> & rotateBySpeedParam,const RotateDegreeLimit & limit,float pitchResult)941 void MotionManager::CheckPitchSpeed(const std::shared_ptr<RotateBySpeedParam> &rotateBySpeedParam,
942     const RotateDegreeLimit &limit, float pitchResult)
943 {
944     if (rotateBySpeedParam == nullptr) {
945         HILOGW("rotateBySpeedParam is nullptr");
946         return;
947     }
948     bool flag = pitchResult < NO_LIMIT_MIN;
949     while (flag) {
950         pitchResult += DEGREE_CIRCLED_MAX;
951         flag = pitchResult < NO_LIMIT_MIN;
952     }
953     flag = pitchResult > NO_LIMIT_MAX;
954     while (flag) {
955         pitchResult -= DEGREE_CIRCLED_MAX;
956         flag = pitchResult > NO_LIMIT_MAX;
957     }
958     if (IsLimited(limit.negMax.pitch, limit.posMax.pitch, pitchResult)) {
959         if (status_.pitchLimited == RotationAxisLimited::POS_LIMITED  ||
960             status_.pitchLimited == RotationAxisLimited::NEG_LIMITED) {
961             return;
962         }
963         if (rotateBySpeedParam->speed.pitchSpeed > 0) {
964             status_.pitchLimited = RotationAxisLimited::POS_LIMITED;
965         } else {
966             status_.pitchLimited = RotationAxisLimited::NEG_LIMITED;
967         }
968     } else {
969         status_.pitchLimited = RotationAxisLimited::NOT_LIMITED;
970     }
971 }
972 
StopRotate(uint32_t & tokenId,std::string & napiCmdId)973 int32_t MotionManager::StopRotate(uint32_t &tokenId, std::string &napiCmdId)
974 {
975     HILOGI("Stop rotate start.");
976     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
977         HILOGE("Access is not allowed if the phone is not placed on mech.");
978         return DEVICE_NOT_PLACED_ON_MECH;
979     }
980     std::shared_ptr<CommandBase> motionCmd = factory.CreateSetMechStopCmd();
981     CHECK_POINTER_RETURN_VALUE(motionCmd, INVALID_PARAMETERS_ERR, "StopCmd is empty.");
982     sendAdapter_->SendCommand(motionCmd);
983     MechBodyControllerService::GetInstance().NotifyOperationResult(tokenId, napiCmdId, ExecResult::COMPLETED);
984     HILOGI("Stop rotate end.");
985     return ERR_OK;
986 }
987 
GetSpeedControlTimeLimit(std::shared_ptr<TimeLimit> & timeLimit)988 int32_t MotionManager::GetSpeedControlTimeLimit(std::shared_ptr<TimeLimit> &timeLimit)
989 {
990     HILOGI("GetSpeedControlTimeLimit start.");
991     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
992         HILOGE("Access is not allowed if the phone is not placed on mech.");
993         return DEVICE_NOT_PLACED_ON_MECH;
994     }
995     if (timeLimit == nullptr) {
996         timeLimit = std::make_shared<TimeLimit>();
997     }
998     timeLimit->min = 0;
999     timeLimit->max = DEFAULT_DURATION;
1000     return ERR_OK;
1001 }
GetRotateSpeedLimit(RotateSpeedLimit & speedLimit)1002 int32_t MotionManager::GetRotateSpeedLimit(RotateSpeedLimit &speedLimit)
1003 {
1004     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1005         HILOGE("Access is not allowed if the phone is not placed on mech.");
1006         return DEVICE_NOT_PLACED_ON_MECH;
1007     }
1008     {
1009         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1010         speedLimit = deviceStatus_->rotateSpeedLimit;
1011     }
1012     return ERR_OK;
1013 }
1014 
GetCurrentPosition(std::shared_ptr<EulerAngles> & eulerAngles)1015 int32_t MotionManager::GetCurrentPosition(std::shared_ptr<EulerAngles> &eulerAngles)
1016 {
1017     HILOGI("GetCurrentPosition start.");
1018     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1019         HILOGE("Access is not allowed if the phone is not placed on mech.");
1020         return DEVICE_NOT_PLACED_ON_MECH;
1021     }
1022 
1023     {
1024         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1025         eulerAngles = std::make_shared<EulerAngles>(deviceStatus_->eulerAngles);
1026     }
1027     return ERR_OK;
1028 }
1029 
GetRotationLimit(RotateDegreeLimit & rotationLimit)1030 int32_t MotionManager::GetRotationLimit(RotateDegreeLimit &rotationLimit)
1031 {
1032     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1033         HILOGE("Access is not allowed if the phone is not placed on mech.");
1034         return DEVICE_NOT_PLACED_ON_MECH;
1035     }
1036 
1037     {
1038         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1039         rotationLimit = deviceStatus_->rotationLimit;
1040     }
1041     return ERR_OK;
1042 }
1043 
SetMechCameraTrackingFrame(const std::shared_ptr<TrackingFrameParams> trackingFrameParams)1044 int32_t MotionManager::SetMechCameraTrackingFrame(const std::shared_ptr<TrackingFrameParams> trackingFrameParams)
1045 {
1046     HILOGD("start.");
1047     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1048         HILOGE("Access is not allowed if the phone is not placed on mech.");
1049         return DEVICE_NOT_PLACED_ON_MECH;
1050     }
1051     CHECK_POINTER_RETURN_VALUE(trackingFrameParams, INVALID_PARAMETERS_ERR, "trackingFrameParams");
1052 
1053     auto cameraTrackingFrameCallback = [this]() {
1054     };
1055 
1056     if (deviceStatus_->isEnabled) {
1057         HILOGI("Start tracking.");
1058         std::shared_ptr<CommandBase> cameraTrackingFrameCmd = factory
1059                 .CreateSetMechCameraTrackingFrameCmd(*trackingFrameParams);
1060         CHECK_POINTER_RETURN_VALUE(cameraTrackingFrameCmd, INVALID_PARAMETERS_ERR, "CameraTrackingFrameCmd is empty.");
1061         cameraTrackingFrameCmd->SetResponseCallback(cameraTrackingFrameCallback);
1062         cameraTrackingFrameCmd->SetTimeoutCallback(SetTimeout);
1063         sendAdapter_->SendCommand(cameraTrackingFrameCmd);
1064     }
1065     return ERR_OK;
1066 }
1067 
GetMechCameraTrackingEnabled(bool & isEnabled)1068 int32_t MotionManager::GetMechCameraTrackingEnabled(bool &isEnabled)
1069 {
1070     HILOGI("GetCameraTrackingEnabled start.");
1071     {
1072         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1073         isEnabled = deviceStatus_->isEnabled;
1074     }
1075     return ERR_OK;
1076 }
1077 
SetMechCameraTrackingEnabled(bool & isEnabled)1078 int32_t MotionManager::SetMechCameraTrackingEnabled(bool &isEnabled)
1079 {
1080     HILOGI("SetMechCameraTrackingEnabled start.");
1081     {
1082         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1083         if (deviceStatus_ == nullptr) {
1084             HILOGE("device Status is nullptr");
1085             return ERR_OK;
1086         }
1087         deviceStatus_->isEnabled = isEnabled;
1088         HILOGI("device Status is enable: %{public}s", deviceStatus_->isEnabled ? "true" : "false");
1089     }
1090     return ERR_OK;
1091 }
1092 
SetMechCameraTrackingLayout(const std::shared_ptr<LayoutParams> layoutParams)1093 int32_t MotionManager::SetMechCameraTrackingLayout(const std::shared_ptr<LayoutParams> layoutParams)
1094 {
1095     HILOGI("SetCameraTrackingLayout start.");
1096     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1097         HILOGE("Access is not allowed if the phone is not placed on mech.");
1098         return DEVICE_NOT_PLACED_ON_MECH;
1099     }
1100     CHECK_POINTER_RETURN_VALUE(layoutParams, INVALID_PARAMETERS_ERR, "layoutParams");
1101     auto cameraTrackingLayoutCallback = [this]() {
1102     };
1103 
1104     std::shared_ptr<CommandBase> cameraTrackingLayoutCmd = factory
1105             .CreateSetMechCameraTrackingLayoutCmd(*layoutParams);
1106     CHECK_POINTER_RETURN_VALUE(cameraTrackingLayoutCmd, INVALID_PARAMETERS_ERR, "cameraTrackingLayoutCmd");
1107     cameraTrackingLayoutCmd->SetResponseCallback(cameraTrackingLayoutCallback);
1108     cameraTrackingLayoutCmd->SetTimeoutCallback(SetTimeout);
1109     sendAdapter_->SendCommand(cameraTrackingLayoutCmd);
1110     {
1111         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1112         deviceStatus_->layoutParams = *layoutParams;
1113     }
1114     HILOGI("SetCameraTrackingLayout end.");
1115     return ERR_OK;
1116 }
1117 
GetMechCameraTrackingLayout(std::shared_ptr<LayoutParams> & layoutParams)1118 int32_t MotionManager::GetMechCameraTrackingLayout(std::shared_ptr<LayoutParams> &layoutParams)
1119 {
1120     HILOGI("GetCameraTrackingLayout start.");
1121     std::shared_ptr<GetMechCameraTrackingLayoutCmd> cameraTrackingLayoutCmd = factory
1122             .CreateGetMechCameraTrackingLayoutCmd();
1123     auto cameraTrackingLayoutCallback = [this, cameraTrackingLayoutCmd]() {
1124         HILOGI("GetCameraTrackingLayout: %{public}s", cameraTrackingLayoutCmd->GetParams().ToString().c_str());
1125     };
1126 
1127     CHECK_POINTER_RETURN_VALUE(cameraTrackingLayoutCmd, INVALID_PARAMETERS_ERR, "cameraTrackingLayoutCmd");
1128     cameraTrackingLayoutCmd->SetResponseCallback(cameraTrackingLayoutCallback);
1129     cameraTrackingLayoutCmd->SetTimeoutCallback(SetTimeout);
1130     sendAdapter_->SendCommand(cameraTrackingLayoutCmd);
1131     {
1132         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1133         layoutParams = std::make_shared<LayoutParams>(deviceStatus_->layoutParams);
1134     }
1135     return ERR_OK;
1136 }
1137 
SetMechCameraInfo(const CameraInfoParams & mechCameraInfo)1138 int32_t MotionManager::SetMechCameraInfo(const CameraInfoParams &mechCameraInfo)
1139 {
1140     HILOGI("SetMechCameraInfo start.");
1141     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1142         HILOGE("Access is not allowed if the phone is not placed on mech.");
1143         return DEVICE_NOT_PLACED_ON_MECH;
1144     }
1145 
1146     std::shared_ptr<CommandBase> cameraInfoCmd = factory.CreateSetMechCameraInfoCmd(mechCameraInfo);
1147     CHECK_POINTER_RETURN_VALUE(cameraInfoCmd, INVALID_PARAMETERS_ERR, "cameraInfoCmd is empty.");
1148     sendAdapter_->SendCommand(cameraInfoCmd);
1149     HILOGI("SetMechCameraInfo end.");
1150     return ERR_OK;
1151 }
1152 
GetMechBaseInfo(std::shared_ptr<MechBaseInfo> & mechBaseInfo)1153 int32_t MotionManager::GetMechBaseInfo(std::shared_ptr<MechBaseInfo> &mechBaseInfo)
1154 {
1155     HILOGI("GetMechBaseInfo start.");
1156     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1157         HILOGE("Access is not allowed if the phone is not placed on mech.");
1158         return DEVICE_NOT_PLACED_ON_MECH;
1159     }
1160 
1161     if (deviceStatus_->mechBaseInfo.obtainable) {
1162         mechBaseInfo = std::make_shared<MechBaseInfo>(deviceStatus_->mechBaseInfo);
1163     } else {
1164         HILOGE("No basic platform information is found.");
1165         return INVALID_PARAMETERS_ERR;
1166     }
1167     return ERR_OK;
1168 }
1169 
GetMechCapabilityInfo(std::shared_ptr<MechCapabilityInfo> & mechCapabilityInfo)1170 int32_t MotionManager::GetMechCapabilityInfo(std::shared_ptr<MechCapabilityInfo> &mechCapabilityInfo)
1171 {
1172     HILOGI("GetMechCapabilityInfo start.");
1173     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1174         HILOGE("Access is not allowed if the phone is not placed on mech.");
1175         return DEVICE_NOT_PLACED_ON_MECH;
1176     }
1177 
1178     if (deviceStatus_->mechCapabilityInfo.obtainable) {
1179         mechCapabilityInfo = std::make_shared<MechCapabilityInfo>(deviceStatus_->mechCapabilityInfo);
1180     } else {
1181         HILOGE("No capability platform information is found.");
1182         return INVALID_PARAMETERS_ERR;
1183     }
1184     return ERR_OK;
1185 }
1186 
GetRotationAxesStatus(const int32_t & mechId,RotationAxesStatus & axesStatus)1187 int32_t MotionManager::GetRotationAxesStatus(const int32_t &mechId, RotationAxesStatus &axesStatus)
1188 {
1189     if (!MechConnectManager::GetInstance().GetMechState(mechId_)) {
1190         HILOGE("Access is not allowed if the phone is not placed on mech.");
1191         return DEVICE_NOT_PLACED_ON_MECH;
1192     }
1193 
1194     {
1195         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1196         axesStatus = deviceStatus_->rotationAxesStatus;
1197     }
1198     return ERR_OK;
1199 }
1200 
PerformPresetAction(PresetAction action,int32_t delay)1201 int32_t MotionManager::PerformPresetAction(PresetAction action, int32_t delay)
1202 {
1203     HILOGI("called, action %{public}d", static_cast<int32_t>(action));
1204     std::vector<RotateParam> trace;
1205     std::shared_ptr<SetMechRotationTraceCmd> setMechRotationTraceCmd = nullptr;
1206     switch (action) {
1207         case PresetAction::NOD: {
1208             for (auto action : NOD_ACTIONS) {
1209                 trace.push_back(action);
1210             }
1211             setMechRotationTraceCmd = factory.CreateSetMechRotationTraceCmd(trace);
1212             CHECK_POINTER_RETURN_VALUE(sendAdapter_, INVALID_PARAMETERS_ERR, "sendAdapter_");
1213             sendAdapter_->SendCommand(setMechRotationTraceCmd, delay);
1214             break;
1215         }
1216         default:
1217             break;
1218     }
1219     return ERR_OK;
1220 }
1221 
JudgingYawLimit(RotateDegreeLimit & limit)1222 void MotionManager::JudgingYawLimit(RotateDegreeLimit& limit)
1223 {
1224     if (limit.posMax.yaw == NO_LIMIT_MAX && limit.negMax.yaw == NO_LIMIT_MIN) {
1225         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1226         deviceStatus_->yawLimit = false;
1227     } else {
1228         std::unique_lock <std::mutex> lock(this->deviceStatusMutex_);
1229         deviceStatus_->yawLimit = true;
1230     }
1231 
1232     if (limit.posMax.pitch == NO_LIMIT_MAX && limit.negMax.pitch == NO_LIMIT_MIN) {
1233         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1234         deviceStatus_->pitchLimit = false;
1235     } else {
1236         std::unique_lock <std::mutex> lock(this->deviceStatusMutex_);
1237         deviceStatus_->pitchLimit = true;
1238     }
1239 
1240     if (limit.posMax.roll == NO_LIMIT_MAX && limit.negMax.roll == NO_LIMIT_MIN) {
1241         std::unique_lock <std::mutex> lock(deviceStatusMutex_);
1242         deviceStatus_->rollLimit = false;
1243     } else {
1244         std::unique_lock <std::mutex> lock(this->deviceStatusMutex_);
1245         deviceStatus_->rollLimit = true;
1246     }
1247 }
1248 
AbsolutelyEulerAnglesJudgingLimitLocked(EulerAngles & eulerAngles)1249 void MotionManager::AbsolutelyEulerAnglesJudgingLimitLocked(EulerAngles& eulerAngles)
1250 {
1251     HILOGI("called begin, EulerAngles %{public}s", eulerAngles.ToString().c_str());
1252     if (deviceStatus_ == nullptr) {
1253         HILOGW("deviceStatus_ is empty.");
1254         return;
1255     }
1256     eulerAngles.yaw += deviceStatus_->eulerAngles.yaw;
1257     eulerAngles.roll = 0;
1258     eulerAngles.pitch += deviceStatus_->eulerAngles.pitch;
1259     auto status = deviceStatus_->rotationAxesStatus;
1260     auto limit = deviceStatus_->rotationLimit;
1261     HILOGI("called end, EulerAngles %{public}s", eulerAngles.ToString().c_str());
1262 }
1263 
LimitCalculationLocked(EulerAngles & position,RotationAxesStatus & status,bool callback)1264 void MotionManager::LimitCalculationLocked(EulerAngles& position, RotationAxesStatus& status, bool callback)
1265 {
1266     HILOGI("called begin, EulerAngles %{public}s", position.ToString().c_str());
1267     if (this->deviceStatus_ == nullptr) {
1268         HILOGE("deviceStatus_ is nullptr, can not get limit info");
1269         return;
1270     }
1271     float roll = position.roll;
1272     float pitch = position.pitch;
1273     float yaw = position.yaw;
1274     auto limit = this->deviceStatus_->rotationLimitWithOffset;
1275     HILOGI("Limit is: %{public}s; YAW_OFFSET: %{public}f",
1276         this->deviceStatus_->rotationLimitWithOffset.ToString().c_str(), YAW_OFFSET);
1277     if (deviceStatus_->rollLimit) {
1278         if (std::abs(roll - limit.posMax.roll) <= YAW_DISCREPANCY) {
1279             status.rollLimited = RotationAxisLimited::POS_LIMITED;
1280         } else if (std::abs(roll - limit.negMax.roll) <= YAW_DISCREPANCY) {
1281             status.rollLimited = RotationAxisLimited::NEG_LIMITED;
1282         } else {
1283             status.rollLimited = RotationAxisLimited::NOT_LIMITED;
1284         }
1285     }
1286     if (deviceStatus_->pitchLimit) {
1287         if (pitch > limit.posMax.pitch) {
1288             status.pitchLimited = RotationAxisLimited::POS_LIMITED;
1289         } else if (pitch < limit.negMax.pitch) {
1290             status.pitchLimited = RotationAxisLimited::NEG_LIMITED;
1291         } else {
1292             status.pitchLimited = RotationAxisLimited::NOT_LIMITED;
1293         }
1294     }
1295     if (deviceStatus_->yawLimit) {
1296         float yawOff = yaw - YAW_OFFSET < NO_LIMIT_MIN ? yaw - YAW_OFFSET + DEGREE_CIRCLED_MAX : yaw - YAW_OFFSET;
1297         if (yawOff > limit.posMax.yaw) {
1298             status.yawLimited = RotationAxisLimited::POS_LIMITED;
1299         } else if (yawOff < limit.negMax.yaw) {
1300             status.yawLimited = RotationAxisLimited::NEG_LIMITED;
1301         } else {
1302             status.yawLimited = RotationAxisLimited::NOT_LIMITED;
1303         }
1304     }
1305     if (callback && deviceStatus_->rotationAxesStatus != status) {
1306         deviceStatus_->rotationAxesStatus = status;
1307     }
1308 }
1309 
MechEventListenerImpl(std::shared_ptr<MotionManager> motionManager)1310 MechEventListenerImpl::MechEventListenerImpl(std::shared_ptr<MotionManager> motionManager)
1311 {
1312     HILOGI("MechEventListenerImpl called");
1313     motionManager_ = motionManager;
1314 }
1315 
~MechEventListenerImpl()1316 MechEventListenerImpl::~MechEventListenerImpl()
1317 {
1318     HILOGI("MechEventListenerImpl called");
1319 }
1320 
MechAttitudeNotify(const std::shared_ptr<RegisterMechPositionInfoCmd> & cmd)1321 void MechEventListenerImpl::MechAttitudeNotify(const std::shared_ptr<RegisterMechPositionInfoCmd> &cmd)
1322 {
1323     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1324     motionManager_->MechAttitudeNotify(cmd);
1325 }
1326 
MechButtonEventNotify(const std::shared_ptr<RegisterMechCameraKeyEventCmd> & cmd)1327 void MechEventListenerImpl::MechButtonEventNotify(const std::shared_ptr<RegisterMechCameraKeyEventCmd> &cmd)
1328 {
1329     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1330     motionManager_->MechButtonEventNotify(cmd);
1331 }
1332 
MechParamNotify(const std::shared_ptr<RegisterMechStateInfoCmd> & cmd)1333 void MechEventListenerImpl::MechParamNotify(const std::shared_ptr<RegisterMechStateInfoCmd> &cmd)
1334 {
1335     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1336     motionManager_->MechParamNotify(cmd);
1337 }
1338 
MechExecutionResultNotify(const std::shared_ptr<RegisterMechControlResultCmd> & cmd)1339 void MechEventListenerImpl::MechExecutionResultNotify(const std::shared_ptr<RegisterMechControlResultCmd> &cmd)
1340 {
1341     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1342     motionManager_->MechExecutionResultNotify(cmd);
1343 }
1344 
MechWheelZoomNotify(const std::shared_ptr<RegisterMechWheelDataCmd> & cmd)1345 void MechEventListenerImpl::MechWheelZoomNotify(const std::shared_ptr<RegisterMechWheelDataCmd> &cmd)
1346 {
1347     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1348     motionManager_->MechWheelZoomNotify(cmd);
1349 }
1350 
MechTrackingStatusNotify(const std::shared_ptr<RegisterMechTrackingEnableCmd> & cmd)1351 void MechEventListenerImpl::MechTrackingStatusNotify(const std::shared_ptr<RegisterMechTrackingEnableCmd> &cmd)
1352 {
1353     CHECK_POINTER_RETURN(motionManager_, "motionManager_");
1354     motionManager_->MechTrackingStatusNotify(cmd);
1355 }
1356 
MechNapiCommandCallbackInfo(uint32_t & tokenId,std::string & napiCmdId)1357 MechNapiCommandCallbackInfo::MechNapiCommandCallbackInfo(uint32_t &tokenId, std::string &napiCmdId)
1358     : tokenId(tokenId), napiCmdId(napiCmdId)
1359 {
1360 }
1361 } // namespace MechBodyController
1362 } // namespace OHOS
1363