Skip to content

Commit

Permalink
feat: tilt comp. using dmpQuat & fusedQuat gen.
Browse files Browse the repository at this point in the history
  • Loading branch information
jeki committed May 7, 2021
1 parent af9a153 commit cb95470
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 31 deletions.
2 changes: 2 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,5 @@
.vscode/arduino.json
.vscode/c_cpp_properties.json
.vscode/arduino.json
data/._.DS_Store
data/.DS_Store
8 changes: 4 additions & 4 deletions data/index.html
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,10 +66,10 @@ <h1 id="loaderTxt">Loading...</h1>
<tr><td>MAGNETOMETER</td><td></td><td></td><td></td><td></td></tr>
<tr>
<td>Mag Values</td>
<td><input type="text" name="mvx" id="mvx" value="0"></td>
<td><input type="text" name="mvy" id="mvy" value="0"></td>
<td><input type="text" name="mvz" id="mvz" value="0"></td>
<td><button onclick="ReadMagParam()">Read Mag Param</button></td>
<td><input type="text" name="mx" id="mx" value="0"></td>
<td><input type="text" name="my" id="my" value="0"></td>
<td><input type="text" name="mz" id="mz" value="0"></td>
<td><button onclick="ReadMagParam()">Read Param</button></td>
</tr>
<tr>
<td>Mag Bias</td>
Expand Down
14 changes: 9 additions & 5 deletions data/ssehandler.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ function setMyJson(_val) {

function setMyJsonRadToDeg(_val) {
if(chkMyJSON(_val)) {
getID(_val).value = myJSON[_val] * 180 / Math.PI;
getID(_val).value = (myJSON[_val]*180/Math.PI).toFixed(4);
return true;
}
return false;
Expand Down Expand Up @@ -110,20 +110,20 @@ function got_packet(msgdata) {
setMyJson("latitude"); setMyJson("longitude");

if(chkMyJSON("h")) {
getID("heading").value = myJSON.h
document.getElementById("compass-img").style.transform = 'rotate(' + (myJSON.h - 360 ) + 'deg)';
getID("heading").value = (myJSON.h*180/Math.PI).toFixed(2);
document.getElementById("compass-img").style.transform = 'rotate(' + ((myJSON.h*180/Math.PI) - 360 ) + 'deg)';
}

//magnetometer values
setMyJson("mvx"); setMyJson("mvy"); setMyJson("mvz");
setMyJson("mx"); setMyJson("my"); setMyJson("mz"); setMyJsonRadToDeg("mh");

//magnetometer calibration
setMyJson("mbx"); setMyJson("mby"); setMyJson("mbz");
setMyJson("msx"); setMyJson("msy"); setMyJson("msz");

if(chkMyJSON("mcx")) {
if(chkMyJSON("mci")) {
addpoints(myJSON.mvx/3, myJSON.mvy/3, myJSON.mvz/3, myJSON.mcx);
addpoints(myJSON.mx/3, myJSON.my/3, myJSON.mz/3, myJSON.mcx);
document.getElementById("stat").value = "calibno:" + myJSON.mci + "/" + myJSON.mcx;
if(myJSON.mci >= myJSON.mcx - 2) calibmag = false;
}
Expand Down Expand Up @@ -187,6 +187,10 @@ function MagCalibrate() {
httpGetAsync(getID("windowUrl").value + "calib", MagCalibrateReqCallback);
}

function SetNorth() {
httpGetAsync(getID("windowUrl").value + "setNorth", MagCalibrateReqCallback);
}

function HideInfo() {
getID("tableInfo").hidden = true;
}
Expand Down
4 changes: 2 additions & 2 deletions data/webglhandler.js
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,8 @@ function getRotation() {
if(object != undefined) {
currquat.w = document.getElementById("qw").value;
currquat.x = document.getElementById("qy").value * -1;
currquat.y = document.getElementById("qz").value;
currquat.z = document.getElementById("qx").value * -1;
currquat.y = document.getElementById("qz").value * -1;
currquat.z = document.getElementById("qx").value;
currquat.normalize();
oquat.w = document.getElementById("qrw").value;
oquat.x = document.getElementById("qrx").value;
Expand Down
89 changes: 69 additions & 20 deletions imuUtils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,15 @@ extern "C" {
}

MPU9250_DMP imu;
static char imuTxtBuffer[140];
static char imuTxtBuffer[200];

float qw,qx,qy,qz;
float ax,ay,az, mx, my, mz;
int yaw_mixing_factor = 0;
// float lastYaw, lastDMPYaw;
double magYaw;
vector3d_t dmpEuler;
quaternion_t fusedQuat;
float fHeading[2];

int intPin = 14;
Expand Down Expand Up @@ -45,8 +49,8 @@ void printIMUData(void)
{
if (millis() - printTime > printTimeLimit) {
if (print_flag) {
sprintf(imuTxtBuffer, "{\"qw\":%.4f,\"qx\":%.4f,\"qy\":%.4f,\"qz\":%.4f,\"r\":%.2f,\"p\":%.2f,\"y\":%.2f,\"ax\":%.4f,\"ay\":%.4f,\"az\":%.4f,\"h\":%.2f}",
qw, qx, qy, qz, (float)dmpEuler[VEC3_X], (float)dmpEuler[VEC3_Y], (float)dmpEuler[VEC3_Z], ax, ay, az, fHeading[0]);
sprintf(imuTxtBuffer, "{\"qw\":%.4f,\"qx\":%.4f,\"qy\":%.4f,\"qz\":%.4f,\"r\":%.2f,\"p\":%.2f,\"y\":%.2f,\"ax\":%.4f,\"ay\":%.4f,\"az\":%.4f,\"mx\":%.4f,\"my\":%.4f,\"mz\":%.4f,\"mh\":%.4f,\"h\":%.4f}",
(float)fusedQuat[QUAT_W], (float)fusedQuat[QUAT_X], (float)fusedQuat[QUAT_Y], (float)fusedQuat[QUAT_Z], (float)dmpEuler[VEC3_X], (float)dmpEuler[VEC3_Y], (float)dmpEuler[VEC3_Z], ax, ay, az, mx, my, mz, (float)magYaw, fHeading[0]);
SSEBroadcastTxt(imuTxtBuffer);
print_flag = false;
}
Expand Down Expand Up @@ -117,7 +121,7 @@ int magCal_nonblocking(float * dest1, float * dest2) {
if(magRate <= 10) magCalibDT = 135; // at 8 Hz ODR, new mag data is available every 125 ms
else magCalibDT = 12; // at 100 Hz ODR, new mag data is available every 10 ms

sprintf(imuTxtBuffer, "{\"mcx\":%lu,\"mci\":%lu,\"mvx\":%d,\"mvy\":%d,\"mvz\":\"%d\"}",
sprintf(imuTxtBuffer, "{\"mcx\":%lu,\"mci\":%lu,\"mx\":%d,\"my\":%d,\"mz\":\"%d\"}",
sample_count, magCalibIdx, mag_temp[0], mag_temp[1], mag_temp[2]);
SSEBroadcastTxt(imuTxtBuffer);

Expand Down Expand Up @@ -177,13 +181,13 @@ void imu_send_mag_calib(void) {

int imu_calcHeading(void) {
vector3d_t fusedEuler;
vector3d_t magEuler;
quaternion_t dmpQuat;
quaternion_t magQuat;
quaternion_t unfusedQuat;
float deltaDMPYaw;
float deltaMagYaw;
double magYaw;
float newYaw;
// float deltaDMPYaw;
// float deltaMagYaw;
// float newYaw;

// // heading in x direction
// fHeading[1] = imu.calcCompassHeadingTiltY(-imu.ay, imu.ax, imu.az, mx, -my, mz);
Expand All @@ -210,35 +214,80 @@ int imu_calcHeading(void) {
quaternionToEuler(dmpQuat, dmpEuler);

fusedEuler[VEC3_X] = dmpEuler[VEC3_X];
fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y];
fusedEuler[VEC3_Z] = 0;
fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y]; // dmp pitch is going down + but the eulerToQuaternion requires the pitch is up +
fusedEuler[VEC3_Z] = 0; //-atan2(mx, my); // this good with calibration but very prone to tilt

// X axis = dmp X axis = mag Y axis
// Y axis = -dmp Y axis = -mag X axis
// Z axis = -dmp Z axis = mag Z axis
eulerToQuaternion(fusedEuler, unfusedQuat);

// deltaDMPYaw = (float)-dmpEuler[VEC3_Z] + lastDMPYaw;
// lastDMPYaw = (float)dmpEuler[VEC3_Z];

magQuat[QUAT_W] = 0;
magQuat[QUAT_X] = (double)my;
magQuat[QUAT_Y] = -(double)mx;
magQuat[QUAT_Z] = (double)mz;

// quaternionNormalize(magQuat);
// quaternionToEuler(magQuat, magEuler);
tiltCompensate(magQuat, unfusedQuat);

// deltaDMPYaw = (float)-dmpEuler[VEC3_Z] + lastDMPYaw;
// lastDMPYaw = (float)dmpEuler[VEC3_Z];

magYaw = -atan2(magQuat[QUAT_Y], magQuat[QUAT_X]);
fHeading[1] = (float)magYaw * 180.0f / (float)PI;
// magYaw = fusedEuler[VEC3_Z];

if (magYaw != magYaw) {
Serial.println("magYaw NAN\n");
return -1;
}

if(fHeading[1] - fHeading[0] > 180.0f) {
fHeading[0] += 360;
if (magYaw < 0.0)
magYaw += (double)TWO_PI;
// else if(magYaw >= TWO_PI) {
// magYaw -= (double)TWO_PI;
// }

// newYaw = lastYaw + deltaDMPYaw;

// if (newYaw > TWO_PI)
// newYaw -= TWO_PI;
// else if (newYaw < 0.0f)
// newYaw += TWO_PI;

// deltaMagYaw = (float)magYaw - newYaw;

// if (deltaMagYaw >= (float)M_PI)
// deltaMagYaw -= TWO_PI;
// else if (deltaMagYaw < -(float)M_PI)
// deltaMagYaw += TWO_PI;

// if (yaw_mixing_factor > 0)
// newYaw += deltaMagYaw / yaw_mixing_factor;

// if (newYaw > TWO_PI)
// newYaw -= TWO_PI;
// else if (newYaw < 0.0f)
// newYaw += TWO_PI;

// lastYaw = newYaw;

// if (newYaw > (float)M_PI)
// newYaw -= TWO_PI;

fHeading[1] = (float)magYaw;
if(fHeading[1] - fHeading[0] > PI) {
fHeading[0] += TWO_PI;
} else {
if(fHeading[0] - fHeading[1] > 180.0f) {
fHeading[0] -= 360;
if(fHeading[0] - fHeading[1] > PI) {
fHeading[0] -= TWO_PI;
}
}

// add LPF for smoother result
fHeading[0] = (fHeading[0] * 14.0f + fHeading[1] * 2.0f) / 16.0f;

fusedEuler[VEC3_Z] = (double)fHeading[0];
eulerToQuaternion(fusedEuler, fusedQuat);

return 0;
}

Expand Down

0 comments on commit cb95470

Please sign in to comment.