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