Skip to content

Commit 7cae778

Browse files
author
jeki
committed
feat: repair compass
1 parent 4efea10 commit 7cae778

21 files changed

+1786
-1121
lines changed

‎SparkFunMPU9250-DMP.cpp

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -621,6 +621,30 @@ float MPU9250_DMP::qToFloat(long number, unsigned char q)
621621
return (number >> q) + ((number & mask) / (float) (2<<(q-1)));
622622
}
623623

624+
float MPU9250_DMP::calcAzimuth(float Y_r, float X_r, float mag_x, float mag_y, float mag_z) {
625+
float X_h, Y_h, azimuth;
626+
float cosY_r, sinY_r,cosX_r,sinX_r;
627+
/* Calculate Azimuth:
628+
* Magnetic horizontal components, after compensating for Roll(r) and Pitch(p) are:
629+
* X_h = X*cos(p) + Y*sin(r)*sin(p) + Z*cos(r)*sin(p)
630+
* Y_h = Y*cos(r) - Z*sin(r)
631+
* Azimuth = arcTan(Y_h/X_h)
632+
*/
633+
cosY_r = cos(Y_r);
634+
sinY_r = sin(Y_r);
635+
cosX_r = cos(X_r);
636+
sinX_r = sin(X_r);
637+
638+
X_h = (double)mag_x*cosY_r + (double)mag_y*sinX_r*sinY_r + (double)mag_z*cosX_r*sinY_r;
639+
Y_h = (double)mag_y*cosX_r - (double)mag_z*sinX_r;
640+
azimuth = atan2(Y_h, X_h);
641+
642+
if(azimuth < 0) { /* Convert Azimuth in the range (0, 2pi) */
643+
azimuth = 2*PI + azimuth;
644+
}
645+
return (float)azimuth;
646+
}
647+
624648
void MPU9250_DMP::computeEulerAngles(bool degrees)
625649
{
626650
float dqw = qToFloat(qw, 30);
@@ -654,6 +678,19 @@ void MPU9250_DMP::computeEulerAngles(bool degrees)
654678
}
655679
}
656680

