autopilot/misc/camera/a8/serialCommand.cpp

#include "serialCommand.h"
#include <QDebug>
#include "utilsCRC16.h"
#include <iostream>

SerialCommand::SerialCommand()
{
    /*
Field    Index   Bytes       Description
STX      0       2           0x6655: starting mark. Low byte in the front
CTRL     2       1           0: need_ack (if the current data pack need “ack”)
1: ack_pack (if the current data pack is an “ack” package) 2-7: reserved
Data_len 3       2           Data field byte length. Low byte in the front
SEQ      5       2           Frame sequence (0 ~ 65535). Low byte in the front
CMD_ID   7       1           Command ID
DATA     8       Data_len    Data
CRC16            2           CRC16 check to the complete data package. Low
byte in the front
*/

    mSerialCommands.push_back({COMMAND_ID::TURN_TO_X, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to X"});
    mSerialCommands.push_back({COMMAND_ID::ZOOM_TO_X, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01, 0x00, 0x0F, 0x00, 0x00}), "Zoom to X"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_CURRENT_ZOOM, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x18}), "Acquire current zoom"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}), "Acquire Attitude Data"});
    mSerialCommands.push_back({COMMAND_ID::AUTO_CENTER, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}), "Auto Centering"});
    mSerialCommands.push_back({COMMAND_ID::ZOOM_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}), "Zoom +1"});
    mSerialCommands.push_back({COMMAND_ID::ZOOM_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}), "Zoom -1"});
    mSerialCommands.push_back({COMMAND_ID::FOCUS_MOST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}), "Manual Focus +1"});
    mSerialCommands.push_back({COMMAND_ID::FOCUS_LEAST, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}), "Manual Focus -1"});
    mSerialCommands.push_back({COMMAND_ID::FOCUS_AUTO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}), "Auto Focus"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_UP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x2D}), "Rotate Up"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_DOWN, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, -0x2D}), "Rotate Down"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_RIGHT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x2D, 0x00}), "Rotate Right"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_LEFT, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, -0x2D, 0x00}), "Rotate Left"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_STOP, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00}), "Stop rotation"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_MAX_ZOOM_VALUE, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}), "Acquire the Max Zoom Value"});
    mSerialCommands.push_back({COMMAND_ID::TAKE_PICTURES, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}), "Take Pictures"});
    mSerialCommands.push_back({COMMAND_ID::TAKE_VIDEO, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}), "Record Video"});
    mSerialCommands.push_back({COMMAND_ID::ROTATE_100_100, createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00, 0x00, 0x07, 0x64, 0x64}), "Rotate 100 100"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_GIMBAL_STATUS, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}), "Gimbal Status Information"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_HW_INFO, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}), "Acquire Hardware ID"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_FIRMWARE_VERSION, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}), "Acquire Firmware Version"});
    mSerialCommands.push_back({COMMAND_ID::MODE_LOCK, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}), "Lock Mode"});
    mSerialCommands.push_back({COMMAND_ID::MODE_FOLLOW, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}), "Follow Mode"});
    mSerialCommands.push_back({COMMAND_ID::MODE_FPV, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}), "FPV Mode"});
    mSerialCommands.push_back({COMMAND_ID::ENABLE_HDMI, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x06}), "Set Video Output as HDMI (Only available on A8 mini, restart to take effect)"});
    mSerialCommands.push_back({COMMAND_ID::ENABLE_CVBS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x07}), "Set Video Output as CVBS (Only available on A8 mini, restart to take effect)"});
    mSerialCommands.push_back({COMMAND_ID::DISABLE_HDMI_CVBS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x08}), "Turn Off both CVBS and HDMI Output (Only available on A8 mini, restart to take effect)"});
    mSerialCommands.push_back({COMMAND_ID::ACQUIRE_RANGE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x15}), "Read Range from Laser Rangefinder(Low byte in the front, high byte in the back, available on ZT30)"});

    // Sort vector by COMMAND_ID
    std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const Command& a, const Command& b) { return a.id < b.id; });
}

SerialCommand::~SerialCommand()
{
    // Do something?
}

QByteArray SerialCommand::createByteArray(const std::initializer_list<int>& bytes)
{
    QByteArray byteArray;
    for (int byte : bytes) {
        byteArray.append(static_cast<char>(byte));
    }
    return byteArray;
}

void SerialCommand::printCommands()
{
    for (uint8_t i = 0; i < mSerialCommands.size(); i++) {
        qInfo().noquote() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
    }
}

void SerialCommand::setExtraValues(COMMAND_ID commandId)
{
    uint16_t commandIndex = getCommandIndex(commandId);
    QByteArray command = mSerialCommands.at(commandIndex).command;

    if (commandId == COMMAND_ID::TURN_TO_X) {
        float degrees;

        qInfo() << "Enter yaw degrees (-135.0 -> 135.0): ";
        std::cin >> degrees;

        if (std::cin.fail() || degrees < -135.0f || degrees > 135.0f) {
            qWarning() << "Invalid value, using 0.0";
            degrees = 0.0f;
        }

        int16_t degreesVal = degrees * 10;
        command[8] = degreesVal & 0xFF;
        command[9] = degreesVal >> 8;

        qInfo() << "Enter pitch degrees (-90.0 -> 25.0): ";
        std::cin >> degrees;

        if (std::cin.fail() || degrees < -90.0f || degrees > 25.0f) {
            qWarning() << "Invalid value, using 0.0";
            degrees = 0.0f;
        }

        degreesVal = degrees * 10;
        command[10] = degreesVal & 0xFF;
        command[11] = degreesVal >> 8;
    } else if (commandId == COMMAND_ID::ZOOM_TO_X) {
        float zoom;

        qInfo() << "Enter zoom (1.0 -> 6.0): ";
        std::cin >> zoom;

        if (std::cin.fail() || zoom < 1.0 || zoom > 6.0) {
            qWarning() << "Invalid value, using 1.0";
            zoom = 1.0f;
        }

        uint8_t integerPart = static_cast<uint8_t>(zoom);
        float fractionalPart = zoom - integerPart;
        uint8_t scaledFractional = int(fractionalPart * 10);
        command[8] = integerPart;
        command[9] = scaledFractional;
    }

    mSerialCommands[commandIndex].command = command;
}

uint8_t SerialCommand::getCommandCount()
{
    return mSerialCommands.size();
}

int16_t SerialCommand::getCommandIndex(COMMAND_ID commandId)
{
    for (uint16_t i = 0; i < mSerialCommands.size(); i++) {
        if (mSerialCommands.at(i).id == commandId) {
            return i;
        }
    }

    return -1;
}

QByteArray SerialCommand::getCommandByID(COMMAND_ID commandId)
{
    setExtraValues(commandId);

    uint16_t commandIndex = getCommandIndex(commandId);
    QByteArray command = mSerialCommands.at(commandIndex).command;

    int8_t crcBytes[2];
    UtilsCRC16::getCRCBytes(command, crcBytes);

    command.resize(command.size() + 2);        // Increase array size to accommodate CRC bytes
    command[command.size() - 2] = crcBytes[0]; // Set LSB
    command[command.size() - 1] = crcBytes[1]; // Set MSB

    QString commandStr;
    for (int i = 0; i < command.size(); i++) {
        commandStr += QString("%1").arg(command.at(i), 2, 16, QChar('0')).toUpper();
    }
    qDebug() << "Command: " << commandStr;

    return command;
}