Skip to content

Easy board rotation #204

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
dlktdr wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Easy board rotation #204

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
0703afc
Easy setup for board orientation
dlktdr Apr 6, 2025
6a0d8f5
setForward direction works
dlktdr Apr 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
64 changes: 49 additions & 15 deletions firmware/src/src/MadgwickAHRS/MadgwickAHRS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
}

Expand All @@ -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)
{
Expand Down Expand Up @@ -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;
}
27 changes: 24 additions & 3 deletions firmware/src/src/MadgwickAHRS/MadgwickAHRS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,13 +49,24 @@ 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
public:
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);
Expand Down Expand Up @@ -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()
Expand Down
31 changes: 22 additions & 9 deletions firmware/src/src/basetrackersettings.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;}
Expand Down Expand Up @@ -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
Expand Down
Loading
Loading