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

Simple program to control Siyi A8 mini gimbal

Browse Source 
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
2024-05-26 12:56:51 +03:00
parent a8ba701138
commit 33399370f3
12 changed files with 558 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
misc/camera/a8/.gitignore
+74
View File
@@ -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
misc/camera/a8/a8.pro
+24
View File
@@ -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
misc/camera/a8/crc16.cpp
+52
View File
@@ -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;
}
misc/camera/a8/crc16.h
+12
View File
@@ -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);
};
misc/camera/a8/defines.h
+12
View File
@@ -0,0 +1,12 @@
#pragma once
enum MESSAGE_IDX
{
STX = 0,
CTRL = 2,
Data_len = 3,
SEQ = 5,
CMD_ID = 7,
DATA = 8
};
misc/camera/a8/main.cpp
+59
View File
@@ -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;
}
misc/camera/a8/serialCommand.cpp
+175
View File
@@ -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(); }
misc/camera/a8/serialCommand.h
+23
View File
@@ -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;
};
misc/camera/a8/serialPort.cpp
+53
View File
@@ -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;
}
misc/camera/a8/serialPort.h
+18
View File
@@ -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;
};
misc/camera/a8/serialResponse.cpp
+48
View File
@@ -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;
}
}
misc/camera/a8/serialResponse.h
+8
View File
@@ -0,0 +1,8 @@
#pragma once
#include <QByteArray>
class SerialResponse {
public:
static void printResponse(QByteArray response);
};