mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-23 00:46:34 +00:00
Simple program to control Siyi A8 mini gimbal
Simple program to control Siyi A8 mini (actually some other Siyi cameras too). Receiving responce sometimes gives error when checking CRC.
This commit is contained in:
Nffj84
@@ -0,0 +1,74 @@
|
|||||||
|
# This file is used to ignore files which are generated
|
||||||
|
# ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
*~
|
||||||
|
*.autosave
|
||||||
|
*.a
|
||||||
|
*.core
|
||||||
|
*.moc
|
||||||
|
*.o
|
||||||
|
*.obj
|
||||||
|
*.orig
|
||||||
|
*.rej
|
||||||
|
*.so
|
||||||
|
*.so.*
|
||||||
|
*_pch.h.cpp
|
||||||
|
*_resource.rc
|
||||||
|
*.qm
|
||||||
|
.#*
|
||||||
|
*.*#
|
||||||
|
core
|
||||||
|
!core/
|
||||||
|
tags
|
||||||
|
.DS_Store
|
||||||
|
.directory
|
||||||
|
*.debug
|
||||||
|
Makefile*
|
||||||
|
*.prl
|
||||||
|
*.app
|
||||||
|
moc_*.cpp
|
||||||
|
ui_*.h
|
||||||
|
qrc_*.cpp
|
||||||
|
Thumbs.db
|
||||||
|
*.res
|
||||||
|
*.rc
|
||||||
|
/.qmake.cache
|
||||||
|
/.qmake.stash
|
||||||
|
|
||||||
|
# qtcreator generated files
|
||||||
|
*.pro.user*
|
||||||
|
CMakeLists.txt.user*
|
||||||
|
|
||||||
|
# xemacs temporary files
|
||||||
|
*.flc
|
||||||
|
|
||||||
|
# Vim temporary files
|
||||||
|
.*.swp
|
||||||
|
|
||||||
|
# Visual Studio generated files
|
||||||
|
*.ib_pdb_index
|
||||||
|
*.idb
|
||||||
|
*.ilk
|
||||||
|
*.pdb
|
||||||
|
*.sln
|
||||||
|
*.suo
|
||||||
|
*.vcproj
|
||||||
|
*vcproj.*.*.user
|
||||||
|
*.ncb
|
||||||
|
*.sdf
|
||||||
|
*.opensdf
|
||||||
|
*.vcxproj
|
||||||
|
*vcxproj.*
|
||||||
|
|
||||||
|
# MinGW generated files
|
||||||
|
*.Debug
|
||||||
|
*.Release
|
||||||
|
|
||||||
|
# Python byte code
|
||||||
|
*.pyc
|
||||||
|
|
||||||
|
# Binaries
|
||||||
|
# --------
|
||||||
|
*.dll
|
||||||
|
*.exe
|
||||||
|
|
||||||
@@ -0,0 +1,24 @@
|
|||||||
|
QT += core serialport
|
||||||
|
|
||||||
|
CONFIG += c++17 console
|
||||||
|
|
||||||
|
TARGET = a8
|
||||||
|
|
||||||
|
QMAKE_CXXFLAGS = -O0 -g -ggdb -fsanitize=undefined,bounds,float-divide-by-zero,integer-divide-by-zero,null,return,signed-integer-overflow,unreachable,shift,alignment,nonnull-attribute,returns-nonnull-attribute,enum
|
||||||
|
QMAKE_LFLAGS = -O0 -g -ggdb -fsanitize=undefined,bounds,float-divide-by-zero,integer-divide-by-zero,null,return,signed-integer-overflow,unreachable,shift,alignment,nonnull-attribute,returns-nonnull-attribute,enum
|
||||||
|
|
||||||
|
QMAKE_CXX = clang++
|
||||||
|
QMAKE_CC = clang
|
||||||
|
|
||||||
|
# Not nice, but for some reason QtCreator doesn't use /usr/lib/ccache/g++ from the PATH
|
||||||
|
linux-g++ {
|
||||||
|
QMAKE_CXX = clang++
|
||||||
|
QMAKE_CC = clang
|
||||||
|
}
|
||||||
|
|
||||||
|
#LIBS += $$PWD/some-library.a
|
||||||
|
|
||||||
|
SOURCES += *.cpp
|
||||||
|
HEADERS += *.h
|
||||||
|
|
||||||
|
#INCLUDEPATH += $$QT5_INCLUDES_DIR
|
||||||
@@ -0,0 +1,52 @@
|
|||||||
|
#include "crc16.h"
|
||||||
|
|
||||||
|
static const uint16_t crc16Table[256] = {
|
||||||
|
0x0, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108,
|
||||||
|
0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x210,
|
||||||
|
0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b,
|
||||||
|
0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x420, 0x1401,
|
||||||
|
0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee,
|
||||||
|
0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x630, 0x76d7, 0x66f6,
|
||||||
|
0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d,
|
||||||
|
0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x840, 0x1861, 0x2802, 0x3823,
|
||||||
|
0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5,
|
||||||
|
0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0xa50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc,
|
||||||
|
0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4,
|
||||||
|
0x5cc5, 0x2c22, 0x3c03, 0xc60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd,
|
||||||
|
0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13,
|
||||||
|
0x2e32, 0x1e51, 0xe70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a,
|
||||||
|
0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e,
|
||||||
|
0xe16f, 0x1080, 0xa1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067,
|
||||||
|
0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x2b1,
|
||||||
|
0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb,
|
||||||
|
0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0,
|
||||||
|
0x481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8,
|
||||||
|
0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x691, 0x16b0, 0x6657,
|
||||||
|
0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9,
|
||||||
|
0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x8e1, 0x3882,
|
||||||
|
0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a,
|
||||||
|
0x4a75, 0x5a54, 0x6a37, 0x7a16, 0xaf1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e,
|
||||||
|
0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07,
|
||||||
|
0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0xcc1, 0xef1f, 0xff3e, 0xcf5d,
|
||||||
|
0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74,
|
||||||
|
0x2e93, 0x3eb2, 0xed1, 0x1ef0};
|
||||||
|
|
||||||
|
uint16_t CRC16::calculate(const uint8_t *ptr, uint32_t len, uint16_t crcInit) {
|
||||||
|
uint16_t crc = crcInit;
|
||||||
|
uint8_t temp;
|
||||||
|
|
||||||
|
while (len-- != 0) {
|
||||||
|
temp = (crc >> 8) & 0xFF;
|
||||||
|
crc = (crc << 8) ^ crc16Table[*ptr ^ temp];
|
||||||
|
ptr++;
|
||||||
|
}
|
||||||
|
|
||||||
|
return crc;
|
||||||
|
}
|
||||||
|
|
||||||
|
void CRC16::getCRCBytes(const QByteArray &data, uint8_t *bytes) {
|
||||||
|
uint16_t crc16 = calculate(
|
||||||
|
reinterpret_cast<const uint8_t *>(data.constData()), data.size(), 0);
|
||||||
|
bytes[0] = crc16 & 0xFF;
|
||||||
|
bytes[1] = crc16 >> 8;
|
||||||
|
}
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <QByteArray>
|
||||||
|
#include <cstdint>
|
||||||
|
|
||||||
|
class CRC16 {
|
||||||
|
public:
|
||||||
|
static void getCRCBytes(const QByteArray &data, uint8_t *bytes);
|
||||||
|
|
||||||
|
private:
|
||||||
|
static uint16_t calculate(const uint8_t *ptr, uint32_t len, uint16_t crcInit);
|
||||||
|
};
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
|
||||||
|
enum MESSAGE_IDX
|
||||||
|
{
|
||||||
|
STX = 0,
|
||||||
|
CTRL = 2,
|
||||||
|
Data_len = 3,
|
||||||
|
SEQ = 5,
|
||||||
|
CMD_ID = 7,
|
||||||
|
DATA = 8
|
||||||
|
};
|
||||||
@@ -0,0 +1,59 @@
|
|||||||
|
#include "serialCommand.h"
|
||||||
|
#include "serialPort.h"
|
||||||
|
#include "serialResponse.h"
|
||||||
|
#include <QCoreApplication>
|
||||||
|
#include <QThread>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
int main(int argc, char *argv[]) {
|
||||||
|
QCoreApplication app(argc, argv);
|
||||||
|
|
||||||
|
// Replace with your actual port name
|
||||||
|
const QString portName = "/dev/ttyUSB0";
|
||||||
|
|
||||||
|
// Create SerialPort object
|
||||||
|
SerialPort serial(portName);
|
||||||
|
|
||||||
|
// Open the serial port
|
||||||
|
if (!serial.openPort()) {
|
||||||
|
qDebug() << "Failed to open serial port";
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
SerialCommand commands;
|
||||||
|
|
||||||
|
while (true) {
|
||||||
|
commands.printCommands();
|
||||||
|
|
||||||
|
// Get user input
|
||||||
|
std::cout << "Enter a command (0 to exit): ";
|
||||||
|
int16_t number;
|
||||||
|
std::cin >> number;
|
||||||
|
|
||||||
|
// Check if the input is within the valid range for uint8_t
|
||||||
|
if (number < 0 || number > commands.getCommandCount() - 1) {
|
||||||
|
qWarning() << "Number (" << qPrintable(QString::number(number))
|
||||||
|
<< ") out of range 0 -"
|
||||||
|
<< qPrintable(QString::number(commands.getCommandCount() - 1));
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Exit loop if user enters 0
|
||||||
|
if (number == 0)
|
||||||
|
break;
|
||||||
|
|
||||||
|
// Example command to send (replace with actual Siyi A8 mini camera
|
||||||
|
// commands)
|
||||||
|
QByteArray command = commands.getCommand((uint8_t)number);
|
||||||
|
serial.sendCommand(command);
|
||||||
|
|
||||||
|
// Read response from the camera
|
||||||
|
QByteArray response = serial.readResponse();
|
||||||
|
SerialResponse::printResponse(response);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Close the serial port
|
||||||
|
serial.closePort();
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -0,0 +1,175 @@
|
|||||||
|
#include "serialCommand.h"
|
||||||
|
#include "crc16.h"
|
||||||
|
#include <QDebug>
|
||||||
|
#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.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)"});
|
||||||
|
}
|
||||||
|
|
||||||
|
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() {
|
||||||
|
uint8_t index = 0;
|
||||||
|
foreach (Command command, mSerialCommands) {
|
||||||
|
qInfo().noquote() << QString::number(index) + ": " + command.description;
|
||||||
|
index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SerialCommand::setExtraValues(uint8_t number) {
|
||||||
|
if (number == 1) {
|
||||||
|
QByteArray command = mSerialCommands.at(number).command;
|
||||||
|
int16_t degrees;
|
||||||
|
|
||||||
|
std::cout << "Enter yaw degrees (-135 -> 135): ";
|
||||||
|
std::cin >> degrees;
|
||||||
|
degrees = degrees * 10;
|
||||||
|
command[8] = degrees & 0xFF;
|
||||||
|
command[9] = degrees >> 8;
|
||||||
|
|
||||||
|
std::cout << "Enter pitch degrees (-90 -> 25): ";
|
||||||
|
std::cin >> degrees;
|
||||||
|
degrees = degrees * 10;
|
||||||
|
command[10] = degrees & 0xFF;
|
||||||
|
command[11] = degrees >> 8;
|
||||||
|
|
||||||
|
mSerialCommands[number].command = command;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
QByteArray SerialCommand::getCommand(uint8_t number) {
|
||||||
|
setExtraValues(number);
|
||||||
|
|
||||||
|
QByteArray command = mSerialCommands.at(number).command;
|
||||||
|
|
||||||
|
uint8_t crcBytes[2];
|
||||||
|
CRC16::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;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t SerialCommand::getCommandCount() { return mSerialCommands.size(); }
|
||||||
@@ -0,0 +1,23 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <QList>
|
||||||
|
#include <QString>
|
||||||
|
|
||||||
|
struct Command {
|
||||||
|
QByteArray command;
|
||||||
|
QString description;
|
||||||
|
};
|
||||||
|
|
||||||
|
class SerialCommand {
|
||||||
|
public:
|
||||||
|
SerialCommand();
|
||||||
|
~SerialCommand();
|
||||||
|
QByteArray createByteArray(const std::initializer_list<int> &bytes);
|
||||||
|
void printCommands(void);
|
||||||
|
QByteArray getCommand(uint8_t number);
|
||||||
|
void setExtraValues(uint8_t number);
|
||||||
|
uint8_t getCommandCount();
|
||||||
|
|
||||||
|
private:
|
||||||
|
QList<Command> mSerialCommands;
|
||||||
|
};
|
||||||
@@ -0,0 +1,53 @@
|
|||||||
|
#include "serialPort.h"
|
||||||
|
#include <QDebug>
|
||||||
|
|
||||||
|
SerialPort::SerialPort(const QString &portName) : mSerialPort(portName) {
|
||||||
|
mSerialPort.setPortName(portName);
|
||||||
|
mSerialPort.setBaudRate(QSerialPort::Baud115200);
|
||||||
|
mSerialPort.setDataBits(QSerialPort::Data8);
|
||||||
|
mSerialPort.setStopBits(QSerialPort::OneStop);
|
||||||
|
mSerialPort.setFlowControl(QSerialPort::NoFlowControl);
|
||||||
|
}
|
||||||
|
|
||||||
|
SerialPort::~SerialPort() {
|
||||||
|
closePort(); // Close port if open on destruction
|
||||||
|
}
|
||||||
|
|
||||||
|
bool SerialPort::openPort() {
|
||||||
|
if (mSerialPort.isOpen()) {
|
||||||
|
qDebug() << "Port already open";
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return mSerialPort.open(QIODevice::ReadWrite);
|
||||||
|
}
|
||||||
|
|
||||||
|
void SerialPort::closePort() {
|
||||||
|
if (mSerialPort.isOpen()) {
|
||||||
|
mSerialPort.close();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void SerialPort::sendCommand(const QByteArray &command) {
|
||||||
|
if (!mSerialPort.isOpen()) {
|
||||||
|
qDebug() << "Error: Port not open";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
mSerialPort.write(command);
|
||||||
|
}
|
||||||
|
|
||||||
|
QByteArray SerialPort::readResponse() {
|
||||||
|
if (!mSerialPort.isOpen()) {
|
||||||
|
qDebug() << "Error: Port not open";
|
||||||
|
return QByteArray();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Read data from serial port until timeout or specific criteria met
|
||||||
|
QByteArray response;
|
||||||
|
while (mSerialPort.waitForReadyRead(5000)) { // Adjust timeout as needed
|
||||||
|
response.append(mSerialPort.readAll());
|
||||||
|
}
|
||||||
|
|
||||||
|
return response;
|
||||||
|
}
|
||||||
@@ -0,0 +1,18 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <QString>
|
||||||
|
#include <QSerialPort>
|
||||||
|
|
||||||
|
class SerialPort {
|
||||||
|
public:
|
||||||
|
SerialPort(const QString& portName);
|
||||||
|
~SerialPort();
|
||||||
|
|
||||||
|
bool openPort();
|
||||||
|
void closePort();
|
||||||
|
void sendCommand(const QByteArray& command);
|
||||||
|
QByteArray readResponse();
|
||||||
|
|
||||||
|
private:
|
||||||
|
QSerialPort mSerialPort;
|
||||||
|
};
|
||||||
@@ -0,0 +1,48 @@
|
|||||||
|
#include "serialResponse.h"
|
||||||
|
#include "crc16.h"
|
||||||
|
#include "defines.h"
|
||||||
|
#include <QDebug>
|
||||||
|
#include <QString>
|
||||||
|
|
||||||
|
void SerialResponse::printResponse(QByteArray response) {
|
||||||
|
QString responseStr;
|
||||||
|
for (int i = 0; i < response.size(); i++) {
|
||||||
|
responseStr +=
|
||||||
|
QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
|
||||||
|
}
|
||||||
|
qDebug() << "Response: " << responseStr;
|
||||||
|
|
||||||
|
responseStr = "";
|
||||||
|
uint8_t command = response.at(MESSAGE_IDX::CMD_ID);
|
||||||
|
|
||||||
|
// Check response data validity
|
||||||
|
uint8_t crcCheck[2];
|
||||||
|
CRC16::getCRCBytes(response.mid(0, response.size() - 2), crcCheck);
|
||||||
|
|
||||||
|
// Data not OK
|
||||||
|
if (crcCheck[0] != response.at(response.size() - 2) ||
|
||||||
|
crcCheck[1] != response.at(response.size() - 1)) {
|
||||||
|
qWarning() << "Response data not valid";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
qInfo() << "Response data is valid";
|
||||||
|
|
||||||
|
if (command == 0x0E) {
|
||||||
|
int16_t yaw = (static_cast<uint8_t>(response.at(9)) << 8) |
|
||||||
|
static_cast<uint8_t>(response.at(8));
|
||||||
|
int16_t pitch = (static_cast<uint8_t>(response.at(11)) << 8) |
|
||||||
|
static_cast<uint8_t>(response.at(10));
|
||||||
|
int16_t roll = (static_cast<uint8_t>(response.at(13)) << 8) |
|
||||||
|
static_cast<uint8_t>(response.at(12));
|
||||||
|
|
||||||
|
qInfo().noquote() << "Yaw: " << QString::number(yaw / 10) + "°";
|
||||||
|
qInfo().noquote() << "Pitch: " << QString::number(pitch / 10) + "°";
|
||||||
|
qInfo().noquote() << "Roll: " << QString::number(roll / 10) + "°";
|
||||||
|
} else {
|
||||||
|
for (int i = 0; i < response.size(); i++) {
|
||||||
|
responseStr +=
|
||||||
|
QString("%1").arg(response.at(i), 2, 16, QChar('0')).toUpper();
|
||||||
|
}
|
||||||
|
qInfo() << "Response: " << responseStr;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <QByteArray>
|
||||||
|
|
||||||
|
class SerialResponse {
|
||||||
|
public:
|
||||||
|
static void printResponse(QByteArray response);
|
||||||
|
};
|
||||||
Reference in New Issue
Block a user