71 lines
1.9 KiB
C++
71 lines
1.9 KiB
C++
#include "core/tcm_device.hpp"
|
|
|
|
#include <algorithm>
|
|
|
|
namespace mag160c::core {
|
|
namespace {
|
|
|
|
constexpr uint8_t MAIN_TCM = 0x02;
|
|
constexpr uint8_t SUB_ROTATE = 0x77;
|
|
constexpr uint8_t SUB_LIGHT_STEADY = 0x31;
|
|
constexpr uint8_t SUB_LIGHT_BLINK = 0x32;
|
|
constexpr uint8_t SUB_LIGHT_BREATH = 0x33;
|
|
|
|
uint8_t light_subcommand(mag160c_tcm_light_mode_t mode) {
|
|
switch (mode) {
|
|
case MAG160C_TCM_LIGHT_STEADY:
|
|
return SUB_LIGHT_STEADY;
|
|
case MAG160C_TCM_LIGHT_BLINK:
|
|
return SUB_LIGHT_BLINK;
|
|
case MAG160C_TCM_LIGHT_BREATH:
|
|
return SUB_LIGHT_BREATH;
|
|
}
|
|
return SUB_LIGHT_STEADY;
|
|
}
|
|
|
|
std::vector<uint8_t> light_payload(mag160c_tcm_light_color_t color) {
|
|
switch (color) {
|
|
case MAG160C_TCM_LIGHT_OFF:
|
|
return {0x00, 0x00, 0x00, 0x00};
|
|
case MAG160C_TCM_LIGHT_RED:
|
|
return {0x01, 0xff, 0x00, 0x00};
|
|
case MAG160C_TCM_LIGHT_GREEN:
|
|
return {0x01, 0x00, 0xff, 0x00};
|
|
case MAG160C_TCM_LIGHT_BLUE:
|
|
return {0x01, 0x00, 0x00, 0xff};
|
|
case MAG160C_TCM_LIGHT_YELLOW:
|
|
return {0x01, 0xff, 0xff, 0x00};
|
|
}
|
|
return {0x00, 0x00, 0x00, 0x00};
|
|
}
|
|
|
|
} // namespace
|
|
|
|
TcmCommandBuilder::TcmCommandBuilder() : frame_id_(1) {}
|
|
|
|
uint16_t TcmCommandBuilder::next_frame_id() {
|
|
const uint16_t current = frame_id_;
|
|
++frame_id_;
|
|
if (frame_id_ >= 0x8000) {
|
|
frame_id_ = 1;
|
|
}
|
|
return current;
|
|
}
|
|
|
|
std::vector<uint8_t> TcmCommandBuilder::build_rotate_frame(int angle) {
|
|
angle = std::clamp(angle, -128, 128);
|
|
|
|
const uint8_t direction = angle < 0 ? 0x01 : 0x00;
|
|
const uint8_t magnitude = static_cast<uint8_t>(angle < 0 ? -angle : angle);
|
|
return encode_tcm_frame(MAIN_TCM, SUB_ROTATE, next_frame_id(), {direction, magnitude});
|
|
}
|
|
|
|
std::vector<uint8_t> TcmCommandBuilder::build_light_frame(
|
|
mag160c_tcm_light_color_t color,
|
|
mag160c_tcm_light_mode_t mode
|
|
) {
|
|
return encode_tcm_frame(MAIN_TCM, light_subcommand(mode), next_frame_id(), light_payload(color));
|
|
}
|
|
|
|
} // namespace mag160c::core
|