mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-22 11:56:33 +00:00
Added functionality to calculate target location.
Added functionality to capture camera frame from RTSP stream. Refactored code. Fixed some minor issues.
This commit is contained in:
Nffj84
+123
-132
@@ -1,119 +1,65 @@
|
||||
#include "serialCommand.h"
|
||||
#include "crc16.h"
|
||||
#include <QDebug>
|
||||
#include "utilsCRC16.h"
|
||||
#include <iostream>
|
||||
|
||||
SerialCommand::SerialCommand() {
|
||||
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.append({createByteArray({0x00, 0x00}), "Exit program"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x04, 0x00, 0x00,
|
||||
0x00, 0x0E, 0x00, 0x00, 0x00, 0x00}),
|
||||
"Degrees"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x08, 0x01}),
|
||||
"Auto Centering"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0x01}),
|
||||
"Zoom +1"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x05, 0xFF}),
|
||||
"Zoom -1"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x01,
|
||||
0x00, 0x0F, 0x04, 0x05}),
|
||||
"4.5x"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0x01}),
|
||||
"Manual Focus +1"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x06, 0xff}),
|
||||
"Manual Focus -1"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x04, 0x01}),
|
||||
"Auto Focus"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, 0x00, 0x2D}),
|
||||
"Rotate Up"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, 0x00, -0x2D}),
|
||||
"Rotate Down"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, 0x2D, 0x00}),
|
||||
"Rotate Right"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, -0x2D, 0x00}),
|
||||
"Rotate Left"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, 0x00, 0x00}),
|
||||
"Stop rotation"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x16}),
|
||||
"Acquire the Max Zoom Value"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x00}),
|
||||
"Take Pictures"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x02}),
|
||||
"Record Video"});
|
||||
mSerialCommands.append({createByteArray({0x55, 0x66, 0x01, 0x02, 0x00, 0x00,
|
||||
0x00, 0x07, 0x64, 0x64}),
|
||||
"Rotate 100 100"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0a}),
|
||||
"Gimbal Status Information"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x02}),
|
||||
"Acquire Hardware ID"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x01}),
|
||||
"Acquire Firmware Version"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x03}),
|
||||
"Lock Mode"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x04}),
|
||||
"Follow Mode"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x01, 0x00, 0x00, 0x00, 0x0c, 0x05}),
|
||||
"FPV Mode"});
|
||||
mSerialCommands.append(
|
||||
{createByteArray({0x55, 0x66, 0x01, 0x00, 0x00, 0x00, 0x00, 0x0d}),
|
||||
"Acquire Attitude Data"});
|
||||
mSerialCommands.append(
|
||||
{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.append(
|
||||
{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.append(
|
||||
{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.append(
|
||||
{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::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() {
|
||||
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) {
|
||||
byteArray.append(static_cast<char>(byte));
|
||||
@@ -121,45 +67,92 @@ SerialCommand::createByteArray(const std::initializer_list<int> &bytes) {
|
||||
return byteArray;
|
||||
}
|
||||
|
||||
void SerialCommand::printCommands() {
|
||||
uint8_t index = 0;
|
||||
foreach (Command command, mSerialCommands) {
|
||||
qInfo().noquote() << QString::number(index) + ": " + command.description;
|
||||
index++;
|
||||
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(uint8_t number) {
|
||||
if (number == 1) {
|
||||
QByteArray command = mSerialCommands.at(number).command;
|
||||
int16_t degrees;
|
||||
void SerialCommand::setExtraValues(COMMAND_ID commandId)
|
||||
{
|
||||
uint16_t commandIndex = getCommandIndex(commandId);
|
||||
QByteArray command = mSerialCommands.at(commandIndex).command;
|
||||
|
||||
std::cout << "Enter yaw degrees (-135 -> 135): ";
|
||||
if (commandId == COMMAND_ID::TURN_TO_X) {
|
||||
float degrees;
|
||||
|
||||
qInfo() << "Enter yaw degrees (-135.0 -> 135.0): ";
|
||||
std::cin >> degrees;
|
||||
degrees = degrees * 10;
|
||||
command[8] = degrees & 0xFF;
|
||||
command[9] = degrees >> 8;
|
||||
|
||||
std::cout << "Enter pitch degrees (-90 -> 25): ";
|
||||
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;
|
||||
degrees = degrees * 10;
|
||||
command[10] = degrees & 0xFF;
|
||||
command[11] = degrees >> 8;
|
||||
|
||||
mSerialCommands[number].command = command;
|
||||
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;
|
||||
}
|
||||
|
||||
QByteArray SerialCommand::getCommand(uint8_t number) {
|
||||
setExtraValues(number);
|
||||
uint8_t SerialCommand::getCommandCount()
|
||||
{
|
||||
return mSerialCommands.size();
|
||||
}
|
||||
|
||||
QByteArray command = mSerialCommands.at(number).command;
|
||||
int16_t SerialCommand::getCommandIndex(COMMAND_ID commandId)
|
||||
{
|
||||
for (uint16_t i = 0; i < mSerialCommands.size(); i++) {
|
||||
if (mSerialCommands.at(i).id == commandId) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t crcBytes[2];
|
||||
CRC16::getCRCBytes(command, crcBytes);
|
||||
return -1;
|
||||
}
|
||||
|
||||
command.resize(command.size() +
|
||||
2); // Increase array size to accommodate CRC bytes
|
||||
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
|
||||
|
||||
@@ -171,5 +164,3 @@ QByteArray SerialCommand::getCommand(uint8_t number) {
|
||||
|
||||
return command;
|
||||
}
|
||||
|
||||
uint8_t SerialCommand::getCommandCount() { return mSerialCommands.size(); }
|
||||
|
||||
Reference in New Issue
Block a user