681+
float MPU9250_DMP::calcCompassHeadingTilt(float acc_x, float acc_y, float acc_z, float mag_x, float mag_y, float mag_z) {
682+
float Y_r, X_r;
683+
684+
// pitch is along Y-Axis using XYZ rotation
685+
Y_r = atan2((float)-acc_x, sqrt((long)acc_z*(long)acc_z + (long)acc_y*(long)acc_y));
686+
// roll is along X-Axis
687+
if(acc_z < 0) X_r = atan2((float)acc_y, -sqrt((long)acc_z*(long)acc_z + (long)acc_x*(long)acc_x));
688+
else X_r = atan2((float)acc_y, sqrt((long)acc_z*(long)acc_z + (long)acc_x*(long)acc_x));
689+
// // pitch = (double)_pitch; roll = (double)_roll;
690+
691+
return calcAzimuth(Y_r, X_r, mag_x, mag_y, mag_z);
692+
}
693+
657694
float MPU9250_DMP::computeCompassHeading(void)
658695
{
659696
if (my == 0)

‎SparkFunMPU9250-DMP.h

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -364,6 +364,17 @@ class MPU9250_DMP
364364
// Output: class variables roll, pitch, and yaw will be updated on exit.
365365
void computeEulerAngles(bool degrees = true);
366366

367+
// calcAzimuth -- Compute azimuth / heading with tilt compensation
368+
// Input: pitch and roll in radian and 3-axis magnetometer values
369+
// Output: azimuth / heading based on tilt compensation
370+
float calcAzimuth(float Y_r, float X_r, float mag_x, float mag_y, float mag_z);
371+
372+
// calcCompassHeadingTilt -- Compute heading based on most recently read mx, my, and mz values
373+
// with tilt compensation based on accelerometer values
374+
// Input: float of 3-axis accelerometer and 3-axis magnetometer
375+
// Output: class variable heading will be updated on exit
376+
float calcCompassHeadingTilt(float acc_x, float acc_y, float acc_z, float mag_x, float mag_y, float mag_z);
377+
367378
// computeCompassHeading -- Compute heading based on most recently read mx, my, and mz values
368379
// Output: class variable heading will be updated on exit
369380
float computeCompassHeading(void);

‎data/assets/compass_indicator.png

19 KB
Loading

‎data/assets/compass_rose.svg

Lines changed: 136 additions & 0 deletions
Loading

‎data/index.html

Lines changed: 68 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -14,18 +14,74 @@ <h1 id="loaderTxt">Loading...</h1>
1414
</div>
1515
<canvas id="c"></canvas>
1616
<div id="info">
17-
<h3>Url</h3>
18-
<input id="windowUrl"></input><button onclick="SSESubscribe()">Subscribe</button>
19-
<h3>IP Address</h3><input id="ip"></input>
20-
<h3>Coordinate</h3>
21-
<input id="latitude" value="invalid"></input><input id="longitude" value="invalid"></input>
22-
<h3>IMU Quaternion</h3>
23-
<input id="qw" value="1"></input><input id="qx" value="0"></input><input id="qy" value="0"></input><input id="qz" value="0"></input>
24-
<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>
25-
<h3>IMU RPY</h3>
26-
<input id="roll" value="invalid"></input><input id="pitch" value="invalid"></input><input id="yaw" value="invalid"></input><input id="heading" value="invalid"></input>
27-
<h3>IMU Accel</h3>
28-
<input id="ax" value="invalid"></input><input id="ay" value="invalid"></input><input id="az" value="invalid"></input>
17+
<table>
18+
<tr>
19+
<td>Url</td>
20+
<td><input id="windowUrl"></input></td>
21+
<td><button onclick="SSESubscribe()">Subscribe</button></td>
22+
<td></td>
23+
<td></td>
24+
</tr>
25+
<tr>
26+
<td>IP Address</td>
27+
<td><input id="ip"></input></td>
28+
<td></td>
29+
<td></td>
30+
<td></td>
31+
</tr>
32+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
33+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
34+
<tr>
35+
<td>Quaternion</td>
36+
<td><input id="qw" value="1"></input></td>
37+
<td><input id="qx" value="0"></input></td>
38+
<td><input id="qy" value="0"></input></td>
39+
<td><input id="qz" value="0"></input></td>
40+
</tr>
41+
<tr>
42+
<td>R Quat</td>
43+
<td><input id="qrw" value="1"></input></td>
44+
<td><input id="qrx" value="0"></input></td>
45+
<td><input id="qry" value="0"></input></td>
46+
<td><input id="qrz" value="0"></input></td>
47+
</tr>
48+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
49+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
50+
<tr>
51+
<td>RPY</td>
52+
<td><input id="roll" value="invalid"></input></td>
53+
<td><input id="pitch" value="invalid"></input></td>
54+
<td><input id="yaw" value="invalid"></input></td>
55+
<td></td>
56+
</tr>
57+
<tr>
58+
<td>Accel</td>
59+
<td><input id="ax" value="invalid"></input></td>
60+
<td><input id="ay" value="invalid"></input></td>
61+
<td><input id="az" value="invalid"></input></td>
62+
<td></td>
63+
</tr>
64+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
65+
<tr><td></td><td></td><td></td><td></td><td></td></tr>
66+
<tr>
67+
<td>LatLong</td>
68+
<td><input id="latitude" value="latitude"></input></td>
69+
<td><input id="longitude" value="longitude"></input></td>
70+
<td></td>
71+
<td></td>
72+
</tr>
73+
<tr>
74+
<td>Heading</td>
75+
<td><input id="heading" value="invalid"></input></td>
76+
<td></td>
77+
<td></td>
78+
<td></td>
79+
</tr>
80+
</table>
81+
</div>
82+
<div id="compass">
83+
<img id="compass-img" src="/assets/compass_rose.svg" alt="Compass">
84+
<img id="compass-ind" src="/assets/compass_indicator.png">
2985
</div>
3086
<div id="container">
3187
</div>

‎data/main.css

100644100755
Lines changed: 51 additions & 39 deletions
Original file line numberDiff line numberDiff line change
@@ -1,40 +1,52 @@
1-
#info {
2-
position: absolute;
3-
top: 0px;
4-
padding: 1rem;
5-
box-sizing: border-box;
6-
-moz-user-select: none;
7-
-webkit-user-select: none;
8-
-ms-user-select: none;
9-
user-select: none;
10-
z-index: 1;
11-
}
12-
13-
#loader {
14-
position: absolute;
15-
top: calc(50% - 120px);
16-
left: calc(50% - 60px);
17-
width: 100%;
18-
height: 100%;
19-
color: #0a3e61;
20-
box-sizing: border-box;
21-
-moz-user-select: none;
22-
-webkit-user-select: none;
23-
-ms-user-select: none;
24-
user-select: none;
25-
z-index: 2;
26-
}
27-
28-
.loader {
29-
border: 16px solid #f3f3f3; /* Light grey */
30-
border-top: 16px solid #3498db; /* Blue */
31-
border-radius: 50%;
32-
width: 120px;
33-
height: 120px;
34-
animation: spin 2s linear infinite;
35-
}
36-
37-
@keyframes spin {
38-
0% { transform: rotate(0deg); }
39-
100% { transform: rotate(360deg); }
1+
#info {
2+
position: absolute;
3+
top: 0px;
4+
padding: 1rem;
5+
box-sizing: border-box;
6+
-moz-user-select: none;
7+
-webkit-user-select: none;
8+
-ms-user-select: none;
9+
user-select: none;
10+
z-index: 1;
11+
}
12+
#compass-img{
13+
width: 300px;
14+
position: absolute;
15+
left: 20px;
16+
bottom: 20px;
17+
}
18+
#compass-ind{
19+
width: 120px;
20+
height: 200px;
21+
position: absolute;
22+
left: 110px;
23+
bottom: 105px;
24+
}
25+
#loader {
26+
position: absolute;
27+
top: calc(50% - 120px);
28+
left: calc(50% - 60px);
29+
width: 100%;
30+
height: 100%;
31+
color: #0a3e61;
32+
box-sizing: border-box;
33+
-moz-user-select: none;
34+
-webkit-user-select: none;
35+
-ms-user-select: none;
36+
user-select: none;
37+
z-index: 2;
38+
}
39+
40+
.loader {
41+
border: 16px solid #f3f3f3; /* Light grey */
42+
border-top: 16px solid #3498db; /* Blue */
43+
border-radius: 50%;
44+
width: 120px;
45+
height: 120px;
46+
animation: spin 2s linear infinite;
47+
}
48+
49+
@keyframes spin {
50+
0% { transform: rotate(0deg); }
51+
100% { transform: rotate(360deg); }
4052
}

‎data/ssehandler.js

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,10 @@ var eventListen = null;
77
function getID(_str) {
88
return document.getElementById(_str);
99
}
10+
1011
function getIr() {
1112
return irval;
12-
}
13+
}
1314

1415
function getRed() {
1516
return redval;
@@ -121,6 +122,7 @@ function got_packet(msgdata) {
121122
}
122123
if(myJSON.h != undefined) {
123124
getID("heading").value = myJSON.h
125+
document.getElementById("compass-img").style.transform = 'rotate(' + (360 - myJSON.h) + 'deg)';
124126
}
125127
}
126128
}

0 commit comments

Comments
 (0)