autopilot/misc/camera/a8/serialResponse.cpp

#include "serialResponse.h"
#include <QDebug>
#include "defines.h"
#include "utilsCRC16.h"

void SerialResponse::printResponse(QByteArray response)
{
    if (response.size() == 0) {
        qWarning().noquote() << "Response is empty";
        return;
    }

    QHash<QString, QVariant> results = getResponceValues(response);

    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);
        }
    }
}

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;
}