290 lines
8.2 KiB
C++
290 lines
8.2 KiB
C++
#include "gimbal.hpp"
|
|
|
|
#include "src/component/crc.hpp"
|
|
#include "src/component/logger.hpp"
|
|
#include "src/component/math_tools.hpp"
|
|
#include "src/component/yaml.hpp"
|
|
|
|
namespace device
|
|
{
|
|
Gimbal::Gimbal(const std::string & config_path)
|
|
{
|
|
auto yaml = component::load(config_path);
|
|
auto com_port = component::read<std::string>(yaml, "com_port");
|
|
auto baudrate = component::read<int>(yaml, "baudrate");
|
|
|
|
try {
|
|
serial_.setPort(com_port);
|
|
serial_.setBaudrate(baudrate);
|
|
serial_.setTimeout(serial::Timeout::max(), 100, 0, 100, 0);
|
|
serial_.open();
|
|
component::logger()->info("[Gimbal] Serial port {} opened at {} baud", com_port, baudrate);
|
|
} catch (const std::exception & e) {
|
|
component::logger()->error("[Gimbal] Failed to open serial: {}", e.what());
|
|
exit(1);
|
|
}
|
|
|
|
thread_ = std::thread(&Gimbal::read_thread, this);
|
|
|
|
queue_.pop();
|
|
component::logger()->info("[Gimbal] First q received.");
|
|
}
|
|
|
|
Gimbal::~Gimbal()
|
|
{
|
|
quit_ = true;
|
|
if (thread_.joinable()) thread_.join();
|
|
serial_.close();
|
|
}
|
|
|
|
GimbalMode Gimbal::mode() const
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return mode_;
|
|
}
|
|
|
|
GimbalState Gimbal::state() const
|
|
{
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
return state_;
|
|
}
|
|
|
|
std::string Gimbal::str(GimbalMode mode) const
|
|
{
|
|
switch (mode) {
|
|
case GimbalMode::IDLE:
|
|
return "IDLE";
|
|
case GimbalMode::AUTO_AIM:
|
|
return "AUTO_AIM";
|
|
case GimbalMode::SMALL_BUFF:
|
|
return "SMALL_BUFF";
|
|
case GimbalMode::BIG_BUFF:
|
|
return "BIG_BUFF";
|
|
default:
|
|
return "INVALID";
|
|
}
|
|
}
|
|
|
|
Eigen::Quaterniond Gimbal::q(std::chrono::steady_clock::time_point t)
|
|
{
|
|
while (true) {
|
|
auto [q_a, t_a] = queue_.pop();
|
|
auto [q_b, t_b] = queue_.front();
|
|
auto t_ab = component::delta_time(t_a, t_b);
|
|
auto t_ac = component::delta_time(t_a, t);
|
|
auto k = t_ac / t_ab;
|
|
Eigen::Quaterniond q_c = q_a.slerp(k, q_b).normalized();
|
|
if (t < t_a) return q_c;
|
|
if (!(t_a < t && t <= t_b)) continue;
|
|
|
|
return q_c;
|
|
}
|
|
}
|
|
|
|
void Gimbal::send(device::VisionToGimbal VisionToGimbal)
|
|
{
|
|
tx_data_.mode = VisionToGimbal.mode;
|
|
tx_data_.yaw = VisionToGimbal.yaw;
|
|
tx_data_.yaw_vel = VisionToGimbal.yaw_vel;
|
|
tx_data_.yaw_acc = VisionToGimbal.yaw_acc;
|
|
tx_data_.pitch = VisionToGimbal.pitch;
|
|
tx_data_.pitch_vel = VisionToGimbal.pitch_vel;
|
|
tx_data_.pitch_acc = VisionToGimbal.pitch_acc;
|
|
tx_data_.crc16 = component::get_crc16(
|
|
reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));
|
|
|
|
try {
|
|
serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
|
|
} catch (const std::exception & e) {
|
|
component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
|
|
}
|
|
}
|
|
|
|
void Gimbal::send(
|
|
bool control, bool fire, float yaw, float yaw_vel, float yaw_acc, float pitch, float pitch_vel,
|
|
float pitch_acc)
|
|
{
|
|
tx_data_.mode = control ? (fire ? 2 : 1) : 0;
|
|
tx_data_.yaw = yaw;
|
|
tx_data_.yaw_vel = yaw_vel;
|
|
tx_data_.yaw_acc = yaw_acc;
|
|
tx_data_.pitch = pitch;
|
|
tx_data_.pitch_vel = pitch_vel;
|
|
tx_data_.pitch_acc = pitch_acc;
|
|
tx_data_.crc16 = component::get_crc16(
|
|
reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_) - sizeof(tx_data_.crc16));
|
|
|
|
try {
|
|
serial_.write(reinterpret_cast<uint8_t *>(&tx_data_), sizeof(tx_data_));
|
|
} catch (const std::exception & e) {
|
|
component::logger()->warn("[Gimbal] Failed to write serial: {}", e.what());
|
|
}
|
|
}
|
|
|
|
bool Gimbal::read(uint8_t * buffer, size_t size)
|
|
{
|
|
try {
|
|
return serial_.read(buffer, size) == size;
|
|
} catch (const std::exception & e) {
|
|
// component::logger()->warn("[Gimbal] Failed to read serial: {}", e.what());
|
|
return false;
|
|
}
|
|
}
|
|
|
|
void Gimbal::read_thread()
|
|
{
|
|
component::logger()->info("[Gimbal] read_thread started.");
|
|
int error_count = 0;
|
|
uint8_t byte;
|
|
int total_bytes_read = 0;
|
|
int valid_packets = 0;
|
|
|
|
while (!quit_) {
|
|
if (error_count > 5000) {
|
|
error_count = 0;
|
|
component::logger()->warn("[Gimbal] Too many errors (read {} bytes, {} valid packets), attempting to reconnect...",
|
|
total_bytes_read, valid_packets);
|
|
reconnect();
|
|
continue;
|
|
}
|
|
|
|
// 逐字节查找包头第一个字节 'M'
|
|
if (!read(&byte, 1)) {
|
|
error_count++;
|
|
continue;
|
|
}
|
|
|
|
// 读取成功，重置错误计数
|
|
error_count = 0;
|
|
total_bytes_read++;
|
|
|
|
if (byte != 'M') continue;
|
|
|
|
// 读取第二个字节检查是否为 'R'
|
|
if (!read(&byte, 1)) {
|
|
error_count++;
|
|
continue;
|
|
}
|
|
|
|
total_bytes_read++;
|
|
|
|
if (byte != 'R') {
|
|
if (valid_packets < 3) {
|
|
component::logger()->debug("[Gimbal] Found 'M' but next byte is 0x{:02X}, not 'R'", byte);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// 找到包头，记录时间戳
|
|
rx_data_.head[0] = 'M';
|
|
rx_data_.head[1] = 'R';
|
|
auto t = std::chrono::steady_clock::now();
|
|
|
|
// 读取剩余数据
|
|
if (!read(
|
|
reinterpret_cast<uint8_t *>(&rx_data_) + sizeof(rx_data_.head),
|
|
sizeof(rx_data_) - sizeof(rx_data_.head))) {
|
|
error_count++;
|
|
component::logger()->warn("[Gimbal] Failed to read packet body");
|
|
continue;
|
|
}
|
|
|
|
// 验证数据合理性
|
|
if (rx_data_.mode > 3) {
|
|
// mode 应该在 0-3 范围内
|
|
if (valid_packets < 10) {
|
|
component::logger()->warn("[Gimbal] Invalid mode {}, skipping packet (possible misalignment)", rx_data_.mode);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// 验证四元数范数是否接近1
|
|
float q_norm = rx_data_.q[0] * rx_data_.q[0] +
|
|
rx_data_.q[1] * rx_data_.q[1] +
|
|
rx_data_.q[2] * rx_data_.q[2] +
|
|
rx_data_.q[3] * rx_data_.q[3];
|
|
if (q_norm < 0.9f || q_norm > 1.1f) {
|
|
if (valid_packets < 10) {
|
|
component::logger()->warn("[Gimbal] Invalid quaternion norm {:.3f}, skipping packet", q_norm);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
total_bytes_read += sizeof(rx_data_) - sizeof(rx_data_.head);
|
|
valid_packets++;
|
|
|
|
if (valid_packets <= 5) {
|
|
component::logger()->info("[Gimbal] Packet #{}: mode={}, q=[{:.3f},{:.3f},{:.3f},{:.3f}], yaw={:.3f}",
|
|
valid_packets, (int)rx_data_.mode,
|
|
(float)rx_data_.q[0], (float)rx_data_.q[1], (float)rx_data_.q[2], (float)rx_data_.q[3],
|
|
(float)rx_data_.yaw);
|
|
} else if (valid_packets % 100 == 0) {
|
|
// 每100个包打印一次状态
|
|
component::logger()->info("[Gimbal] Received {} packets, total {} bytes", valid_packets, total_bytes_read);
|
|
}
|
|
|
|
// if (!component::check_crc16(reinterpret_cast<uint8_t *>(&rx_data_), sizeof(rx_data_))) {
|
|
// component::logger()->debug("[Gimbal] CRC16 check failed.");
|
|
// continue;
|
|
// }
|
|
|
|
error_count = 0;
|
|
Eigen::Quaterniond q(rx_data_.q[0], rx_data_.q[1], rx_data_.q[2], rx_data_.q[3]);
|
|
queue_.push({q, t});
|
|
|
|
std::lock_guard<std::mutex> lock(mutex_);
|
|
|
|
state_.yaw = rx_data_.yaw;
|
|
state_.yaw_vel = rx_data_.yaw_vel;
|
|
state_.pitch = rx_data_.pitch;
|
|
state_.pitch_vel = rx_data_.pitch_vel;
|
|
state_.bullet_speed = rx_data_.bullet_speed;
|
|
state_.bullet_count = rx_data_.bullet_count;
|
|
|
|
switch (rx_data_.mode) {
|
|
case 0:
|
|
mode_ = GimbalMode::IDLE;
|
|
break;
|
|
case 1:
|
|
mode_ = GimbalMode::AUTO_AIM;
|
|
break;
|
|
case 2:
|
|
mode_ = GimbalMode::SMALL_BUFF;
|
|
break;
|
|
case 3:
|
|
mode_ = GimbalMode::BIG_BUFF;
|
|
break;
|
|
default:
|
|
mode_ = GimbalMode::IDLE;
|
|
component::logger()->warn("[Gimbal] Invalid mode: {}", rx_data_.mode);
|
|
break;
|
|
}
|
|
}
|
|
|
|
component::logger()->info("[Gimbal] read_thread stopped.");
|
|
}
|
|
|
|
void Gimbal::reconnect()
|
|
{
|
|
int max_retry_count = 10;
|
|
for (int i = 0; i < max_retry_count && !quit_; ++i) {
|
|
component::logger()->warn("[Gimbal] Reconnecting serial, attempt {}/{}...", i + 1, max_retry_count);
|
|
try {
|
|
serial_.close();
|
|
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
} catch (...) {
|
|
}
|
|
|
|
try {
|
|
serial_.open(); // 尝试重新打开
|
|
queue_.clear();
|
|
component::logger()->info("[Gimbal] Reconnected serial successfully.");
|
|
break;
|
|
} catch (const std::exception & e) {
|
|
component::logger()->warn("[Gimbal] Reconnect failed: {}", e.what());
|
|
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace device
|