autopilot/misc/camera/a8/serialResponse.cpp

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

void SerialResponse::printResponse(QByteArray response)
{
    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)
{
    QHash<QString, QVariant> results;

    if (response.size() == 0) {
        qWarning().noquote().nospace() << "Response is empty, exiting...";
        return results;
    }

    // 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().noquote().nospace() << "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().nospace() << responseCRC << "!=" << recalcCRC;
    }

    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);
        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        results.insert("yaw", (float) (yaw / 10));
        results.insert("pitch", (float) (pitch / 10));
        results.insert("roll", (float) (roll / 10));
    } else if (command == 0x0D) {
        int16_t yaw = ((uint8_t) response.at(9) << 8) | (uint8_t) response.at(8);
        int16_t pitch = ((uint8_t) response.at(11) << 8) | (uint8_t) response.at(10);
        int16_t roll = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        int16_t yawSpeed = ((uint8_t) response.at(15) << 8) | (uint8_t) response.at(14);
        int16_t pitchSpeed = ((uint8_t) response.at(17) << 8) | (uint8_t) response.at(16);
        int16_t rollSpeed = ((uint8_t) response.at(19) << 8) | (uint8_t) response.at(18);
        results.insert("yaw", (float) (yaw / 10));
        results.insert("pitch", (float) (pitch / 10));
        results.insert("roll", (float) (roll / 10));
        results.insert("yaw_speed", (float) (yawSpeed / 10));
        results.insert("pitch_speed", (float) (pitchSpeed / 10));
        results.insert("roll_speed", (float) (rollSpeed / 10));
    } else if (command == 0x0F) {
        int8_t zoom = (int8_t) response.at(8);
        results.insert("zoom", zoom);
    } else if (command == 0x16 || 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);
        uint16_t height = ((uint8_t) response.at(13) << 8) | (uint8_t) response.at(12);
        results.insert("width", width);
        results.insert("height", height);
    } else {
        qWarning().noquote().nospace() << "Getting responce values not implemented yet for command " << QString("0x%1").arg(command, 2, 16, QLatin1Char('0'));
        QString responseStr;
        for (int i = 0; i < response.size(); i++) {
            if (i > 0) {
                responseStr += ",";
            }
            responseStr += QString("0x%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
            responseStr.replace("0X", "0x");
        }
        qWarning().noquote().nospace() << "Responce byte array: " << responseStr;
    }

    return results;
}