FK – 野生黑猩猩自然保护基地

2024年11月10日2024年11月10日

[FK]天空端和地面端的摄像头发送接收程序-1

———————-.pro—————————–
QT += core gui

QT += core gui widgets # 确保 widgets 模块被包含

CONFIG += link_pkgconfig
PKGCONFIG += opencv4

QT += multimedia multimediawidgets

greaterThan(QT_MAJOR_VERSION, 4): QT += widgets

CONFIG += c++17

# You can make your code fail to compile if it uses deprecated APIs.
# In order to do so, uncomment the following line.
#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000 # disables all the APIs deprecated before Qt 6.0.0

SOURCES += \
CameraWorker.cpp \
main.cpp \
mainwindow.cpp \
picDecode.c \
tcpsend.cpp \
udpsendthread.cpp

HEADERS += \
CameraWorker.h \
includes.h \
mainwindow.h \
picDecode.h \
tcpsend.h \
udpsendthread.h

FORMS += \
mainwindow.ui

# Default rules for deployment.
qnx: target.path = /tmp/$${TARGET}/bin
else: unix:!android: target.path = /opt/$${TARGET}/bin
!isEmpty(target.path): INSTALLS += target

————–CameraWorker.h——————————

#ifndef CAMERAWORKER_H
#define CAMERAWORKER_H

#include
#include
#include
#include
#include

class CameraWorker : public QObject {
Q_OBJECT

public:
CameraWorker(QObject *parent = nullptr);
~CameraWorker();
void startCamera(int deviceIndex);
void stopCamera();

signals:
void frameCompress(const QByteArray &frameCompressData);

private:
cv::VideoCapture capture;
QMutex mutex;
bool isRunning;
void process();
void compressImageToMemory(const QImage &image);

};

#endif // CAMERAWORKER_H
—————————–includes.h——————————
#ifndef INCLUDES_H
#define INCLUDES_H

#define BASEPORT

#ifdef SKYPORT
#define LOCAL_IP “192.168.0.108”
#define LOCAL_PORT 12345
#define TARGET_IP “192.168.0.107”
#define TARGET_PORT 12000
#endif

#ifdef BASEPORT
#define LOCAL_IP “192.168.0.107”
#define LOCAL_PORT 12000
#define TARGET_IP “192.168.0.108”
#define TARGET_PORT 12345
#endif

#endif // INCLUDES_H

———————mainwindow.h—————————————

#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include
#include
#include “CameraWorker.h” // 添加这一行以包含 CameraWorker
#include

QT_BEGIN_NAMESPACE
namespace Ui { class MainWindow; }
QT_END_NAMESPACE

class MainWindow : public QMainWindow {
Q_OBJECT

public:
MainWindow(QWidget *parent = nullptr);
~MainWindow();

private slots:
void updateCameraFrame(const QImage &image);

void debugslot(const QByteArray &data);

private:
Ui::MainWindow *ui;
CameraWorker *cameraWorker; // 确保这个声明是正确的
QThread *cameraThread;
void bindThreadToCPU3(QThread *thread);
};

#endif // MAINWINDOW_H

—————————tcpsend.h————实际没用上—————
#ifndef TCPSEND_H
#define TCPSEND_H

#include
#include
#include
#include

class TcpSend : public QThread
{
Q_OBJECT

public:
explicit TcpSend(QObject *parent = nullptr);
~TcpSend();

void setTargetIp(const QString &ip);
void setTargetPort(quint16 port);

signals:
void dataReceived(const QByteArray &data);

protected:
void run() override;

private:
QTcpSocket *tcpSocket;
QString targetIp;
quint16 targetPort;

public slots:
void sendData(); // 实现发送功能
};

#endif // TCPSEND_H

——————————udpsendthread.h———————-
// udpsendthread.h
#ifndef UDPSENDTHREAD_H
#define UDPSENDTHREAD_H

#include
#include
#include

