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_by_speed_cmd.h"
17
18 #include "mechbody_controller_log.h"
19
20 namespace OHOS {
21 namespace MechBodyController {
22 namespace {
23 const std::string TAG = "SetMechRotationBySpeedCmd";
24 constexpr uint8_t CMD_SET_ROTATE_BY_SPEED_CONTROL = 0b10000000;
25 constexpr uint8_t CMD_SET_ROTATE_BY_SPEED_CONTROL_EXTEND = 0x1;
26 }
27
SetMechRotationBySpeedCmd(const RotateBySpeedParam & params)28 SetMechRotationBySpeedCmd::SetMechRotationBySpeedCmd(const RotateBySpeedParam& params)
29 : params_(params)
30 {
31 cmdSet_ = CMD_SET;
32 cmdId_ = CMD_ID;
33 reqSize_ = REQ_SIZE;
34 rspSize_ = RSP_SIZE;
35 needResponse_ = (RSP_SIZE > 0);
36 timeoutMs_ = MECHBODY_MSG_TIMEOUT;
37 retryTimes_ = CMD_PRIORITY_MIDDLE;
38 }
39
Marshal() const40 std::shared_ptr<MechDataBuffer> SetMechRotationBySpeedCmd::Marshal() const
41 {
42 HILOGW("start.");
43 auto buffer = std::make_shared<MechDataBuffer>(reqSize_ + BIT_OFFSET_2);
44 if (buffer == nullptr) {
45 HILOGE("Failed to allocate memory for Marshal buffer");
46 return nullptr;
47 }
48
49 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(cmdSet_), nullptr, "append cmdSet_");
50 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(cmdId_), nullptr, "append cmdId_");
51
52 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.speed.yawSpeed), nullptr, "append yawSpeed");
53 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.speed.rollSpeed), nullptr, "append rollSpeed");
54 CHECK_ERR_RETURN_VALUE(buffer->AppendFloat(params_.speed.pitchSpeed), nullptr, "append pitchSpeed");
55 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_BY_SPEED_CONTROL), nullptr, "append control byte");
56 CHECK_ERR_RETURN_VALUE(buffer->AppendUint8(CMD_SET_ROTATE_BY_SPEED_CONTROL_EXTEND), nullptr, "append extend");
57
58 HILOGW("end.");
59 return buffer;
60 }
61
TriggerResponse(std::shared_ptr<MechDataBuffer> data)62 void SetMechRotationBySpeedCmd::TriggerResponse(std::shared_ptr<MechDataBuffer> data)
63 {
64 if (data == nullptr || data->Size() < RSP_SIZE + BIT_OFFSET_2) {
65 HILOGE("Invalid input data for response");
66 return;
67 }
68
69 CHECK_ERR_RETURN(data->ReadUint8(BIT_OFFSET_2, result_), "read result_");
70 HILOGI("response code: %{public}u", result_);
71
72 if (responseCb_) {
73 HILOGI("trigger response callback.");
74 responseCb_();
75 }
76 }
77
GetParams() const78 const RotateBySpeedParam& SetMechRotationBySpeedCmd::GetParams() const
79 {
80 return params_;
81 }
82
GetResult() const83 uint8_t SetMechRotationBySpeedCmd::GetResult() const
84 {
85 return result_;
86 }
87 } // namespace MechBodyController
88 } // namespace OHOS
89