From 0703afceb8eb6062ec069f057f0456d3604b8294 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sun, 6 Apr 2025 10:51:46 -0700 Subject: [PATCH 1/2] Easy setup for board orientation --- .../src/src/MadgwickAHRS/MadgwickAHRS.cpp | 64 ++++++--- firmware/src/src/MadgwickAHRS/MadgwickAHRS.h | 27 +++- firmware/src/src/basetrackersettings.h | 31 +++-- firmware/src/src/sense.cpp | 92 ++++--------- gui/src/basetrackersettings.h | 33 +++-- gui/src/mainwindow.cpp | 122 +++++++++++++++-- gui/src/mainwindow.h | 10 +- gui/src/mainwindow.ui | 123 +++++++----------- gui/src/trackersettings.cpp | 34 +++-- gui/src/trackersettings.h | 3 + settings/settings.csv | 9 +- 11 files changed, 331 insertions(+), 217 deletions(-) diff --git a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.cpp b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.cpp index efc25687..02a97bd4 100644 --- a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.cpp +++ b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.cpp @@ -50,6 +50,10 @@ Madgwick::Madgwick() q1 = 0.0f; q2 = 0.0f; q3 = 0.0f; + qrot[0] = 1.0f; + qrot[1] = 0.0f; + qrot[2] = 0.0f; + qrot[3] = 0.0f; anglesComputed = 0; } @@ -101,6 +105,22 @@ void Madgwick::begin(float ax, float ay, float az, float mx, float my, float mz) anglesComputed = 0; } +void Madgwick::alignToAccelVect(float ax, float ay, float az) +{ + LOG_INF("alignToAccelVect: ax=%f, ay=%f, az=%f",(double)ax, (double)ay, (double)az); + + float va, vx, vy, vz; // rotation angle and vector + cross(ax, ay, az, 0, 0, 1, vx, vy, vz); + norm(ax, ay, az); + norm(vx, vy, vz); + va = acos(dot(ax, ay, az, 0, 0, 1)); + qrot[0] = cos(va / 2.0f); + qrot[1] = vx * sin(va / 2.0f); + qrot[2] = vy * sin(va / 2.0f); + qrot[3] = vz * sin(va / 2.0f); + normalizeQuat(qrot[0], qrot[1], qrot[2], qrot[3]); +} + void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat) { @@ -210,12 +230,7 @@ void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az q2 += qDot3 * deltat; q3 += qDot4 * deltat; - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; + normalizeQuat(q0, q1, q2, q3); // Normalise quaternion anglesComputed = 0; } @@ -285,12 +300,7 @@ void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float q2 += qDot3 * deltat; q3 += qDot4 * deltat; - // Normalise quaternion - recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; + normalizeQuat(q0, q1, q2, q3); // Normalise quaternion anglesComputed = 0; } @@ -312,6 +322,17 @@ float Madgwick::invSqrt(float x) return y; } + + +void Madgwick::normalizeQuat(float &qi0, float &qi1, float &qi2, float &qi3) +{ + float recipNorm = invSqrt(qi0 * qi0 + qi1 * qi1 + qi2 * qi2 + qi3 * qi3); + qi0 *= recipNorm; + qi1 *= recipNorm; + qi2 *= recipNorm; + qi3 *= recipNorm; +} + // Aligns two vectors (changes quaternion!) void Madgwick::align(float ax, float ay, float az, float bx, float by, float bz) { @@ -382,8 +403,21 @@ void Madgwick::norm(float &ax, float &ay, float &az) void Madgwick::computeAngles() { - roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); - pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); - yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); + float a2 = q0; + float b2 = q1; + float c2 = q2; + float d2 = q3; + float a1 = qrot[0]; + float b1 = -qrot[1]; + float c1 = -qrot[2]; + float d1 = -qrot[3]; + float qo0, qo1, qo2, qo3; + qo0 = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2; + qo1 = a1 * b2 + b1 * a2 - c1 * d2 + d1 * c2; + qo2 = a1 * c2 + c1 * a2 - d1 * b2 + b1 * d2; + qo3 = a1 * d2 + d1 * a2 - b1 * c2 + c1 * b2; + roll = atan2f(qo0 * qo1 + qo2 * qo3, 0.5f - qo1 * qo1 - qo2 * qo2); + pitch = asinf(-2.0f * (qo1 * qo3 - qo0 * qo2)); + yaw = atan2f(qo1 * qo2 + qo0 * qo3, 0.5f - qo2 * qo2 - qo3 * qo3); anglesComputed = 1; } diff --git a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h index f26d129f..9bb738ce 100644 --- a/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h +++ b/firmware/src/src/MadgwickAHRS/MadgwickAHRS.h @@ -49,6 +49,8 @@ class Madgwick void align(float ax, float ay, float az, float bx, float by, float bz); void combine(float p0, float p1, float p2, float p3); void rotate(float &ax, float &ay, float &az); + void normalizeQuat(float &q0, float &q1, float &q2, float &q3); + float qrot[4]; //------------------------------------------------------------------------------------------- // Function declarations @@ -56,6 +58,15 @@ class Madgwick Madgwick(void); void begin(float pitch, float roll, float yaw); void begin(float ax, float ay, float az, float mx, float my, float mz); + void alignToAccelVect(float ax, float ay, float az); + void setRotQuat(float qw, float qx, float qy, float qz) + { + qrot[0] = qw; + qrot[1] = qx; + qrot[2] = qy; + qrot[3] = qz; + normalizeQuat(qrot[0], qrot[1], qrot[2], qrot[3]); + } void setGain(float gain) { beta = gain; } void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat); @@ -92,10 +103,20 @@ class Madgwick return yaw; } - float *getQuat() + void getQuat(float dest[4]) { - memcpy(_copyQuat, &q0, sizeof(float) * 4); - return _copyQuat; + float a2 = q0; + float b2 = q1; + float c2 = q2; + float d2 = q3; + float a1 = qrot[0]; + float b1 = -qrot[1]; + float c1 = -qrot[2]; + float d1 = -qrot[3]; + dest[0] = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2; + dest[1] = a1 * b2 + b1 * a2 - c1 * d2 + d1 * c2; + dest[2] = a1 * c2 + c1 * a2 - d1 * b2 + b1 * d2; + dest[3] = a1 * d2 + d1 * a2 - b1 * c2 + c1 * b2; } float deltatUpdate() diff --git a/firmware/src/src/basetrackersettings.h b/firmware/src/src/basetrackersettings.h index f950b553..493e825b 100644 --- a/firmware/src/src/basetrackersettings.h +++ b/firmware/src/src/basetrackersettings.h @@ -675,30 +675,40 @@ class BaseTrackerSettings { inline const bool& getDisMag() {return dismag;} void setDisMag(bool val=1) { dismag = val; } - // Board Rotation X + // Board Rotation Quaternion W + inline const float& getRotW() {return rotw;} + bool setRotW(float val=1) { + if(val >= -1 && val <= 1) { + rotw = val; + return true; + } + return false; + } + + // Board Rotation Quaternion X inline const float& getRotX() {return rotx;} bool setRotX(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { rotx = val; return true; } return false; } - // Board Rotation Y + // Board Rotation Quaternion Y inline const float& getRotY() {return roty;} bool setRotY(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { roty = val; return true; } return false; } - // Board Rotation Z + // Board Rotation Quaternion Z inline const float& getRotZ() {return rotz;} bool setRotZ(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { rotz = val; return true; } @@ -1042,6 +1052,7 @@ class BaseTrackerSettings { json["so21"] = so21; json["so22"] = so22; json["dismag"] = dismag; + json["rotw"] = rotw; json["rotx"] = rotx; json["roty"] = roty; json["rotz"] = rotz; @@ -1130,6 +1141,7 @@ class BaseTrackerSettings { v = json["so21"]; if(!v.isNull()) {setso21(v); chresetfusion = true;} v = json["so22"]; if(!v.isNull()) {setso22(v); chresetfusion = true;} v = json["dismag"]; if(!v.isNull()) {setDisMag(v);} + v = json["rotw"]; if(!v.isNull()) {setRotW(v); chresetfusion = true;} v = json["rotx"]; if(!v.isNull()) {setRotX(v); chresetfusion = true;} v = json["roty"]; if(!v.isNull()) {setRotY(v); chresetfusion = true;} v = json["rotz"]; if(!v.isNull()) {setRotZ(v); chresetfusion = true;} @@ -1542,9 +1554,10 @@ class BaseTrackerSettings { float so21 = 0; // Soft Iron Offset 21 float so22 = 1; // Soft Iron Offset 22 bool dismag = 1; // Disable Magnetometer - float rotx = 0; // Board Rotation X - float roty = 0; // Board Rotation Y - float rotz = 0; // Board Rotation Z + float rotw = 1; // Board Rotation Quaternion W + float rotx = 0; // Board Rotation Quaternion X + float roty = 0; // Board Rotation Quaternion Y + float rotz = 0; // Board Rotation Quaternion Z uint8_t uartmode = 0; // Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT) uint8_t crsftxrate = 140; // CRSF Transmit Frequncy uint8_t sbustxrate = 80; // SBUS Transmit Freqency diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index ce02b2eb..6355f69b 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -79,7 +79,7 @@ static float accx = 0, accy = 0, accz = 0; static float magx = 0, magy = 0, magz = 0; static float gyrx = 0, gyry = 0, gyrz = 0; static float tilt = 0, roll = 0, pan = 0; -static float rolloffset = 0, panoffset = 0, tiltoffset = 0; +static float panoffset = 0; static float magxoff = 0, magyoff = 0, magzoff = 0; static float accxoff = 0, accyoff = 0, acczoff = 0; static float gyrxoff = 0, gyryoff = 0, gyrzoff = 0; @@ -401,17 +401,16 @@ void calculate_Thread() // Zero button was pressed, adjust all values to zero if (wasButtonPressed()) { LOG_INF("Reset Center Short Pressed"); - rolloffset = roll; - panoffset = pan; - tiltoffset = tilt; + + //panoffset = pan; butdnw = true; } // Tilt output - tiltout = (tilt - tiltoffset) * trkset.getTlt_Gain() * (trkset.isTiltReversed() ? -1.0f : 1.0f); + tiltout = tilt * trkset.getTlt_Gain() * (trkset.isTiltReversed() ? -1.0f : 1.0f); // Roll output - rollout = (roll - rolloffset) * trkset.getRll_Gain() * (trkset.isRollReversed() ? -1.0f : 1.0f); + rollout = roll * trkset.getRll_Gain() * (trkset.isRollReversed() ? -1.0f : 1.0f); // Pan output, Normalize to +/- 180 Degrees panout = normalize((pan - panoffset), -180, 180) * trkset.getPan_Gain() * @@ -770,12 +769,18 @@ void calculate_Thread() trkset.setDataTilt(tilt); trkset.setDataRoll(roll); trkset.setDataPan(pan); + + // Qaternion Data + float qd[4]; + madgwick.getQuat(qd); + trkset.setDataQuat(qd); + k_mutex_unlock(&sensor_mutex); } - trkset.setDataTiltOff(tilt - tiltoffset); - trkset.setDataRollOff(roll - rolloffset); trkset.setDataPanOff(normalize(pan - panoffset, -180, 180)); + trkset.setDataTiltOff(tilt); + trkset.setDataRollOff(roll); trkset.setDataTiltOut(tiltout_ui); trkset.setDataRollOut(rollout_ui); @@ -790,10 +795,6 @@ void calculate_Thread() trkset.setDataTrpEnabled(trpOutputEnabled); trkset.setDataGyroCal(gyroCalibrated); - // Qauterion Data - float *qd = madgwick.getQuat(); - trkset.setDataQuat(qd); - // Bluetooth connected trkset.setDataBtCon(bleconnected); k_mutex_unlock(&data_mutex); @@ -828,6 +829,7 @@ void calculate_Thread() void sensor_Thread() { + static bool resetfus=true; LOG_INF("Sensor Thread Loaded"); while (1) { // Do not execute below until after initialization has happened @@ -838,6 +840,12 @@ void sensor_Thread() continue; } + // Reset fustion algorithm on startup + if(resetfus) { + reset_fusion(); + resetfus = false; + } + senseUsDuration = micros64(); #if defined(HAS_APDS9960) @@ -873,9 +881,6 @@ void sensor_Thread() } #endif - // Setup Rotations - float rotation[3] = {trkset.getRotX(), trkset.getRotY(), trkset.getRotZ()}; - // Read the data from the sensors float tacc[3] = {0.0f,0.0f,0.0f}, tgyr[3] = {0.0f,0.0f,0.0f}, tmag[3] = {0.0f,0.0f,0.0f}; bool accValid = false; @@ -1016,13 +1021,6 @@ void sensor_Thread() accy = raccy - accyoff; accz = raccz - acczoff; - // Apply Rotation - float tmpacc[3] = {accx, accy, accz}; - rotate(tmpacc, rotation); - accx = tmpacc[0]; - accy = tmpacc[1]; - accz = tmpacc[2]; - // For intial orientation setup madgsensbits |= MADGINIT_ACCEL; } @@ -1039,13 +1037,6 @@ void sensor_Thread() gyrx = rgyrx - gyrxoff; gyry = rgyry - gyryoff; gyrz = rgyrz - gyrzoff; - - // Apply Rotation - float tmpgyr[3] = {gyrx, gyry, gyrz}; - rotate(tmpgyr, rotation); - gyrx = tmpgyr[0]; - gyry = tmpgyr[1]; - gyrz = tmpgyr[2]; } if (!trkset.getDisMag()) { @@ -1069,13 +1060,6 @@ void sensor_Thread() magy = (magx * magsioff[3]) + (magy * magsioff[4]) + (magz * magsioff[5]); magz = (magx * magsioff[6]) + (magy * magsioff[7]) + (magz * magsioff[8]); - // Apply Rotation - float tmpmag[3] = {magx, magy, magz}; - rotate(tmpmag, rotation); - magx = tmpmag[0]; - magy = tmpmag[1]; - magz = tmpmag[2]; - // For inital orientation setup madgsensbits |= MADGINIT_MAG; } @@ -1147,7 +1131,7 @@ void sensor_Thread() pan = madgwick.getYaw(); if (firstrun && pan != 0) { - panoffset = pan; +// panoffset = pan; firstrun = false; } } @@ -1298,38 +1282,6 @@ float normalize(const float value, const float start, const float end) // + start to reset back to start of original range } -// Rotate, in Order X -> Y -> Z - -void rotate(float pn[3], const float rotation[3]) -{ - float rot[3] = {0, 0, 0}; - float out[3] = {0, 0, 0}; - std::copy(rotation, rotation + 3, rot); - - // Passed in Degrees - rot[0] *= DEG_TO_RAD; - rot[1] *= DEG_TO_RAD; - rot[2] *= DEG_TO_RAD; - - // X Rotation - out[0] = pn[0] * 1 + pn[1] * 0 + pn[2] * 0; - out[1] = pn[0] * 0 + pn[1] * cosf(rot[0]) - pn[2] * sinf(rot[0]); - out[2] = pn[0] * 0 + pn[1] * sinf(rot[0]) + pn[2] * cosf(rot[0]); - std::copy(out, out + 3, pn); - - // Y Rotation - out[0] = pn[0] * cosf(rot[1]) - pn[1] * 0 + pn[2] * sinf(rot[1]); - out[1] = pn[0] * 0 + pn[1] * 1 + pn[2] * 0; - out[2] = -pn[0] * sinf(rot[1]) + pn[1] * 0 + pn[2] * cosf(rot[1]); - std::copy(out, out + 3, pn); - - // Z Rotation - out[0] = pn[0] * cosf(rot[2]) - pn[1] * sinf(rot[2]) + pn[2] * 0.0f; - out[1] = pn[0] * sinf(rot[2]) + pn[1] * cosf(rot[2]) + pn[2] * 0.0f; - out[2] = pn[0] * 0.0f + pn[1] * 0.0f + pn[2] * 1.0f; - std::copy(out, out + 3, pn); -} - /* reset_fusion() * Causes the madgwick filter to reset. Used when board rotation changes */ @@ -1340,6 +1292,8 @@ void reset_fusion() madgreads = 0; madgsensbits = 0; firstrun = true; + madgwick.setRotQuat(trkset.getRotW(), trkset.getRotX(), trkset.getRotY(), + trkset.getRotZ()); // Set the rotation quaternion aacc[0] = 0; aacc[1] = 0; aacc[2] = 0; diff --git a/gui/src/basetrackersettings.h b/gui/src/basetrackersettings.h index d5d128f0..5b42779d 100644 --- a/gui/src/basetrackersettings.h +++ b/gui/src/basetrackersettings.h @@ -134,6 +134,7 @@ class BaseTrackerSettings : public QObject _setting["so21"] = 0; _setting["so22"] = 1; _setting["dismag"] = 1; + _setting["rotw"] = 1; _setting["rotx"] = 0; _setting["roty"] = 0; _setting["rotz"] = 0; @@ -248,9 +249,10 @@ class BaseTrackerSettings : public QObject descriptions["so21"] = tr("Soft Iron Offset 21"); descriptions["so22"] = tr("Soft Iron Offset 22"); descriptions["dismag"] = tr("Disable Magnetometer"); - descriptions["rotx"] = tr("Board Rotation X"); - descriptions["roty"] = tr("Board Rotation Y"); - descriptions["rotz"] = tr("Board Rotation Z"); + descriptions["rotw"] = tr("Board Rotation Quaternion W"); + descriptions["rotx"] = tr("Board Rotation Quaternion X"); + descriptions["roty"] = tr("Board Rotation Quaternion Y"); + descriptions["rotz"] = tr("Board Rotation Quaternion Z"); descriptions["uartmode"] = tr("Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT)"); descriptions["crsftxrate"] = tr("CRSF Transmit Frequncy"); descriptions["sbustxrate"] = tr("SBUS Transmit Freqency"); @@ -1049,12 +1051,25 @@ class BaseTrackerSettings : public QObject bool getDisMag() {return _setting["dismag"].toBool();} void setDisMag(bool val=1) { _setting["dismag"] = val; } - // Board Rotation X + // Board Rotation Quaternion W + float getRotW() { + return _setting["rotw"].toFloat(); + } + bool setRotW(float val=1) { + if(val >= -1 && val <= 1) { + _setting["rotw"] = QString::number(val,'g',4); + return true; + } + return false; + } + + + // Board Rotation Quaternion X float getRotX() { return _setting["rotx"].toFloat(); } bool setRotX(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { _setting["rotx"] = QString::number(val,'g',4); return true; } @@ -1062,12 +1077,12 @@ class BaseTrackerSettings : public QObject } - // Board Rotation Y + // Board Rotation Quaternion Y float getRotY() { return _setting["roty"].toFloat(); } bool setRotY(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { _setting["roty"] = QString::number(val,'g',4); return true; } @@ -1075,12 +1090,12 @@ class BaseTrackerSettings : public QObject } - // Board Rotation Z + // Board Rotation Quaternion Z float getRotZ() { return _setting["rotz"].toFloat(); } bool setRotZ(float val=0) { - if(val >= -360 && val <= 360) { + if(val >= -1 && val <= 1) { _setting["rotz"] = QString::number(val,'g',4); return true; } diff --git a/gui/src/mainwindow.cpp b/gui/src/mainwindow.cpp index 85bb24a4..d6bfb4db 100644 --- a/gui/src/mainwindow.cpp +++ b/gui/src/mainwindow.cpp @@ -3,6 +3,8 @@ #include #include #include +#include +#include #include "mainwindow.h" #include "ui_mainwindow.h" @@ -100,6 +102,8 @@ MainWindow::MainWindow(QWidget *parent) connect(ui->cmdReboot, &QPushButton::clicked,this, &MainWindow::reboot); connect(ui->cmdChannelViewer, &QPushButton::clicked, ui->actionChannel_Viewer, &QAction::trigger); connect(ui->cmdRefresh,&QPushButton::clicked,this,&MainWindow::findSerialPorts); + connect(ui->cmdSetOrientation,&QPushButton::clicked,this,&MainWindow::setOrientationClicked); + connect(ui->cmdSetForward,&QPushButton::clicked,this,&MainWindow::setForwardClicked); // Check Boxes connect(ui->chkpanrev, &QPushButton::clicked, this, &MainWindow::updateFromUI); @@ -129,9 +133,6 @@ MainWindow::MainWindow(QWidget *parent) connect(ui->spnA2Off, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnA3Gain, &QDoubleSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnA3Off, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); - connect(ui->spnRotX, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); - connect(ui->spnRotY, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); - connect(ui->spnRotZ, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnSBUSRate, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnCRSFRate, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnRstDblTapMax, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); @@ -162,6 +163,8 @@ MainWindow::MainWindow(QWidget *parent) connect(&trkset,&TrackerSettings::offOrientChanged,this,&MainWindow::offOrientChanged); connect(&trkset,&TrackerSettings::ppmOutChanged,this,&MainWindow::ppmOutChanged); connect(&trkset,&TrackerSettings::liveDataChanged,this,&MainWindow::liveDataChanged); + connect(&trkset,&TrackerSettings::calAccelChanged ,this,&MainWindow::calAccelChanged); + connect(&trkset,&TrackerSettings::quaternionChanged ,this,&MainWindow::quaternionChanged); connect(&trkset,&TrackerSettings::bleAddressDiscovered,this,&MainWindow::bleAddressDiscovered); connect(ui->cmbpanchn, &QComboBox::currentIndexChanged, this, &MainWindow::updateFromUI); @@ -310,6 +313,8 @@ void MainWindow::serialDisconnect() boardDiscover = false; boardDiscoveryStarted = false; + setOrientationStart = false; + setForwardStart = false; jsonht->disconnected(); statusMessage(tr("Disconnected")); @@ -328,6 +333,7 @@ void MainWindow::serialDisconnect() ui->servoTilt->setShowActualPosition(false); ui->servoRoll->setShowActualPosition(false); ui->actionEraseFlash->setEnabled(false); + ui->cmdSetForward->setEnabled(false); sending = false; connectTimer.stop(); @@ -638,11 +644,6 @@ void MainWindow::updateToUI() ui->cmbPWM3->setCurrentIndex(pwm3Ch==-1?0:pwm3Ch); ui->cmbBtMode->setCurrentIndex(trkset.getBtMode()); - int rot[3]; - trkset.orientation(rot[0],rot[1],rot[2]); - ui->spnRotX->setValue(rot[0]); - ui->spnRotY->setValue(rot[1]); - ui->spnRotZ->setValue(rot[2]); ui->til_gain->setValue(trkset.getTlt_Gain()*10); ui->pan_gain->setValue(trkset.getPan_Gain()*10); @@ -829,10 +830,6 @@ void MainWindow::updateFromUI() trkset.setBtPairedAddress(ui->cmbBTRmtMode->currentText().simplified()); } - trkset.setOrientation(ui->spnRotX->value(), - ui->spnRotY->value(), - ui->spnRotZ->value()); - trkset.setPpmOutInvert(ui->chkInvertedPPM->isChecked()); trkset.setPpmInInvert(ui->chkInvertedPPMIn->isChecked()); trkset.setRstOnWave(ui->chkResetCenterWave->isChecked()); @@ -1217,7 +1214,106 @@ void MainWindow::openGitHub() void MainWindow::showPinView() { - imageViewer->show(); + imageViewer->show(); +} + +void MainWindow::setOrientationClicked() +{ + // Save a list if the currently sending data items + // to be restored on calibration completed/canceled + cursendingdataitems = trkset.getDataItems(); + QMap dat = cursendingdataitems; + + QMapIterator i(dat); + while (i.hasNext()) { + i.next(); + dat[i.key()] = false; + } + dat["off_accx"] = true; + dat["off_accy"] = true; + dat["off_accz"] = true; + trkset.setDataItemSend(dat); + setOrientationStart = true; +} + +void MainWindow::setForwardClicked() +{ + // Save a list if the currently sending data items + // to be restored on calibration completed/canceled + cursendingdataitems = trkset.getDataItems(); + QMap dat = cursendingdataitems; + + QMapIterator i(dat); + while (i.hasNext()) { + i.next(); + dat[i.key()] = false; + } + dat["quat"] = true; + trkset.setDataItemSend(dat); + setForwardStart = true; +} + +void MainWindow::calAccelChanged(float x, float y, float z) +{ + if(setOrientationStart) { + QVector3D accv(x,y,z); + QVector3D desv(0,0,1); + accv.normalize(); + QQuaternion rot = QQuaternion::rotationTo(accv,desv); + rot.normalize(); + + cursendingdataitems["off_accx"] = false; + cursendingdataitems["off_accy"] = false; + cursendingdataitems["off_accz"] = false; + trkset.setDataItemSend(cursendingdataitems); + + trkset.setRotW(rot.scalar()); + trkset.setRotX(rot.x()); + trkset.setRotY(rot.y()); + trkset.setRotZ(rot.z()); + + ui->cmdSaveNVM->setEnabled(true); + ui->cmdSetForward->setEnabled(true); + // Use timer to prevent too many writes while drags, etc.. happen + saveToRAMTimer.start(500); + + setOrientationStart = false; + } +} + +void MainWindow::quaternionChanged(QQuaternion q) +{ + if(setForwardStart) { + QQuaternion rot(trkset.getRotW(), + trkset.getRotX(), + trkset.getRotY(), + trkset.getRotZ()); + + QQuaternion oprot; + QQuaternion curr = q; + q.normalize(); + + qreal x = asinf(-2.0f * (q.x() * q.z() - q.scalar() * q.y())); + qreal y = atan2f(q.scalar() * q.x() + q.y() * q.z(), 0.5f - q.x() * q.x() - q.y() * q.y()); + qreal magxy = sqrt(x*x + y*y); + qreal yaw = qRadiansToDegrees(qAtan2(y,x)); + + QQuaternion rotationCorrection = QQuaternion::fromAxisAndAngle(0, 0, 1, yaw); + + oprot = rot * rotationCorrection; + + trkset.setRotW(oprot.scalar()); + trkset.setRotX(oprot.x()); + trkset.setRotY(oprot.y()); + trkset.setRotZ(oprot.z()); + + cursendingdataitems["quat"] = false; + trkset.setDataItemSend(cursendingdataitems); + ui->cmdSaveNVM->setEnabled(true); + ui->cmdSetForward->setEnabled(false); + saveToRAMTimer.start(500); + setForwardStart = false; + } } void MainWindow::paramSendStart() diff --git a/gui/src/mainwindow.h b/gui/src/mainwindow.h index 949da706..d0b93587 100644 --- a/gui/src/mainwindow.h +++ b/gui/src/mainwindow.h @@ -75,11 +75,16 @@ class MainWindow : public QMainWindow QTimer connectTimer; QTimer saveToRAMTimer; QTimer requestParamsTimer; + bool waitingOnParameters; bool waitingOnFeatures; bool boardDiscover; bool boardDiscoveryStarted; + QMap cursendingdataitems; + bool setOrientationStart; + bool setForwardStart; + QString logd; FirmwareWizard *firmwareWizard; DiagnosticDisplay *diagnostic; @@ -142,7 +147,10 @@ private slots: void openDonate(); void openGitHub(); void showPinView(); - + void setOrientationClicked(); + void setForwardClicked(); + void calAccelChanged(float x, float y, float z); + void quaternionChanged(QQuaternion q); // Board Connections void paramSendStart(); diff --git a/gui/src/mainwindow.ui b/gui/src/mainwindow.ui index 1d344dfd..b6fd8d4f 100644 --- a/gui/src/mainwindow.ui +++ b/gui/src/mainwindow.ui @@ -6,8 +6,8 @@ 0 0 - 601 - 751 + 621 + 817 @@ -336,7 +336,7 @@ false - 3 + 0 @@ -518,82 +518,56 @@ - - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> - - - Tilt (X) - - - - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> - - - ° - - - -360 - - - 360 - - - - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> - - - ° - - - -360 + + + + 0 + 0 + + + + <html><head/><body><p><span style=" font-size:8pt;">1) Mount your board on your headset in any position.<br/>2) Holding it level and as still as possible. Press </span><span style=" font-size:8pt; font-weight:700;">Set Orientation</span><span style=" font-size:8pt;">. This will set tilt and roll parallel to earth.<br/>3) Look straight down at least 20degrees then press </span><span style=" font-size:8pt; font-weight:700;">Set Forward</span><span style=" font-size:8pt;"> button. This will set which way is looking forward (Tilt).</span></p></body></html> + + + true + + + true + + + + + + + + + Qt::Horizontal - - 360 + + + 40 + 20 + - + - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> + + + + + 0 + 20 + - Pan (Z) + Set Orientation - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> - - - ° - - - -360 - - - 360 - - - - - - - <html><head/><body><p>Please see the video online <span style=" font-weight:700;">Help-&gt;Online Help -&gt; GUI Setup -&gt; General</span> for an explanation</p></body></html> - + + - Roll (Y) + Set Forward @@ -609,8 +583,8 @@ - 20 - 40 + 0 + 0 @@ -3873,7 +3847,7 @@ 0 0 - 601 + 621 22 @@ -4035,9 +4009,6 @@ cmdRefresh cmdConnect cmdReboot - spnRotX - spnRotY - spnRotZ spnPPMFrameLen cmbBtMode cmbBTRmtMode diff --git a/gui/src/trackersettings.cpp b/gui/src/trackersettings.cpp index c692e672..084a4608 100644 --- a/gui/src/trackersettings.cpp +++ b/gui/src/trackersettings.cpp @@ -47,21 +47,6 @@ bool TrackerSettings::isPanReversed() return (_setting["servoreverse"].toUInt() & PAN_REVERSE_BIT); } -void TrackerSettings::orientation(int &x,int &y,int &z) -{ - x = _setting["rotx"].toInt(); - y = _setting["roty"].toInt(); - z = _setting["rotz"].toInt(); - return; -} - -void TrackerSettings::setOrientation(int x,int y,int z) -{ - _setting["rotx"] = x; - _setting["roty"] = y; - _setting["rotz"] = z; -} - void TrackerSettings::gyroOffset(float &x, float &y, float &z) { x=_setting["gyrxoff"].toFloat(); @@ -215,7 +200,7 @@ void TrackerSettings::setLiveDataMap(const QVariantMap &datalist, bool reset) _data[ll.key()] = ll.value(); // Emit if a value has been updated - bool ge=false,ae=false,me=false,oe=false,ooe=false,ppm=false; + bool ge=false,ae=false,me=false,oe=false,ooe=false,ppm=false,ofa=false,qu=false; QMapIterator i(datalist); while (i.hasNext()) { i.next(); @@ -246,16 +231,29 @@ void TrackerSettings::setLiveDataMap(const QVariantMap &datalist, bool reset) } if((key == "tiltoff" || key == "rolloff" || key == "panoff") && !ooe) { emit(offOrientChanged(_data["tiltoff"].toFloat(), - _data["rolloff"].toFloat(), - _data["panoff"].toFloat())); + _data["rolloff"].toFloat(), + _data["panoff"].toFloat())); ooe = true; } + if((key == "off_accx" || key == "off_accy" || key == "off_accz") && !ofa) { + emit(calAccelChanged(_data["off_accx"].toFloat(), + _data["off_accy"].toFloat(), + _data["off_accz"].toFloat())); + ofa = true; + } if((key == "panout" || key == "tiltout" || key == "rollout") && !ppm) { emit(ppmOutChanged(_data["tiltout"].toUInt(), _data["rollout"].toUInt(), _data["panout"].toUInt())); ppm = true; } + if((key == "quat[3]") && !qu) { + emit(quaternionChanged(QQuaternion(_data["quat[0]"].toFloat(), + _data["quat[1]"].toFloat(), + _data["quat[2]"].toFloat(), + _data["quat[3]"].toFloat()))); + qu = true; + } if(key == "btrmt") { if(_data["btrmt"] != "") emit(bleAddressDiscovered(_data["btrmt"].toString())); diff --git a/gui/src/trackersettings.h b/gui/src/trackersettings.h index c0bbdee0..c2ee68f7 100644 --- a/gui/src/trackersettings.h +++ b/gui/src/trackersettings.h @@ -2,6 +2,7 @@ #define TRACKERSETTINGS_H #include +#include #include #include "basetrackersettings.h" @@ -92,9 +93,11 @@ class TrackerSettings : public BaseTrackerSettings signals: void rawGyroChanged(float x, float y, float z); void rawAccelChanged(float x, float y, float z); + void calAccelChanged(float x, float y, float z); void rawMagChanged(float x, float y, float z); void rawOrientChanged(float t, float r, float p); void offOrientChanged(float t, float r, float p); + void quaternionChanged(QQuaternion qu); void bleAddressDiscovered(QString); void ppmOutChanged(int t, int r, int p); void liveDataChanged(); diff --git a/settings/settings.csv b/settings/settings.csv index 07587880..7e7c8a6b 100644 --- a/settings/settings.csv +++ b/settings/settings.csv @@ -152,10 +152,11 @@ float,Setting,so22,1,FLOAT_MIN,FLOAT_MAX,Soft Iron Offset 22,resetFusion,,4, ,,,,,,,,,, bool,Setting,DisMag,1,,,Disable Magnetometer,,,,F117 ,,,,,,,,,, -Board Rotation Settings,,,,,,,,,, -float,Setting,RotX,0,-360,360,Board Rotation X,resetFusion,,4, -float,Setting,RotY,0,-360,360,Board Rotation Y,resetFusion,,4, -float,Setting,RotZ,0,-360,360,Board Rotation Z,resetFusion,,4, +Board Rotation Quaternion,,,,,,,,,, +float,Setting,RotW,1,-1,1,Board Rotation Quaternion W,resetFusion,,4, +float,Setting,RotX,0,-1,1,Board Rotation Quaternion X,resetFusion,,4, +float,Setting,RotY,0,-1,1,Board Rotation Quaternion Y,resetFusion,,4, +float,Setting,RotZ,0,-1,1,Board Rotation Quaternion Z,resetFusion,,4, ,,,,,,,,,, Serial Settings,,,,,,,,,, u8,Setting,UartMode,0,0,3,"Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT)",,,, From 6a0d8f5f6bdbc93d01b6d99c58291961decd4307 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sun, 6 Apr 2025 11:36:25 -0700 Subject: [PATCH 2/2] setForward direction works --- firmware/src/src/sense.cpp | 4 ++-- gui/src/mainwindow.cpp | 44 ++++++++++++++++++++------------------ 2 files changed, 25 insertions(+), 23 deletions(-) diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index 6355f69b..e645e206 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -402,7 +402,7 @@ void calculate_Thread() if (wasButtonPressed()) { LOG_INF("Reset Center Short Pressed"); - //panoffset = pan; + panoffset = pan; butdnw = true; } @@ -1131,7 +1131,7 @@ void sensor_Thread() pan = madgwick.getYaw(); if (firstrun && pan != 0) { -// panoffset = pan; + panoffset = pan; firstrun = false; } } diff --git a/gui/src/mainwindow.cpp b/gui/src/mainwindow.cpp index d6bfb4db..ac6ba980 100644 --- a/gui/src/mainwindow.cpp +++ b/gui/src/mainwindow.cpp @@ -1256,6 +1256,7 @@ void MainWindow::setForwardClicked() void MainWindow::calAccelChanged(float x, float y, float z) { if(setOrientationStart) { + setOrientationStart = false; QVector3D accv(x,y,z); QVector3D desv(0,0,1); accv.normalize(); @@ -1276,43 +1277,44 @@ void MainWindow::calAccelChanged(float x, float y, float z) ui->cmdSetForward->setEnabled(true); // Use timer to prevent too many writes while drags, etc.. happen saveToRAMTimer.start(500); - - setOrientationStart = false; } } void MainWindow::quaternionChanged(QQuaternion q) { if(setForwardStart) { + setForwardStart = false; QQuaternion rot(trkset.getRotW(), trkset.getRotX(), trkset.getRotY(), trkset.getRotZ()); - QQuaternion oprot; - QQuaternion curr = q; q.normalize(); - qreal x = asinf(-2.0f * (q.x() * q.z() - q.scalar() * q.y())); - qreal y = atan2f(q.scalar() * q.x() + q.y() * q.z(), 0.5f - q.x() * q.x() - q.y() * q.y()); - qreal magxy = sqrt(x*x + y*y); - qreal yaw = qRadiansToDegrees(qAtan2(y,x)); - - QQuaternion rotationCorrection = QQuaternion::fromAxisAndAngle(0, 0, 1, yaw); - - oprot = rot * rotationCorrection; + qreal y = -1.0 * asinf(-2.0f * (q.x() * q.z() - q.scalar() * q.y())); + qreal x = -1.0 * atan2f(q.scalar() * q.x() + q.y() * q.z(), 0.5f - q.x() * q.x() - q.y() * q.y()); - trkset.setRotW(oprot.scalar()); - trkset.setRotX(oprot.x()); - trkset.setRotY(oprot.y()); - trkset.setRotZ(oprot.z()); + qreal xd = qRadiansToDegrees(x); + qreal yd = qRadiansToDegrees(y); + qreal yaw = qRadiansToDegrees(qAtan2(y,x)); + qreal magxy = sqrt(xd*xd + yd*yd); + if(magxy < 20) { + QMessageBox::information(this, "Error","Unable to determine forward direction\nNot enough tilt"); + } else { + QQuaternion rotationCorrection = QQuaternion::fromAxisAndAngle(0, 0, 1, yaw); + QQuaternion oprot = rotationCorrection * rot; + + trkset.setRotW(oprot.scalar()); + trkset.setRotX(oprot.x()); + trkset.setRotY(oprot.y()); + trkset.setRotZ(oprot.z()); + ui->cmdSaveNVM->setEnabled(true); + saveToRAMTimer.start(500); + ui->cmdSetForward->setEnabled(false); + } cursendingdataitems["quat"] = false; - trkset.setDataItemSend(cursendingdataitems); - ui->cmdSaveNVM->setEnabled(true); - ui->cmdSetForward->setEnabled(false); - saveToRAMTimer.start(500); - setForwardStart = false; + trkset.setDataItemSend(cursendingdataitems); } }