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_set_mech_rotation_cmd.h"
17
18 #include "mechbody_controller_log.h"
19
20 namespace OHOS {
21 namespace MechBodyController {
22 namespace {
23 const std::string TAG = "SetMechRotationCmd";
24 constexpr uint8_t CMD_SET_ROTATE_TYPE = 0x04;
25 constexpr uint8_t CMD_SET_ROTATE_SIZE = 5 + 13 * 2;
26 constexpr uint8_t CMD_SET_ROTATE_CONTROL_IRRALEVENT = 0b00000100;
27 constexpr uint8_t CMD_SET_ROTATE_PITCH = 0x01;
28 constexpr uint8_t CMD_SET_ROTATE_YAW = 0x02;
29 constexpr float CMD_SET_ROTATE_MS_TO_S = 1000.0f;
30 }
31
SetMechRotationCmd(const RotateParam & params)32 SetMechRotationCmd::SetMechRotationCmd(const RotateParam& params)
33 : params_(params)
34 {
35 cmdSet_ = CMD_SET;
36 cmdId_ = CMD_ID;
37 reqSize_ = REQ_SIZE;
38 rspSize_ = RSP_SIZE;
39 needResponse_ = (RSP_SIZE > 0);
40 timeoutMs_ = MECHBODY_MSG_TIMEOUT;
41 retryTimes_ = CMD_PRIORITY_MIDDLE;
42 }
43
Marshal() const44 std::shared_ptr<MechDataBuffer> SetMechRotationCmd::Marshal() const
45 {
46 HILOGI("start.");
47 auto buffer = std::make_shared<MechDataBuffer>(reqSize_ + BIT_OFFSET_2);
48 if (buffer == nullptr) {
49 HILOGE("Failed to allocate memory for Marshal buffer");
50 return nullptr;
51 }
52
53 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(cmdSet_), nullptr, "append cmdSet_");
54 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(cmdId_), nullptr, "append cmdId_");
55
56 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_TYPE), nullptr, "append tlv type");
57 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_SIZE), nullptr, "append tlv size");
58
59 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_CONTROL_IRRALEVENT), nullptr, "append control byte");
60
61 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(BIT_0), nullptr, "append reserve byte");
62 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(BIT_0), nullptr, "append reserve byte");
63 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(BIT_2), nullptr, "append point number");
64 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_PITCH), nullptr, "append pitch sign");
65 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.duration / CMD_SET_ROTATE_MS_TO_S), nullptr, "append duration");
66 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.degree.pitch), nullptr, "append pitch");
67 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(0.0f), nullptr, "append reserve float");
68 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_YAW), nullptr, "append yaw sign");
69 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.duration / CMD_SET_ROTATE_MS_TO_S), nullptr, "append duration");
70 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.degree.yaw), nullptr, "append yaw");
71 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(0.0f), nullptr, "append reserve float");
72 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(params_.taskId), nullptr, "append taskId");
73
74 HILOGI("end.");
75 return buffer;
76 }
77
TriggerResponse(std::shared_ptr<MechDataBuffer> data)78 void SetMechRotationCmd::TriggerResponse(std::shared_ptr<MechDataBuffer> data)
79 {
80 if (data == nullptr || data->Size() < RSP_SIZE + BIT_OFFSET_2) {
81 HILOGE("Invalid input data for response");
82 return;
83 }
84
85 CHECK_ERR_RETURN(data->ReadUint8(BIT_OFFSET_2, result_), "read result_");
86 HILOGI("response code: %{public}u", result_);
87
88 if (responseCb_) {
89 HILOGI("trigger response callback.");
90 responseCb_();
91 }
92 }
93
GetParams() const94 const RotateParam& SetMechRotationCmd::GetParams() const
95 {
96 return params_;
97 }
98
GetResult() const99 uint8_t SetMechRotationCmd::GetResult() const
100 {
101 return result_;
102 }
103 } // namespace MechBodyController
104 } // namespace OHOS
105