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

Refactored a8 codes and added remote testing app a8_remote.

Browse Source
This commit is contained in:
Nffj84
2024-07-02 11:31:12 +03:00
parent c03d477c45
commit b39e58dbc1
499 changed files with 1209 additions and 463 deletions
Download Patch File Download Diff File Expand all files Collapse all files
misc/camera/a8/serialCommand.cpp
+111 -59
View File
@@ -1,33 +1,35 @@
#include "serialCommand.h"
#include "serialCommand.hpp"
#include <QDebug>
#include "utilsCRC16.h"
#include "config.hpp"
#include "utilsTargetLocation.hpp"
#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
*/
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::TURN_TO_DEGREES, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to degrees"});
mSerialCommands.push_back({COMMAND_ID::TURN_TO_PIXEL, createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}), "Turn to pixel"});
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::ACQUIRE_ATTITUDE_DATA, createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0D}), "Acquire Attitude Data"});
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_CAMERA_CODEC_SPECS, createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x20, 0x00}), "Acquire Camera Codec Specs"});
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_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"});
@@ -35,33 +37,41 @@ byte in the front
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::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_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)"});
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)"});
mSerialCommands.push_back({COMMAND_ID::RUN_TARGET_LOCATION_TEST, createByteArray({0x00, 0x00}), "TEST target location calculations"});
// Sort vector by COMMAND_ID
std::sort(mSerialCommands.begin(), mSerialCommands.end(), [](const Command& a, const Command& b) { return a.id < b.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 SerialCommand::createByteArray(const std::initializer_list<int> &bytes)
{
QByteArray byteArray;
for (int byte : bytes) {
for (int8_t byte : bytes) {
byteArray.append(static_cast<char>(byte));
}
return byteArray;
@@ -69,8 +79,10 @@ QByteArray SerialCommand::createByteArray(const std::initializer_list<int>& byte
void SerialCommand::printCommands()
{
qInfo().noquote().nospace() << " ";
for (uint8_t i = 0; i < mSerialCommands.size(); i++) {
qInfo().noquote() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
qInfo().noquote().nospace() << QString::number(mSerialCommands.at(i).id) + ": " + mSerialCommands.at(i).description;
}
}
@@ -79,14 +91,14 @@ void SerialCommand::setExtraValues(COMMAND_ID commandId)
uint16_t commandIndex = getCommandIndex(commandId);
QByteArray command = mSerialCommands.at(commandIndex).command;
if (commandId == COMMAND_ID::TURN_TO_X) {
if (commandId == COMMAND_ID::TURN_TO_DEGREES) {
float degrees;
qInfo() << "Enter yaw degrees (-135.0 -> 135.0): ";
qInfo().noquote().nospace() << "Enter yaw degrees (" << GIMBAL_YAW_MIN << " -> " << GIMBAL_YAW_MAX << "): ";
std::cin >> degrees;
if (std::cin.fail() || degrees < -135.0f || degrees > 135.0f) {
qWarning() << "Invalid value, using 0.0";
if (std::cin.fail() || degrees < GIMBAL_YAW_MIN || degrees > GIMBAL_YAW_MAX) {
qWarning().noquote().nospace() << "Invalid value, using 0.0";
degrees = 0.0f;
}
@@ -94,31 +106,61 @@ void SerialCommand::setExtraValues(COMMAND_ID commandId)
command[8] = degreesVal & 0xFF;
command[9] = degreesVal >> 8;
qInfo() << "Enter pitch degrees (-90.0 -> 25.0): ";
qInfo().noquote().nospace() << "Enter pitch degrees (" << GIMBAL_PITCH_MIN << " -> " << GIMBAL_PITCH_MAX << "): ";
std::cin >> degrees;
if (std::cin.fail() || degrees < -90.0f || degrees > 25.0f) {
qWarning() << "Invalid value, using 0.0";
if (std::cin.fail() || degrees < GIMBAL_PITCH_MIN || degrees > GIMBAL_PITCH_MAX) {
qWarning().noquote().nospace() << "Invalid value, using 0.0";
degrees = 0.0f;
}
degreesVal = degrees * 10;
command[10] = degreesVal & 0xFF;
command[11] = degreesVal >> 8;
} else if (commandId == COMMAND_ID::TURN_TO_PIXEL) {
float targetX, targetY;
qInfo().noquote().nospace() << "Enter left_to_right (1 - " << Config::getResolutionWidth() << "): ";
std::cin >> targetX;
if (std::cin.fail() || targetX < 1 || targetX > Config::getResolutionWidth()) {
qWarning().noquote().nospace() << "Invalid value, using " << Config::getResolutionWidth() / 2;
targetX = Config::getResolutionWidth() / 2;
}
qInfo().noquote().nospace() << "Enter top_to_bottom (1 -> " << Config::getResolutionHeight() << "): ";
std::cin >> targetY;
if (std::cin.fail() || targetY < 1 || targetY > Config::getResolutionHeight()) {
qWarning().noquote().nospace() << "Invalid value, using " << Config::getResolutionHeight() / 2;
targetY = Config::getResolutionHeight() / 2;
}
float resultYaw = 0.0f;
float resultPitch = 0.0f;
UtilsTargetLocation::getAnglesToOnScreenTarget(targetX, targetY, resultYaw, resultPitch);
int16_t degreesVal = resultYaw * 10;
command[8] = degreesVal & 0xFF;
command[9] = degreesVal >> 8;
degreesVal = resultPitch * 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): ";
qInfo().noquote().nospace() << "Enter zoom (1.0 -> " << Config::getMaxZoom() << "): ";
std::cin >> zoom;
if (std::cin.fail() || zoom < 1.0 || zoom > 6.0) {
qWarning() << "Invalid value, using 1.0";
if (std::cin.fail() || zoom < 1.0f || zoom > Config::getMaxZoom()) {
qWarning().noquote().nospace() << "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);
uint8_t scaledFractional = uint8_t(fractionalPart * 10);
command[8] = integerPart;
command[9] = scaledFractional;
}
@@ -142,25 +184,35 @@ int16_t SerialCommand::getCommandIndex(COMMAND_ID commandId)
return -1;
}
QByteArray SerialCommand::getCommandByID(COMMAND_ID commandId)
QByteArray SerialCommand::getCommandForUI(COMMAND_ID commandId)
{
setExtraValues(commandId);
uint16_t commandIndex = getCommandIndex(commandId);
int16_t commandIndex = getCommandIndex(commandId);
if (commandIndex == -1) {
qCritical().noquote().nospace() << "Command not found for command: " << 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();
if (i > 0) {
commandStr += ",";
}
commandStr += QString("0x%1").arg(command.at(i), 2, 16, QChar('0')).toUpper();
commandStr.replace("0X", "0x");
}
qDebug() << "Command: " << commandStr;
qDebug().noquote().nospace() << "Command: " << commandStr;
return command;
}
QByteArray SerialCommand::getCommandForInternal(COMMAND_ID commandId)
{
int16_t commandIndex = getCommandIndex(commandId);
if (commandIndex == -1) {
qCritical().noquote().nospace() << "Command not found for command: " << commandId;
}
return mSerialCommands.at(commandIndex).command;
}