azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 13:26:34 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added functionality to calculate target location.

Browse Source 
Added functionality to capture camera frame from RTSP stream.
Refactored code.
Fixed some minor issues.
This commit is contained in:
Nffj84
2024-06-18 17:29:40 +03:00
parent 05722c0e09
commit db2652d979
15 changed files with 689 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files
misc/camera/a8/serialResponse.cpp
+100 -38
View File
@@ -1,48 +1,110 @@
#include "serialResponse.h"
#include "crc16.h"
#include "defines.h"
#include <QDebug>
#include <QString>
#include "defines.h"
#include "utilsCRC16.h"
void SerialResponse::printResponse(QByteArray response) {
QString responseStr;
for (int i = 0; i < response.size(); i++) {
responseStr +=
QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
}
qDebug() << "Response: " << responseStr;
responseStr = "";
uint8_t command = response.at(MESSAGE_IDX::CMD_ID);
// Check response data validity
uint8_t crcCheck[2];
CRC16::getCRCBytes(response.mid(0, response.size() - 2), crcCheck);
// Data not OK
if (crcCheck[0] != response.at(response.size() - 2) ||
crcCheck[1] != response.at(response.size() - 1)) {
qWarning() << "Response data not valid";
void SerialResponse::printResponse(QByteArray response)
{
if (response.size() == 0) {
qWarning().noquote() << "Response is empty";
return;
}
qInfo() << "Response data is valid";
if (command == 0x0E) {
int16_t yaw = (static_cast<uint8_t>(response.at(9)) << 8) |
static_cast<uint8_t>(response.at(8));
int16_t pitch = (static_cast<uint8_t>(response.at(11)) << 8) |
static_cast<uint8_t>(response.at(10));
int16_t roll = (static_cast<uint8_t>(response.at(13)) << 8) |
static_cast<uint8_t>(response.at(12));
QHash<QString, QVariant> results = getResponceValues(response);
qInfo().noquote() << "Yaw: " << QString::number(yaw / 10) + "°";
qInfo().noquote() << "Pitch: " << QString::number(pitch / 10) + "°";
qInfo().noquote() << "Roll: " << QString::number(roll / 10) + "°";
} else {
for (int i = 0; i < response.size(); i++) {
responseStr +=
QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
QList<QString> keys = results.keys();
std::sort(keys.begin(), keys.end());
// Loop through the sorted keys
for (const QString &key : keys) {
if (results.value(key).typeId() == QMetaType::UInt) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toUInt();
} else if (results.value(key).typeId() == QMetaType::Int) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toInt();
} else if (results.value(key).typeId() == QMetaType::Double) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toDouble();
} else if (results.value(key).typeId() == QMetaType::Float) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toFloat();
} else if (results.value(key).typeId() == QMetaType::Bool) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toBool();
} else if (results.value(key).typeId() == QMetaType::QString) {
qInfo().noquote().nospace() << key << ": " << results.value(key).toString();
} else {
qInfo().noquote().nospace() << key << ": " << results.value(key);
}
qInfo() << "Response: " << responseStr;
}
}
QHash<QString, QVariant> SerialResponse::getResponceValues(QByteArray response)
{
// Check response data validity
int8_t crcCheck[2];
uint8_t desiredLength = response.size() - 2;
QByteArray subData(response.data(), desiredLength);
UtilsCRC16::getCRCBytes(subData, crcCheck);
int8_t crcOriginal[2];
crcOriginal[0] = response.at(response.size() - 2);
crcOriginal[1] = response.at(response.size() - 1);
// Data not OK
if (crcCheck[0] != crcOriginal[0] || crcCheck[1] != crcOriginal[1]) {
qWarning() << "Response data INVALID";
QString responseCRC = QString("0x%1,0x%2").arg(crcOriginal[0], 2, 16, QLatin1Char('0')).arg(crcOriginal[1], 2, 16, QLatin1Char('0')).toUpper();
QString recalcCRC = QString("0x%1,0x%2").arg(crcCheck[0], 2, 16, QLatin1Char('0')).arg(crcCheck[1], 2, 16, QLatin1Char('0')).toUpper();
qWarning().noquote() << responseCRC << "!=" << recalcCRC;
}
QHash<QString, QVariant> results;
uint8_t command = response.at(MESSAGE_IDX::CMD_ID);
if (command == 0x0E) {
int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
results.insert("yaw", (float) (yaw / 10));
int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
results.insert("pitch", (float) (pitch / 10));
int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
results.insert("roll", (float) (roll / 10));
} else if (command == 0x0D) {
int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
results.insert("yaw", (float) (yaw / 10));
int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
results.insert("pitch", (float) (pitch / 10));
int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
results.insert("roll", (float) (roll / 10));
int16_t yaw_speed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14);
results.insert("yaw_speed", (float) (yaw_speed / 10));
int16_t pitch_speed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16);
results.insert("pitch_speed", (float) (pitch_speed / 10));
int16_t roll_speed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18);
results.insert("roll_speed", (float) (roll_speed / 10));
} else if (command == 0x0F) {
int8_t zoom = (int8_t) response.at(8);
results.insert("zoom", zoom);
} else if (command == 0x18) {
float zoomInt = (float) response.at(8);
float zoomFloat = (float) ((float) response.at(9) / 10);
results.insert("zoom", zoomInt + zoomFloat);
} else if (command == 0x20) {
uint16_t width = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
results.insert("width", width);
uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
results.insert("height", height);
} else {
QString responseStr;
for (int i = 0; i < response.size(); i++) {
responseStr += QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
}
results.insert("Response", responseStr);
}
return results;
}