Skip to content

Commit

Permalink
feat: repair compass
Browse files Browse the repository at this point in the history
  • Loading branch information
jeki committed Mar 4, 2021
1 parent 4efea10 commit 7cae778
Show file tree
Hide file tree
Showing 21 changed files with 1,786 additions and 1,121 deletions.
37 changes: 37 additions & 0 deletions SparkFunMPU9250-DMP.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -621,6 +621,30 @@ float MPU9250_DMP::qToFloat(long number, unsigned char q)
return (number >> q) + ((number & mask) / (float) (2<<(q-1)));
}

float MPU9250_DMP::calcAzimuth(float Y_r, float X_r, float mag_x, float mag_y, float mag_z) {
float X_h, Y_h, azimuth;
float cosY_r, sinY_r,cosX_r,sinX_r;
/* Calculate Azimuth:
* Magnetic horizontal components, after compensating for Roll(r) and Pitch(p) are:
* X_h = X*cos(p) + Y*sin(r)*sin(p) + Z*cos(r)*sin(p)
* Y_h = Y*cos(r) - Z*sin(r)
* Azimuth = arcTan(Y_h/X_h)
*/
cosY_r = cos(Y_r);
sinY_r = sin(Y_r);
cosX_r = cos(X_r);
sinX_r = sin(X_r);

X_h = (double)mag_x*cosY_r + (double)mag_y*sinX_r*sinY_r + (double)mag_z*cosX_r*sinY_r;
Y_h = (double)mag_y*cosX_r - (double)mag_z*sinX_r;
azimuth = atan2(Y_h, X_h);

if(azimuth < 0) { /* Convert Azimuth in the range (0, 2pi) */
azimuth = 2*PI + azimuth;
}
return (float)azimuth;
}

void MPU9250_DMP::computeEulerAngles(bool degrees)
{
float dqw = qToFloat(qw, 30);
Expand Down Expand Up @@ -654,6 +678,19 @@ void MPU9250_DMP::computeEulerAngles(bool degrees)
}
}

float MPU9250_DMP::calcCompassHeadingTilt(float acc_x, float acc_y, float acc_z, float mag_x, float mag_y, float mag_z) {
float Y_r, X_r;

// pitch is along Y-Axis using XYZ rotation
Y_r = atan2((float)-acc_x, sqrt((long)acc_z*(long)acc_z + (long)acc_y*(long)acc_y));
// roll is along X-Axis
if(acc_z < 0) X_r = atan2((float)acc_y, -sqrt((long)acc_z*(long)acc_z + (long)acc_x*(long)acc_x));
else X_r = atan2((float)acc_y, sqrt((long)acc_z*(long)acc_z + (long)acc_x*(long)acc_x));
// // pitch = (double)_pitch; roll = (double)_roll;

return calcAzimuth(Y_r, X_r, mag_x, mag_y, mag_z);
}