class UdpSendThread : public QObject
{
Q_OBJECT

public:
unsigned int times_rcv = 0;
int flag_rcv = 0;
int udpbagflag;
unsigned int datasize_max;
int connect_flag =0;
QByteArray picData;
public:
explicit UdpSendThread(QObject *parent = nullptr);
~UdpSendThread();

public slots:
void process(); // 模拟数据接收并触发dataReceived信号
void sendData(const QByteArray &data); // 实现UDP发送功能

void dataReceived(); // 当接收到数据时发出此信号

signals:

void frameCaptured(const QImage &image);

private:
QUdpSocket* udpSocket;
QString targetIP;
quint16 targetPort;
QImage decompressImageFromMemory( QByteArray &compressedData);

signals:
void dataRec(const QByteArray &data);

public slots:
void processData();
};

#endif // UDPSENDTHREAD_H

———————————–CameraWorker.cpp————————-

#include “CameraWorker.h”
#include // 包含 QDebug 以使用 qDebug()
#include
#include
#include
#include
#include “includes.h”

CameraWorker::CameraWorker(QObject *parent) : QObject(parent), isRunning(false) {}

CameraWorker::~CameraWorker() {
stopCamera();
}

void CameraWorker::startCamera(int deviceIndex) {
if (capture.isOpened()) return;

capture.open(deviceIndex);
if (!capture.isOpened()) {
qDebug() << "无法打开摄像头"; // 这里可以正常使用 qDebug() return; } capture.set(cv::CAP_PROP_FRAME_WIDTH, 640); // 设置宽度 capture.set(cv::CAP_PROP_FRAME_HEIGHT, 480); // 设置高度 capture.set(cv::CAP_PROP_FPS, 25); // 设置帧率 capture.set(cv::CAP_PROP_EXPOSURE, -4); // 设置曝光（根据需要调整） capture.set(cv::CAP_PROP_GAIN, 0); // 设置增益（根据需要调整） QThread::msleep(100); // 暂停以让设置生效 isRunning = true; QThread *thread = QThread::create([this]() { process(); }); connect(thread, &QThread::finished, this, &CameraWorker::deleteLater); thread->start();
}

void CameraWorker::stopCamera() {
QMutexLocker locker(&mutex);
isRunning = false;
capture.release();
}

void CameraWorker::process() {

#ifdef SKYPORT
while (true) {
{
QMutexLocker locker(&mutex);
if (!isRunning) break;
}

cv::Mat frame;
capture >> frame;
if (frame.empty()) continue;

cv::cvtColor(frame, frame, cv::COLOR_BGR2RGB);
QImage image(frame.data, frame.cols, frame.rows, frame.step[0], QImage::Format_RGB888);

size_t memory_usage_qimage = image.width() * image.height() * image.depth() / 8;
// qDebug() << "QImage memory usage: " << memory_usage_qimage / 1024.0 / 1024.0 << " MB"; compressImageToMemory(image); //emit frameCaptured(image); QThread::msleep(50); // 限制帧率 } #endif } void CameraWorker::compressImageToMemory(const QImage &image) { // 创建一个 QBuffer 对象，它将充当内存缓冲区 QBuffer buffer; buffer.open(QIODevice::WriteOnly); // 创建 QImageWriter 对象，用于将图像压缩并写入内存缓存 QImageWriter writer(&buffer, "JPEG"); // 使用 JPEG 格式进行压缩，可以选择其他格式 // 设置压缩质量（如果适用） writer.setQuality(80); // 设定压缩质量，范围是 0-100 // 将 QImage 压缩并写入缓存 if (writer.write(image)) { // 压缩成功后，获取压缩后的数据大小 size_t compressedSize = buffer.size(); //qDebug() << "Compressed image size: " << compressedSize / 1024.0 / 1024.0 << " MB"; // 你可以从缓存中获取压缩后的数据 QByteArray compressedData = buffer.data(); // 例如，可以将数据保存到文件，或者发送到网络 emit frameCompress(compressedData); } else { qDebug() << "Image compression failed!"; } } ----------------------------------main.cpp------------------------ #include "mainwindow.h" #include
#include

int main(int argc, char *argv[]) {
QApplication a(argc, argv);
MainWindow w;
w.show();

int cpu = sched_getcpu();
qDebug() << "Running on CPU: " << cpu ; return a.exec(); } --------------------------------CameraWorker.cpp------------------- #include "CameraWorker.h" #include // 包含 QDebug 以使用 qDebug()
#include
#include
#include
#include
#include “includes.h”

CameraWorker::CameraWorker(QObject *parent) : QObject(parent), isRunning(false) {}

CameraWorker::~CameraWorker() {
stopCamera();
}

void CameraWorker::startCamera(int deviceIndex) {
if (capture.isOpened()) return;

void CameraWorker::stopCamera() {
QMutexLocker locker(&mutex);
isRunning = false;
capture.release();
}

void CameraWorker::process() {

#ifdef SKYPORT
while (true) {
{
QMutexLocker locker(&mutex);
if (!isRunning) break;
}

cv::Mat frame;
capture >> frame;
if (frame.empty()) continue;

cv::cvtColor(frame, frame, cv::COLOR_BGR2RGB);
QImage image(frame.data, frame.cols, frame.rows, frame.step[0], QImage::Format_RGB888);

size_t memory_usage_qimage = image.width() * image.height() * image.depth() / 8;
// qDebug() << "QImage memory usage: " << memory_usage_qimage / 1024.0 / 1024.0 << " MB"; compressImageToMemory(image); //emit frameCaptured(image); QThread::msleep(50); // 限制帧率 } #endif } void CameraWorker::compressImageToMemory(const QImage &image) { // 创建一个 QBuffer 对象，它将充当内存缓冲区 QBuffer buffer; buffer.open(QIODevice::WriteOnly); // 创建 QImageWriter 对象，用于将图像压缩并写入内存缓存 QImageWriter writer(&buffer, "JPEG"); // 使用 JPEG 格式进行压缩，可以选择其他格式 // 设置压缩质量（如果适用） writer.setQuality(80); // 设定压缩质量，范围是 0-100 // 将 QImage 压缩并写入缓存 if (writer.write(image)) { // 压缩成功后，获取压缩后的数据大小 size_t compressedSize = buffer.size(); //qDebug() << "Compressed image size: " << compressedSize / 1024.0 / 1024.0 << " MB"; // 你可以从缓存中获取压缩后的数据 QByteArray compressedData = buffer.data(); // 例如，可以将数据保存到文件，或者发送到网络 emit frameCompress(compressedData); } else { qDebug() << "Image compression failed!"; } } -------------------mainwindow.cpp-------------------------------- #include "mainwindow.h" #include "ui_mainwindow.h" #include // 包含 QDebug 以使用 qDebug()
#include “udpsendthread.h”
#include “tcpsend.h”
#include
#include “UdpReceiver.h”
#include “includes.h”

MainWindow::MainWindow(QWidget *parent)
: QMainWindow(parent), ui(new Ui::MainWindow), cameraWorker(new CameraWorker)
{
ui->setupUi(this);

#ifdef SKYPORT
// 创建线程
cameraThread = new QThread;
cameraWorker->moveToThread(cameraThread);

connect(cameraThread, &QThread::started, [this]() {
cameraWorker->startCamera(0); // 打开默认摄像头
});
connect(cameraThread, &QThread::finished, cameraWorker, &CameraWorker::deleteLater);

bindThreadToCPU3(cameraThread);

cameraThread->start();
#endif
// 创建线程和数据发送对象
QThread* udpthread = new QThread();
UdpSendThread* udpSendThread = new UdpSendThread();

// 将 udpSendThread 移动到新线程
udpSendThread->moveToThread(udpthread);

// 使用lambda函数启动信号槽
QObject::connect(udpthread, &QThread::started, udpSendThread, &UdpSendThread::process);

// 绑定信号槽，当udpSendThread接收到数据后发送数据
//QObject::connect(udpSendThread, &UdpSendThread::dataReceived, udpSendThread, &UdpSendThread::sendData);

QObject::connect(udpSendThread, &UdpSendThread::frameCaptured, this, &MainWindow::updateCameraFrame);

QObject::connect(udpSendThread, &UdpSendThread::dataRec, [](const QByteArray &data) {
qDebug() << "Received data:" << data; }); bindThreadToCPU3(udpthread); // 启动线程 udpthread->start();

connect(cameraWorker, &CameraWorker::frameCompress, udpSendThread, &UdpSendThread::sendData);

}

MainWindow::~MainWindow() {
cameraThread->quit();
cameraThread->wait();
delete ui;
}

void MainWindow::updateCameraFrame(const QImage &image) {
ui->cameraLabel->setPixmap(QPixmap::fromImage(image)); // 假设你在 UI 中有一个 QLabel 名为 cameraLabel
}

void MainWindow::bindThreadToCPU3(QThread *thread) {
// 获取当前线程的原生线程句柄
pthread_t nativeThread = pthread_self(); // 使用 pthread_self() 获取当前线程的句柄

// 创建 CPU 亲和性掩码
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
CPU_SET(3, &cpuset); // 绑定到 CPU3

// 设置线程 CPU 亲和性
if (pthread_setaffinity_np(nativeThread, sizeof(cpu_set_t), &cpuset) != 0) {
qDebug() << "无法设置线程的 CPU 亲和性"; } else { qDebug() << "线程成功绑定到 CPU3"; } } void MainWindow::debugslot(const QByteArray &data) { qDebug()<<"hello"; } --------------------------------tcpsend.cpp------------------------------------ #include "tcpsend.h" #include
#include
#include

TcpSend::TcpSend(QObject *parent) : QThread(parent), targetPort(0), tcpSocket(nullptr) {
tcpSocket = new QTcpSocket(this);
}

TcpSend::~TcpSend() {
if (tcpSocket->isOpen()) {
tcpSocket->close();
}
}

void TcpSend::setTargetIp(const QString &ip) {
targetIp = ip;
}

void TcpSend::setTargetPort(quint16 port) {
targetPort = port;
}

void TcpSend::run() {
// Bind thread to CPU3 (core 3)
cpu_set_t cpuset;
CPU_ZERO(&cpuset);
CPU_SET(3, &cpuset);
pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);

// Connect to TCP server
tcpSocket->connectToHost(QHostAddress(targetIp), targetPort);
if (!tcpSocket->waitForConnected(3000)) {
qDebug() << "Error: Unable to connect to server!"; return; } /* connect(this, &TcpSend::dataReceived, [this](const QByteArray &data) { if (tcpSocket->isOpen()) {
tcpSocket->write(data);
tcpSocket->flush();
qDebug() << "Data sent: " << data; } }); */ exec(); // Start the event loop for the thread } void TcpSend::sendData() { QByteArray data = "Hello from tcpsend"; // 要发送的数据 if (tcpSocket->isOpen()) {
tcpSocket->write(data);
tcpSocket->flush();
qDebug() << "tcp Data sent: " << data; } int cpu = sched_getcpu(); qDebug() << "Running on CPU: " << cpu ; } --------------------------------udpsendthread.cpp------------------------ // udpsendthread.cpp #include "udpsendthread.h" #include
#include
#include
#include
#include
#include “includes.h”
#include
#include
#include
#include

UdpSendThread::UdpSendThread(QObject *parent) : QObject(parent),
udpSocket(new QUdpSocket(this))
{
if (!udpSocket->bind(QHostAddress(LOCAL_IP), LOCAL_PORT))
{
qDebug() << "Bind failed!"; } else { qDebug() << "Bind successful!"; } qDebug()<<"start startReceiving"; connect(udpSocket, &QUdpSocket::readyRead, this, &UdpSendThread::processData); } UdpSendThread::~UdpSendThread() { udpSocket->close();
delete udpSocket;
}

void UdpSendThread::process()
{
#ifdef BASEPORT
// 模拟数据接收，可以使用 QTimer 定时发出 dataReceived 信号
QTimer* timer = new QTimer(this);
connect(timer, &QTimer::timeout, this, &UdpSendThread::dataReceived);
timer->start(2); // 每5ms触发一次 dataReceived 信号
#endif
}

void UdpSendThread::sendData(const QByteArray & data)
{
//QByteArray datas = “Hello from udpsend”; // 要发送的数据
//udpSocket->writeDatagram(data, QHostAddress(targetIP), targetPort);
//qDebug() << "Data sent to" << targetIP << ":" << targetPort; //int cpu = sched_getcpu(); //qDebug() << "Running on CPU: " << cpu ; const int maxPacketSize = 65500; // UDP 单个数据包的最大数据量 int dataSize = data.size(); int offset = 0; if (datasize_max < dataSize) { datasize_max = dataSize; qDebug()<< "mx = " <writeDatagram(packet.mid(bytesSent), QHostAddress(TARGET_IP), TARGET_PORT);
if (result == -1) {
qDebug() << "Failed to send data:" << udpSocket->errorString();
QThread::msleep(10); // 短暂等待后再重试，避免过多的 CPU 占用
continue;
}
bytesSent += result;
}

offset += packetSize; // 更新偏移量
QThread::msleep(10); // 短暂等待后再重试，避免过多的 CPU 占用
/*
int packetSize = qMin(maxPacketSize, dataSize – offset); // 每次发送的字节数
QByteArray packet = data.mid(offset, packetSize); // 获取分包数据

qint64 bytesSent = udpSocket->writeDatagram(packet, QHostAddress(TARGET_IP), TARGET_PORT);
offset += packetSize;

if (udpSocket->waitForBytesWritten(3000)) {
;
} else {
qDebug() << "Timeout or error while writing data."; } //QThread::msleep(2); // 限制帧率 */ } auto end = std::chrono::high_resolution_clock::now(); // 计算时间差 auto duration = std::chrono::duration_cast(end – start).count();

//qDebug()<< "发送耗时: " << duration << "us" ; } void UdpSendThread::processData() { QImage rcvimage ; QNetworkDatagram datagram ; while (udpSocket->hasPendingDatagrams())
{
times_rcv = 0;
flag_rcv = 1;
datagram = udpSocket->receiveDatagram();
picData.append(datagram.data());
}

/*
QByteArray lastTwoBytes ;
if (datagram.data().size()>=2)
{
lastTwoBytes = datagram.data().right(2);
if ((lastTwoBytes[0]==0xFF)&&(lastTwoBytes[1]==0xD9))
{
udpbagflag = 1;
}
}

if (udpbagflag!=0)
{
rcvimage = decompressImageFromMemory(picData);
emit frameCaptured(rcvimage);
picData.clear();
udpbagflag = 0;

}
*/
//int cpu = sched_getcpu();
//qDebug() << "Running on CPU: " << cpu ; } QImage UdpSendThread::decompressImageFromMemory(QByteArray &compressedData) { int i = 0; QBuffer buffer(&compressedData); buffer.open(QIODevice::ReadOnly); // 打开为只读模式 // 使用 QImageReader 从 QBuffer 中读取压缩的图像 QImageReader reader(&buffer); QImage image = reader.read(); if (image.isNull()) { qDebug() << "Failed to decompress the image."; } return image; } void UdpSendThread::dataReceived() { QImage rcvimage ; if (times_rcv < 60000) { times_rcv++; } if (times_rcv < 10) { return; } if (flag_rcv ==0) { return; } flag_rcv = 0; rcvimage = decompressImageFromMemory(picData); emit frameCaptured(rcvimage); picData.clear(); }

2024年6月11日2024年6月15日

[FK]穿越机翻滚

————————–以下方式没实际使用上———————-

pitch的范围是-90°-0-90°
roll的范围是-180°-180°
yaw的范围是-180°-180°

1.首先确定坐标系，是ENU坐标系还是NED坐标系，
方法是看x,y,z的正方向，x指向正东，y指向正北，z指向天则为ENU坐标系。x指向正北，y指向正东，z指向地则为NED坐标系
已经确认我用的是ENU坐标系，z对应yaw ， y对应pitch ，x对应roll

2.确定旋转方向
方法用右手定则，大拇指先指向Z，绕Z的正方向（其余手指）旋转，yaw变化，其余pitch和roll不变。第二步大拇指指向Y，pitch变化，其余yaw和roll不变化，第三步，大拇指指向X的正方向，roll变化，其余两轴不变化。
则确认欧拉角的旋转方向是Z-Y-X，确认旋转方向至关重要。

3.理解欧拉角、姿态角、四元数、旋转矩阵

端午节期间在疑似新冠头晕后遗症、娃的爱的奥特曼铁拳以及老婆的鼓励下，终于发现点眉目

发现roll偏转一定角度后，pitch怎么旋转，角度都不能到90°，在pitch由0°-90°-0°旋转时，得出的角度都是0°-30°-0°这样，于是查阅资料，发现有旋转矩阵这个东西，找个各个版本的，重新确认旋转方向，将角度带进去，不对，求转置矩阵，还是不对，最后把三个向量[1 0 0 ][0 1 0] [0 0 1]带进去，好像是对了 – -，以下是代码

—————————————————————————————————————
#include #include
#include
#include

#define MI 0.017453292519943295
#define M_PI 3.14159265358979323846

struct _Quat{
double w;
double x;
double y;
double z;
};

struct _Quat Quat;

void ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
// Abbreviations for the various angular function

yaw = yaw*MI;
pitch = pitch*MI;
roll = roll*MI;

double w,x,y,z;
double cy = cos(yaw * 0.5);
double sy = sin(yaw * 0.5);
double cp = cos(pitch * 0.5);
double sp = sin(pitch * 0.5);
double cr = cos(roll * 0.5);
double sr = sin(roll * 0.5);

Quat.w = cy * cp * cr + sy * sp * sr;
Quat.x = cy * cp * sr – sy * sp * cr;
Quat.y = sy * cp * sr + cy * sp * cr;
Quat.z = sy * cp * cr – cy * sp * sr;
// printf(“四元数:\r\n”);
// printf(“w = %f\r\nx = %f\r\ny = %f\r\nz = %f\r\n”,Quat.w,Quat.x,Quat.y,Quat.z);
}

int Att_Convert(float y,float p,float r)
{
float cx ,cy ,cz;
float yx ,yy ,yz;

r = r*MI;
p = p*MI;
y = y*MI;

float yc11 = cos(y);
float yc12 = -sin(y);
float yc13 = 0;

float yc21 = sin(y);
float yc22 = cos(y);
float yc23 = 0;

float yc31 = 0;
float yc32 = 0;
float yc33 = 1;

float rc11 = 1;
float rc12 = 0;
float rc13 = 0;

float rc21 = 0;
float rc22 = cos(r);
float rc23 = -sin(r);

float rc31 = 0;
float rc32 = sin(r);
float rc33 = cos(r);

float c11 = cos(y)*cos(p);
float c21 = cos(p)*sin(y);
float c31 = -sin(p);

float c12 = cos(y)*sin(r)*sin(p)-cos(r)*sin(y);
float c22 = cos(y)*cos(r)+sin(y)*sin(r)*sin(p);
float c32 = cos(p)*sin(r);

float c13 = sin(y)*sin(r)+cos(y)*cos(r)*sin(p);
float c23 = cos(y)*sin(r)+cos(r)*sin(y)*sin(p);;
float c33 = cos(r)*cos(p);
/*
cx = y*c11+ p*c12 + r*c13;
cy = y*c21+ p*c22 + r*c23;
cz = y*c31+ p*c32 + r*c33;
*/

cx = r*c11+ p*c21 + y*c31;
cy = r*c12+ p*c22 + y*c32;
cz = r*c13+ p*c23 + y*c33;

yx = r * yc11 + p * yc12 + y * yc13;
yy = r * yc21 + p * yc22 + y * yc23;
yz = r * yc31 + p * yc32 + y * yc33;

cx = cx / MI;
cy = cy / MI;
cz = yz / MI;

yx = yx / MI;
yy = yy / MI;
yz = yz / MI;

printf(“yx’=%f\r\nyy’=%f\r\nyz’=%f\r\n”,cx,cy,cz);
}
int main(int argc,char * argv[])
{
float yaw = atof(argv[1]);
float pitch = atof(argv[2]);
float roll = atof(argv[3]);

// float q3 = atof(argv[4]);

float q0,q1,q2,q3;

double c11,c12,c13,c21,c22,c23,c31,c32,c33;

double g1;
double g2;
double g3;
double g4;
double g5;

float ax=0;
float ay=0;
float az=0;

float px,py,pz;

float cx1,cx2,cx3,cy1,cy2,cy3,cz1,cz2,cz3;

ToQuaternion(yaw,pitch,roll);

q0 = Quat.w;
q1 = Quat.x;
q2 = Quat.y;
q3 = Quat.z;

// printf (“四元数:\r\nq0=%f\r\nq1=%f\r\nq2=%f\r\nq3=%f\r\n”,q0,q1,q2,q3);

g1 = 2*(q1*q3-q0*q2);
g2 = 2*(q2*q3+q0*q1);
g3 = q0*q0-q1*q1-q2*q2+q3*q3;
g4 = 2*(q1*q2+q0*q3);
g5 = q0*q0+q1*q1-q2*q2-q3*q3;

c11 = q0*q0+q1*q1-q2*q2-q3*q3;
c21 = 2*(q1*q2+q0*q3);
c31 = 2*(q1*q3-q0*q2);

c12 = 2*(q1*q2 -q0*q3);
c22 = q0*q0-q1*q1+q2*q2-q3*q3;
c32 = 2*(q2*q3+q0*q1);

c13 = 2*(q1*q3 +q0*q2);
c23 = 2*(q2*q3-q0*q1);
c33 = q0*q0-q1*q1-q2*q2+q3*q3;
/*
cx1 = c11*0.6+c12 * 0 + c13 * 0;
cx2 = c21*0.6+c22 * 0 +c23 * 0;
cx3 = c31*0.6+c32 * 0 + c33 * 0;

cy1 = c11*0+c12 * -0.6 + c13 * 0;
cy2 = c21*0+c22 * -0.6 +c23 * 0;
cy3 = c31*0+c32 * -0.6 + c33 * 0;

cz1 = c11*0+c12 * 0 + c13 * -0.6;
cz2 = c21*0+c22 * 0 +c23 * -0.6;
cz3 = c31*0+c32 * 0 + c33 * -0.6;
*/

cx1 = c11*1+c12 * 0 + c13 * 0;
cx2 = c21*1+c22 * 0 + c23 * 0;
cx3 = c31*1+c32 * 0 + c33 * 0;

cy1 = c11*0+c12 * 1 + c13 * 0;
cy2 = c21*0+c22 * 1 + c23 * 0;
cy3 = c31*0+c32 * 1 + c33 * 0;

cz1 = c11*0+c12 * 0 + c13 * 1;
cz2 = c21*0+c22 * 0 + c23 * 1;
cz3 = c31*0+c32 * 0 + c33 * 1;

//printf(“旋转矩阵\r\n”);
// printf(“c11=%f c12=%f c13=%f\r\nc21=%f c22=%f c23=%f\r\nc31=%f c32=%f c33=%f\r\n”,c11,c12,c13,c21,c22,c23,c31,c32,c33);
printf(“向量\r\n”);
printf(“cx1=%f cx2=%f cx3=%f\r\n”,cx1,cx2,cx3);
printf(“cy1=%f cy2=%f cy3=%f\r\n”,cy1,cy2,cy3);
printf(“cz1=%f cz2=%f cz3=%f\r\n”,cz1,cz2,cz3);

ax = atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI; //pitch ,绕y轴旋转，取y-xoz平面的角度
ay = atan2(cz1,sqrt(cz2*cz2 + cz3*cz3))/MI; //yaw ,绕z轴旋转，取z-xoy平面的角度
az = atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //roll ,绕X轴旋转，取x-yoz平面的角度
/*
if(cx1 >=0)
{
ax = atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI;
}
else
{
ax = 180 – atan2(cx2,sqrt(cx1*cx1 + cx3*cx3))/MI;
}

if(ax > 180)
{
ax = ax-360;
}

if (cx1 >=0 )
{
ay = -atan2(cx3,sqrt(cx1*cx1 + cx2*cx2))/MI; //PITCH 绕
}
else
{
ay = 180+atan2(cx3,sqrt(cx1*cx1 + cx2*cx2))/MI; //PITCH 绕
}
if (ay>180)
{
ay = ay-360;
}

if (cy2>=0)
{
az = atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //PITCH 绕
}
else
{
az = 180 – atan2(cy3,sqrt(cy1*cy1 + cy2*cy2))/MI; //PITCH 绕
}
if (az > 180)
{
az = az-360;
}
*/
/*
ax = atan2(cz1,sqrt(cz2*cz2 + cz3*cz3))/MI;
ay = atan2(cz2,sqrt(cz1*cz1 + cz3*cz3))/MI;
az = atan2(cz3,sqrt(cz1*cz1 + cz2*cz2))/MI;
*/
// ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3)); //
// az = atan2(cx3,sqrt(cx1*cx1 + cx2*cx2)); //
/*
ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3));

ay = atan2(cy2,sqrt(cy1*cy1 + cy3*cy3));
az = atan2(cz3,sqrt(cz1*cz1 + cz2*cz2));

*/
printf(“角度\r\n”);
printf(“ax =%f \r\n”,ax);
printf(“ay=%f \r\n”,ay);
printf(“az =%f \r\n”,az);

// Att_Convert(y,p,r);
}

—————————————————————————————
求完后还是感觉不对，压根就没解决pitch到不了90°的问题，于是还是从欧拉角下手，根据pitch-roll的旋转方向，打算推出roll对pitch的影响，即当pitch是30°时，roll从0-90°变化的时候，让pitch由30-90°变化，继续冥思苦想，说实话年纪大了空间想象能力是真的差，最后想到了切西瓜的模型，假设在半径为1的西瓜上来回切，最终通过三角函数解决了，以下是代码
————————————————————————————————————————————–
#include #include
#include
#include
#define MI 0.017453292519943295
#define M_PI 3.14159265358979323846

int main(int argc,char * argv[])
{
float a1,a2,a3;

float a = atof(argv[1]);
float b = atof(argv[2]);

float x1,x2,x3,x4;

float c = 0;

float theta = a*MI;

float A = 2*sin(theta/2);

float B = sqrt(2);
float C = sqrt(2);

float D = acos(A/2/C)/MI; //

float E = sin(45*MI);
float F = 0;
float H = 0;

float G = 0;
b = 45+b;

F = E / tan(b*MI);
F = E -F;

//A F D
G = sqrt(A*A+F*F-2*A*F*cos(D*MI));
//1 F 45
H = sqrt(1*1+F*F-2*1*F*cos(45*MI));

printf(“A = %f\r\n”,A);
printf(“b = %f\r\n”,b);
printf(“E = %f\r\n”,E);
printf(“F = %f\r\n”,F);
printf(“D = %f\r\n”,D);
printf(“G = %f\r\n”,G);
printf(“H = %f\r\n”,H);

// 1 G H

a = 1;
b = G;
c = H;

printf(“\r\n”);
printf(“D1 = %f\r\n”,a);
printf(“D2 = %f\r\n”,b);
printf(“D3 = %f\r\n”,c);

/*
cosA=（b²+c²-a²）/2bc；
cosB=(a²+c²-b²)/2ac；
cosC=(a²+b²-c²)/2ab。
*/

x1 = acos((b*b+b*b-a*a)/(2*b*c));
x2 = acos((a*a+c*c-b*b)/(2*a*c));
x3 = acos((a*a+b*b-c*c)/(2*a*b));

x1= x1/MI;
x2= x2/MI;
x3= x3/MI;

printf(“x1 = %f\r\n”,x1);
printf(“x2 = %f\r\n”,x2);
printf(“x3 = %f\r\n”,x3);
}

x2是最终求的数据
———————————————————————————————
求出来了带进飞控里看下呗，还真不行，如果roll转动的话直接导致pitch变化，当有roll的时候，pitch会基本等于roll，但是遥控器给出的pitch还是0，肯定会导致飞机转圈，以后再解决把，最后还是用的原始欧拉角- –

陆陆续续四轴搞了一年了，手搓的机架、飞控、遥控器、姿态调节、上位机程序、无线LoRa数传，也飞起来的，那就暂时告一段落吧，先搞搞linux，搞定图传后再研究四轴FPV