539 lines
18 KiB
C++
539 lines
18 KiB
C++
#include "uv_warning_device.h"
|
|
#include <QtGlobal>
|
|
#include <QDebug>
|
|
#include <QMetaType>
|
|
|
|
namespace payload_protocol {
|
|
|
|
// UvDownlinkData 实现
|
|
|
|
UvDownlinkData::UvDownlinkData()
|
|
: integration_time(0), average_brightness(0),
|
|
intensity_threshold(0), range_threshold(0) {
|
|
warning_flags.has_target = 0;
|
|
warning_flags.target_position = 0;
|
|
warning_flags.reserved = 0;
|
|
|
|
device_status1.exposure_mode = 0;
|
|
device_status1.contrast_mode = 0;
|
|
device_status1.gamma_mode = 0;
|
|
device_status1.dynamic_range_mode = 0;
|
|
device_status1.tracker_serial_status = 0;
|
|
device_status1.camera_serial_status = 0;
|
|
device_status1.flight_controller_status = 0;
|
|
device_status1.image_output_status = 0;
|
|
|
|
reserved_bytes.fill(0, 17);
|
|
}
|
|
|
|
QByteArray UvDownlinkData::serialize() const {
|
|
QByteArray data(UV_DOWNLINK_FRAME_LENGTH, 0);
|
|
|
|
// 设置帧头
|
|
data[0] = static_cast<char>(UV_HEADER1);
|
|
data[1] = static_cast<char>(UV_HEADER2);
|
|
|
|
// 设备识别码
|
|
data[2] = static_cast<char>(device_id);
|
|
|
|
// 告警标志字节
|
|
data[3] = static_cast<char>((warning_flags.has_target & 0x01) |
|
|
((warning_flags.target_position & 0x03) << 1) |
|
|
((warning_flags.reserved & 0x1F) << 3));
|
|
|
|
// 积分时间（3字节，小端序）
|
|
data[4] = static_cast<char>(integration_time & 0xFF);
|
|
data[5] = static_cast<char>((integration_time >> 8) & 0xFF);
|
|
data[6] = static_cast<char>((integration_time >> 16) & 0xFF);
|
|
|
|
// 图像平均亮度（2字节，小端序）
|
|
data[7] = static_cast<char>(average_brightness & 0xFF);
|
|
data[8] = static_cast<char>((average_brightness >> 8) & 0xFF);
|
|
|
|
// 设备状态1
|
|
data[9] = static_cast<char>((device_status1.exposure_mode & 0x01) |
|
|
((device_status1.contrast_mode & 0x01) << 1) |
|
|
((device_status1.gamma_mode & 0x01) << 2) |
|
|
((device_status1.dynamic_range_mode & 0x01) << 3) |
|
|
((device_status1.tracker_serial_status & 0x01) << 4) |
|
|
((device_status1.camera_serial_status & 0x01) << 5) |
|
|
((device_status1.flight_controller_status & 0x01) << 6) |
|
|
((device_status1.image_output_status & 0x01) << 7));
|
|
|
|
// 设备状态2和3
|
|
data[10] = static_cast<char>(intensity_threshold);
|
|
data[11] = static_cast<char>(range_threshold);
|
|
|
|
// 保留字节
|
|
for (qint32 i = 0; i < reserved_bytes.size() && i < 17; ++i) {
|
|
data[12 + i] = reserved_bytes.at(i);
|
|
}
|
|
|
|
// 计算校验和（前29个字节累加和，取低8位）
|
|
quint8 checksum = 0;
|
|
for (qint32 i = 0; i < UV_DOWNLINK_FRAME_LENGTH - 1; ++i) {
|
|
checksum += static_cast<quint8>(data.at(i));
|
|
}
|
|
data[UV_DOWNLINK_FRAME_LENGTH - 1] = static_cast<char>(checksum);
|
|
|
|
return data;
|
|
}
|
|
|
|
bool UvDownlinkData::deserialize(const QByteArray& data) {
|
|
if (data.size() < UV_DOWNLINK_FRAME_LENGTH) {
|
|
return false;
|
|
}
|
|
|
|
// 验证帧头
|
|
if (static_cast<quint8>(data[0]) != UV_HEADER1 || static_cast<quint8>(data[1]) != UV_HEADER2) {
|
|
return false;
|
|
}
|
|
|
|
// 验证设备识别码
|
|
if (static_cast<quint8>(data[2]) != UV_WARNING_DEVICE_ID) {
|
|
return false;
|
|
}
|
|
|
|
// 验证校验和
|
|
if (!validateChecksum(data)) {
|
|
return false;
|
|
}
|
|
|
|
// 解析告警标志字节
|
|
quint8 byte3 = static_cast<quint8>(data[3]);
|
|
warning_flags.has_target = byte3 & 0x01;
|
|
warning_flags.target_position = (byte3 >> 1) & 0x03;
|
|
warning_flags.reserved = (byte3 >> 3) & 0x1F;
|
|
|
|
// 解析积分时间（3字节，小端序）
|
|
integration_time = static_cast<quint32>(static_cast<quint8>(data[4])) |
|
|
(static_cast<quint32>(static_cast<quint8>(data[5])) << 8) |
|
|
(static_cast<quint32>(static_cast<quint8>(data[6])) << 16);
|
|
|
|
// 解析图像平均亮度（2字节，小端序）
|
|
average_brightness = static_cast<quint16>(static_cast<quint8>(data[7])) |
|
|
(static_cast<quint16>(static_cast<quint8>(data[8])) << 8);
|
|
|
|
// 解析设备状态1
|
|
quint8 byte9 = static_cast<quint8>(data[9]);
|
|
device_status1.exposure_mode = byte9 & 0x01;
|
|
device_status1.contrast_mode = (byte9 >> 1) & 0x01;
|
|
device_status1.gamma_mode = (byte9 >> 2) & 0x01;
|
|
device_status1.dynamic_range_mode = (byte9 >> 3) & 0x01;
|
|
device_status1.tracker_serial_status = (byte9 >> 4) & 0x01;
|
|
device_status1.camera_serial_status = (byte9 >> 5) & 0x01;
|
|
device_status1.flight_controller_status = (byte9 >> 6) & 0x01;
|
|
device_status1.image_output_status = (byte9 >> 7) & 0x01;
|
|
|
|
// 解析设备状态2和3
|
|
intensity_threshold = static_cast<quint8>(data[10]);
|
|
range_threshold = static_cast<quint8>(data[11]);
|
|
|
|
// 解析保留字节
|
|
reserved_bytes.resize(17);
|
|
for (qint32 i = 0; i < 17; ++i) {
|
|
if (12 + i < data.size() - 1) { // 排除校验和字节
|
|
reserved_bytes[i] = data[12 + i];
|
|
} else {
|
|
reserved_bytes[i] = 0;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool UvDownlinkData::validateChecksum(const QByteArray& data) const {
|
|
if (data.size() < UV_DOWNLINK_FRAME_LENGTH) {
|
|
return false;
|
|
}
|
|
|
|
quint8 checksum = 0;
|
|
for (qint32 i = 0; i < UV_DOWNLINK_FRAME_LENGTH - 1; ++i) {
|
|
checksum += static_cast<quint8>(data.at(i));
|
|
}
|
|
|
|
return checksum == static_cast<quint8>(data.at(UV_DOWNLINK_FRAME_LENGTH - 1));
|
|
}
|
|
|
|
bool UvDownlinkData::shouldTriggerAlarm(quint8 set_intensity_threshold,
|
|
quint8 set_range_threshold) const {
|
|
return (intensity_threshold >= set_intensity_threshold) &&
|
|
(range_threshold >= set_range_threshold);
|
|
}
|
|
|
|
TargetPosition UvDownlinkData::getTargetPosition() const {
|
|
if (!warning_flags.has_target) {
|
|
return TargetPosition::NO_TARGET;
|
|
}
|
|
|
|
switch (warning_flags.target_position) {
|
|
case 0b01: return TargetPosition::LEFT_FIELD;
|
|
case 0b10: return TargetPosition::RIGHT_FIELD;
|
|
case 0b00: return TargetPosition::CENTER_FIELD;
|
|
default: return TargetPosition::NO_TARGET;
|
|
}
|
|
}
|
|
|
|
void UvDownlinkData::setTargetPosition(TargetPosition position, bool has_target) {
|
|
warning_flags.has_target = has_target ? 1 : 0;
|
|
|
|
if (has_target) {
|
|
if (position == TargetPosition::LEFT_FIELD) {
|
|
warning_flags.target_position = 0b01;
|
|
} else if (position == TargetPosition::RIGHT_FIELD) {
|
|
warning_flags.target_position = 0b10;
|
|
} else {
|
|
// For CENTER_FIELD or any other value (including NO_TARGET when has_target is true)
|
|
warning_flags.target_position = 0b00;
|
|
}
|
|
} else {
|
|
warning_flags.target_position = 0b00;
|
|
}
|
|
}
|
|
|
|
// UvUplinkData 实现
|
|
|
|
UvUplinkData::UvUplinkData()
|
|
: intensity_level(0), range_level(0) {
|
|
command_byte.exposure_mode = 0;
|
|
command_byte.contrast_mode = 0;
|
|
command_byte.gamma_mode = 0;
|
|
command_byte.dynamic_range_mode = 0;
|
|
command_byte.exposure_increase = 0;
|
|
command_byte.exposure_decrease = 0;
|
|
command_byte.set_intensity_threshold = 0;
|
|
command_byte.set_range_threshold = 0;
|
|
|
|
reserved_bytes.fill(0, 6);
|
|
}
|
|
|
|
QByteArray UvUplinkData::serialize() const {
|
|
QByteArray data(UV_UPLINK_FRAME_LENGTH, 0);
|
|
|
|
// 设置帧头
|
|
data[0] = static_cast<char>(UV_HEADER1);
|
|
data[1] = static_cast<char>(UV_HEADER2);
|
|
|
|
// 设备识别码
|
|
data[2] = static_cast<char>(device_id);
|
|
|
|
// 告警设备命令字节
|
|
data[3] = static_cast<char>((command_byte.exposure_mode & 0x01) |
|
|
((command_byte.contrast_mode & 0x01) << 1) |
|
|
((command_byte.gamma_mode & 0x01) << 2) |
|
|
((command_byte.dynamic_range_mode & 0x01) << 3) |
|
|
((command_byte.exposure_increase & 0x01) << 4) |
|
|
((command_byte.exposure_decrease & 0x01) << 5) |
|
|
((command_byte.set_intensity_threshold & 0x01) << 6) |
|
|
((command_byte.set_range_threshold & 0x01) << 7));
|
|
|
|
// 强度档位参数
|
|
data[4] = static_cast<char>(intensity_level);
|
|
|
|
// 范围档位参数
|
|
data[5] = static_cast<char>(range_level);
|
|
|
|
// 保留字节
|
|
for (qint32 i = 0; i < reserved_bytes.size() && i < 6; ++i) {
|
|
data[6 + i] = reserved_bytes.at(i);
|
|
}
|
|
|
|
// 计算校验和（前12个字节累加和，取低8位）
|
|
quint8 checksum = 0;
|
|
for (qint32 i = 0; i < UV_UPLINK_FRAME_LENGTH - 1; ++i) {
|
|
checksum += static_cast<quint8>(data.at(i));
|
|
}
|
|
data[UV_UPLINK_FRAME_LENGTH - 1] = static_cast<char>(checksum);
|
|
|
|
return data;
|
|
}
|
|
|
|
bool UvUplinkData::deserialize(const QByteArray& data) {
|
|
if (data.size() < UV_UPLINK_FRAME_LENGTH) {
|
|
return false;
|
|
}
|
|
|
|
// 验证帧头
|
|
if (static_cast<quint8>(data[0]) != UV_HEADER1 || static_cast<quint8>(data[1]) != UV_HEADER2) {
|
|
return false;
|
|
}
|
|
|
|
// 验证设备识别码
|
|
if (static_cast<quint8>(data[2]) != UV_WARNING_DEVICE_ID) {
|
|
return false;
|
|
}
|
|
|
|
// 验证校验和
|
|
if (!validateChecksum(data)) {
|
|
return false;
|
|
}
|
|
|
|
// 解析命令字节
|
|
quint8 byte3 = static_cast<quint8>(data[3]);
|
|
command_byte.exposure_mode = byte3 & 0x01;
|
|
command_byte.contrast_mode = (byte3 >> 1) & 0x01;
|
|
command_byte.gamma_mode = (byte3 >> 2) & 0x01;
|
|
command_byte.dynamic_range_mode = (byte3 >> 3) & 0x01;
|
|
command_byte.exposure_increase = (byte3 >> 4) & 0x01;
|
|
command_byte.exposure_decrease = (byte3 >> 5) & 0x01;
|
|
command_byte.set_intensity_threshold = (byte3 >> 6) & 0x01;
|
|
command_byte.set_range_threshold = (byte3 >> 7) & 0x01;
|
|
|
|
// 解析强度档位参数
|
|
intensity_level = static_cast<quint8>(data[4]);
|
|
|
|
// 解析范围档位参数
|
|
range_level = static_cast<quint8>(data[5]);
|
|
|
|
// 解析保留字节
|
|
reserved_bytes.resize(6);
|
|
for (qint32 i = 0; i < 6; ++i) {
|
|
if (6 + i < data.size() - 1) { // 排除校验和字节
|
|
reserved_bytes[i] = data[6 + i];
|
|
} else {
|
|
reserved_bytes[i] = 0;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool UvUplinkData::validateChecksum(const QByteArray& data) const {
|
|
if (data.size() < UV_UPLINK_FRAME_LENGTH) {
|
|
return false;
|
|
}
|
|
|
|
quint8 checksum = 0;
|
|
for (qint32 i = 0; i < UV_UPLINK_FRAME_LENGTH - 1; ++i) {
|
|
checksum += static_cast<quint8>(data.at(i));
|
|
}
|
|
|
|
return checksum == static_cast<quint8>(data.at(UV_UPLINK_FRAME_LENGTH - 1));
|
|
}
|
|
|
|
void UvUplinkData::setExposureAdjustment(bool increase) {
|
|
if (increase) {
|
|
command_byte.exposure_increase = 1;
|
|
command_byte.exposure_decrease = 0;
|
|
command_byte.exposure_mode = 1; // 手动曝光模式
|
|
} else {
|
|
command_byte.exposure_increase = 0;
|
|
command_byte.exposure_decrease = 1;
|
|
command_byte.exposure_mode = 1; // 手动曝光模式
|
|
}
|
|
}
|
|
|
|
void UvUplinkData::setThresholds(quint8 intensity, quint8 range,
|
|
bool set_intensity, bool set_range) {
|
|
intensity_level = intensity;
|
|
range_level = range;
|
|
command_byte.set_intensity_threshold = set_intensity ? 1 : 0;
|
|
command_byte.set_range_threshold = set_range ? 1 : 0;
|
|
}
|
|
|
|
bool UvUplinkData::isCommandValid() const {
|
|
// 检查曝光调整命令：不能同时增加和减少
|
|
if (command_byte.exposure_increase && command_byte.exposure_decrease) {
|
|
return false;
|
|
}
|
|
|
|
// 检查阈值设置命令：如果设置了阈值，对应的档位参数应该有效
|
|
if (command_byte.set_intensity_threshold && intensity_level == 0) {
|
|
return false;
|
|
}
|
|
|
|
if (command_byte.set_range_threshold && range_level == 0) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// UvWarningDevice 实现
|
|
|
|
UvWarningDevice::UvWarningDevice(QObject* parent)
|
|
: QObject(parent), device_number_(0x01) {
|
|
// 默认设备编号为1
|
|
intensity_threshold_ = 50;
|
|
range_threshold_ = 30;
|
|
|
|
// Register meta types for Qt signal/slot system
|
|
qRegisterMetaType<payload_protocol::UvDownlinkData>();
|
|
qRegisterMetaType<payload_protocol::TargetPosition>();
|
|
}
|
|
|
|
UvWarningDevice::UvWarningDevice(quint8 device_number, QObject* parent)
|
|
: QObject(parent), device_number_(device_number) {
|
|
intensity_threshold_ = 50;
|
|
range_threshold_ = 30;
|
|
|
|
// Register meta types for Qt signal/slot system
|
|
qRegisterMetaType<payload_protocol::UvDownlinkData>();
|
|
qRegisterMetaType<payload_protocol::TargetPosition>();
|
|
}
|
|
|
|
QByteArray UvWarningDevice::createControlCommand(const UvUplinkData& uplink_data) const {
|
|
QByteArray command = uplink_data.serialize();
|
|
|
|
// 在控制数据中插入设备编号
|
|
// 根据协议，控制数据（第4-12字节）中应该包含设备编号信息
|
|
// 这里我们在保留字节的第一个位置插入设备编号
|
|
if (command.size() >= 7) { // 至少7字节才能访问到第6字节（保留字节的开始）
|
|
command[6] = static_cast<char>(device_number_); // 在第一个保留字节位置插入设备编号
|
|
}
|
|
|
|
return command;
|
|
}
|
|
|
|
bool UvWarningDevice::parseStatusData(const QByteArray& data) {
|
|
UvDownlinkData downlink_data;
|
|
if (!downlink_data.deserialize(data)) {
|
|
emit errorOccurred(QStringLiteral("无法解析紫外告警设备状态数据"));
|
|
return false;
|
|
}
|
|
|
|
last_status_ = downlink_data;
|
|
emit statusUpdated(downlink_data);
|
|
|
|
// 检查目标检测
|
|
if (downlink_data.warning_flags.has_target) {
|
|
emit targetDetected(downlink_data.getTargetPosition());
|
|
}
|
|
|
|
// 检查告警触发
|
|
if (downlink_data.shouldTriggerAlarm(intensity_threshold_, range_threshold_)) {
|
|
emit alarmTriggered(downlink_data, intensity_threshold_, range_threshold_);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool UvWarningDevice::parseStatusData(const QByteArray& data, UvDownlinkData& downlink_data) const {
|
|
return downlink_data.deserialize(data);
|
|
}
|
|
|
|
bool UvWarningDevice::validateFrameHeader(const QByteArray& data) {
|
|
if (data.size() < 2) {
|
|
return false;
|
|
}
|
|
return (static_cast<quint8>(data[0]) == UV_HEADER1) && (static_cast<quint8>(data[1]) == UV_HEADER2);
|
|
}
|
|
|
|
bool UvWarningDevice::validateDeviceId(const QByteArray& data) {
|
|
if (data.size() < 3) {
|
|
return false;
|
|
}
|
|
return static_cast<quint8>(data[2]) == UV_WARNING_DEVICE_ID;
|
|
}
|
|
|
|
void UvWarningDevice::sendControlCommand(const UvUplinkData& uplink_data) {
|
|
QByteArray command = createControlCommand(uplink_data);
|
|
emit commandSent(command);
|
|
// 实际发送逻辑应由上层实现，这里只发出信号
|
|
}
|
|
|
|
void UvWarningDevice::sendRawCommand(const QByteArray& command_data) {
|
|
if (validateFrameHeader(command_data) && validateDeviceId(command_data)) {
|
|
emit commandSent(command_data);
|
|
} else {
|
|
emit errorOccurred(QStringLiteral("无效的紫外告警设备指令格式"));
|
|
}
|
|
}
|
|
|
|
QByteArray UvWarningDevice::createBroadcastCommand(const UvUplinkData& uplink_data) {
|
|
QByteArray command = uplink_data.serialize();
|
|
|
|
// 广播命令：设备编号为0x00
|
|
if (command.size() >= 7) {
|
|
command[6] = static_cast<char>(0x00); // 设备编号0x00表示全控
|
|
}
|
|
|
|
return command;
|
|
}
|
|
|
|
quint8 UvWarningDevice::calculateChecksum(const QByteArray& data, qint32 length) {
|
|
if (length == 0) {
|
|
length = data.size();
|
|
}
|
|
|
|
quint8 checksum = 0;
|
|
for (qint32 i = 0; i < length && i < data.size(); ++i) {
|
|
checksum += static_cast<quint8>(data.at(i));
|
|
}
|
|
return checksum;
|
|
}
|
|
|
|
void UvWarningDevice::insertDeviceNumber(QByteArray& data) const {
|
|
if (data.size() >= 7) {
|
|
data[6] = static_cast<char>(device_number_);
|
|
}
|
|
}
|
|
|
|
// 槽函数实现
|
|
|
|
void UvWarningDevice::setAlarmThresholds(quint8 intensity_threshold, quint8 range_threshold) {
|
|
intensity_threshold_ = intensity_threshold;
|
|
range_threshold_ = range_threshold;
|
|
|
|
qDebug() << QString("告警阈值已设置: 强度=%1, 范围=%2")
|
|
.arg(intensity_threshold_)
|
|
.arg(range_threshold_);
|
|
|
|
// 如果已有状态数据，重新检查告警
|
|
if (last_status_.device_id == UV_WARNING_DEVICE_ID) {
|
|
if (last_status_.shouldTriggerAlarm(intensity_threshold_, range_threshold_)) {
|
|
emit alarmTriggered(last_status_, intensity_threshold_, range_threshold_);
|
|
}
|
|
}
|
|
}
|
|
|
|
void UvWarningDevice::resetDevice() {
|
|
// 重置设备状态
|
|
intensity_threshold_ = 50;
|
|
range_threshold_ = 30;
|
|
|
|
qDebug() << "设备已重置，告警阈值恢复为默认值";
|
|
emit errorOccurred(QStringLiteral("设备已重置"));
|
|
}
|
|
|
|
void UvWarningDevice::requestStatusUpdate() {
|
|
// 创建一个状态查询指令
|
|
UvUplinkData uplink_data;
|
|
// 清空所有命令位，表示状态查询
|
|
uplink_data.command_byte.exposure_mode = 0;
|
|
uplink_data.command_byte.contrast_mode = 0;
|
|
uplink_data.command_byte.gamma_mode = 0;
|
|
uplink_data.command_byte.dynamic_range_mode = 0;
|
|
uplink_data.command_byte.exposure_increase = 0;
|
|
uplink_data.command_byte.exposure_decrease = 0;
|
|
uplink_data.command_byte.set_intensity_threshold = 0;
|
|
uplink_data.command_byte.set_range_threshold = 0;
|
|
|
|
sendControlCommand(uplink_data);
|
|
qDebug() << "状态更新请求已发送";
|
|
}
|
|
|
|
void UvWarningDevice::handleStatusUpdate(const payload_protocol::UvDownlinkData& status) {
|
|
// 存储最后状态
|
|
last_status_ = status;
|
|
|
|
// 检查连接状态变化
|
|
bool is_connected = status.device_status1.camera_serial_status &&
|
|
status.device_status1.flight_controller_status &&
|
|
status.device_status1.image_output_status;
|
|
|
|
// 发出状态更新信号
|
|
emit statusUpdated(status);
|
|
|
|
// 检查目标检测
|
|
if (status.warning_flags.has_target) {
|
|
emit targetDetected(status.getTargetPosition());
|
|
}
|
|
|
|
// 检查告警触发
|
|
if (status.shouldTriggerAlarm(intensity_threshold_, range_threshold_)) {
|
|
emit alarmTriggered(status, intensity_threshold_, range_threshold_);
|
|
}
|
|
}
|
|
|
|
} // namespace payload_protocol
|