float MPU9250_DMP::computeCompassHeading(void)
{
if (my == 0)
Expand Down
11 changes: 11 additions & 0 deletions SparkFunMPU9250-DMP.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,6 +364,17 @@ class MPU9250_DMP
// Output: class variables roll, pitch, and yaw will be updated on exit.
void computeEulerAngles(bool degrees = true);

// calcAzimuth -- Compute azimuth / heading with tilt compensation
// Input: pitch and roll in radian and 3-axis magnetometer values
// Output: azimuth / heading based on tilt compensation
float calcAzimuth(float Y_r, float X_r, float mag_x, float mag_y, float mag_z);

// calcCompassHeadingTilt -- Compute heading based on most recently read mx, my, and mz values
// with tilt compensation based on accelerometer values
// Input: float of 3-axis accelerometer and 3-axis magnetometer
// Output: class variable heading will be updated on exit
float calcCompassHeadingTilt(float acc_x, float acc_y, float acc_z, float mag_x, float mag_y, float mag_z);

// computeCompassHeading -- Compute heading based on most recently read mx, my, and mz values
// Output: class variable heading will be updated on exit
float computeCompassHeading(void);
Expand Down
Binary file added data/assets/compass_indicator.png
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
136 changes: 136 additions & 0 deletions data/assets/compass_rose.svg
View file
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
80 changes: 68 additions & 12 deletions data/index.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,18 +14,74 @@ <h1 id="loaderTxt">Loading...</h1>
</div>
<canvas id="c"></canvas>
<div id="info">
<h3>Url</h3>
<input id="windowUrl"></input><button onclick="SSESubscribe()">Subscribe</button>
<h3>IP Address</h3><input id="ip"></input>
<h3>Coordinate</h3>
<input id="latitude" value="invalid"></input><input id="longitude" value="invalid"></input>
<h3>IMU Quaternion</h3>
<input id="qw" value="1"></input><input id="qx" value="0"></input><input id="qy" value="0"></input><input id="qz" value="0"></input>
<p><input type="text" name="qrw" id="qrw" value="1"><input type="text" name="qrx" id="qrx" value="0"><input type="text" name="qry" id="qry" value="0"><input type="text" name="qrz" id="qrz" value="0"></p>
<h3>IMU RPY</h3>
<input id="roll" value="invalid"></input><input id="pitch" value="invalid"></input><input id="yaw" value="invalid"></input><input id="heading" value="invalid"></input>
<h3>IMU Accel</h3>
<input id="ax" value="invalid"></input><input id="ay" value="invalid"></input><input id="az" value="invalid"></input>
<table>
<tr>
<td>Url</td>
<td><input id="windowUrl"></input></td>
<td><button onclick="SSESubscribe()">Subscribe</button></td>
<td></td>
<td></td>
</tr>
<tr>
<td>IP Address</td>
<td><input id="ip"></input></td>
<td></td>
<td></td>
<td></td>
</tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr>
<td>Quaternion</td>
<td><input id="qw" value="1"></input></td>
<td><input id="qx" value="0"></input></td>
<td><input id="qy" value="0"></input></td>
<td><input id="qz" value="0"></input></td>
</tr>
<tr>
<td>R Quat</td>
<td><input id="qrw" value="1"></input></td>
<td><input id="qrx" value="0"></input></td>
<td><input id="qry" value="0"></input></td>
<td><input id="qrz" value="0"></input></td>
</tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr>
<td>RPY</td>
<td><input id="roll" value="invalid"></input></td>
<td><input id="pitch" value="invalid"></input></td>
<td><input id="yaw" value="invalid"></input></td>
<td></td>
</tr>
<tr>
<td>Accel</td>
<td><input id="ax" value="invalid"></input></td>
<td><input id="ay" value="invalid"></input></td>
<td><input id="az" value="invalid"></input></td>
<td></td>
</tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr><td></td><td></td><td></td><td></td><td></td></tr>
<tr>
<td>LatLong</td>
<td><input id="latitude" value="latitude"></input></td>
<td><input id="longitude" value="longitude"></input></td>
<td></td>
<td></td>
</tr>
<tr>
<td>Heading</td>
<td><input id="heading" value="invalid"></input></td>
<td></td>
<td></td>
<td></td>
</tr>
</table>
</div>
<div id="compass">
<img id="compass-img" src="/assets/compass_rose.svg" alt="Compass">
<img id="compass-ind" src="/assets/compass_indicator.png">
</div>
<div id="container">
</div>
Expand Down
90 changes: 51 additions & 39 deletions data/main.css
100644 → 100755
View file
Original file line number Diff line number Diff line change
@@ -1,40 +1,52 @@
#info {
position: absolute;
top: 0px;
padding: 1rem;
box-sizing: border-box;
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select: none;
user-select: none;
z-index: 1;
}

#loader {
position: absolute;
top: calc(50% - 120px);
left: calc(50% - 60px);
width: 100%;
height: 100%;
color: #0a3e61;
box-sizing: border-box;
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select: none;
user-select: none;
z-index: 2;
}

.loader {
border: 16px solid #f3f3f3; /* Light grey */
border-top: 16px solid #3498db; /* Blue */
border-radius: 50%;
width: 120px;
height: 120px;
animation: spin 2s linear infinite;
}

@keyframes spin {
0% { transform: rotate(0deg); }
100% { transform: rotate(360deg); }
#info {
position: absolute;
top: 0px;
padding: 1rem;
box-sizing: border-box;
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select: none;
user-select: none;
z-index: 1;
}
#compass-img{
width: 300px;
position: absolute;
left: 20px;
bottom: 20px;
}
#compass-ind{
width: 120px;
height: 200px;
position: absolute;
left: 110px;
bottom: 105px;
}
#loader {
position: absolute;
top: calc(50% - 120px);
left: calc(50% - 60px);
width: 100%;
height: 100%;
color: #0a3e61;
box-sizing: border-box;
-moz-user-select: none;
-webkit-user-select: none;
-ms-user-select: none;
user-select: none;
z-index: 2;
}

.loader {
border: 16px solid #f3f3f3; /* Light grey */
border-top: 16px solid #3498db; /* Blue */
border-radius: 50%;
width: 120px;
height: 120px;
animation: spin 2s linear infinite;
}

@keyframes spin {
0% { transform: rotate(0deg); }
100% { transform: rotate(360deg); }
}
4 changes: 3 additions & 1 deletion data/ssehandler.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,10 @@ var eventListen = null;
function getID(_str) {
return document.getElementById(_str);
}

function getIr() {
return irval;
}
}

function getRed() {
return redval;
Expand Down Expand Up @@ -121,6 +122,7 @@ function got_packet(msgdata) {
}
if(myJSON.h != undefined) {
getID("heading").value = myJSON.h
document.getElementById("compass-img").style.transform = 'rotate(' + (360 - myJSON.h) + 'deg)';
}
}
}
Expand Down
Loading

0 comments on commit 7cae778

Please sign in to comment.