mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 11:56:33 +00:00
Nffj84
33399370f3
Simple program to control Siyi A8 mini (actually some other Siyi cameras too). Receiving responce sometimes gives error when checking CRC.
49 lines
1.7 KiB
C++
49 lines
1.7 KiB
C++
#include "serialResponse.h"
|
|
#include "crc16.h"
|
|
#include "defines.h"
|
|
#include <QDebug>
|
|
#include <QString>
|
|
|
|
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";
|
|
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));
|
|
|
|
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();
|
|
}
|
|
qInfo() << "Response: " << responseStr;
|
|
}
|
|
}
|