bryan
/
BangleApps
0
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

magnav: Outsource tilt compensation to library

master
Erik Andresen 2023-01-22 11:01:44 +01:00
parent d612937bb7
commit fe491e2d09
1 changed files with 2 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
apps/magnav/magnav_b2.js
View File

@ -53,24 +53,7 @@ function newHeading(m,h){
var candraw = false;
var CALIBDATA = require("Storage").readJSON("magnav.json",1)||null;
function tiltfixread(O,S){
"ram"
var m = Bangle.getCompass();
var g = Bangle.getAccel();
m.dx =(m.x-O.x)*S.x; m.dy=(m.y-O.y)*S.y; m.dz=(m.z-O.z)*S.z;
var d = Math.atan2(-m.dx,m.dy)*180/Math.PI;
if (d<0) d+=360;
var phi = Math.atan(-g.x/-g.z);
var cosphi = Math.cos(phi), sinphi = Math.sin(phi);
var theta = Math.atan(-g.y/(-g.x*sinphi-g.z*cosphi));
var costheta = Math.cos(theta), sintheta = Math.sin(theta);
var xh = m.dy*costheta + m.dx*sinphi*sintheta + m.dz*cosphi*sintheta;
var yh = m.dz*sinphi - m.dx*cosphi;
var psi = Math.atan2(yh,xh)*180/Math.PI;
if (psi<0) psi+=360;
return psi;
}
const tiltfixread = require("magnav").tiltfixread;
// Note actual mag is 360-m, error in firmware
function reading() {
@ -94,30 +77,6 @@ function reading() {
g.flip();
}
function calibrate(){
var max={x:-32000, y:-32000, z:-32000},
min={x:32000, y:32000, z:32000};
var ref = setInterval(()=>{
var m = Bangle.getCompass();
max.x = m.x>max.x?m.x:max.x;
max.y = m.y>max.y?m.y:max.y;
max.z = m.z>max.z?m.z:max.z;
min.x = m.x<min.x?m.x:min.x;
min.y = m.y<min.y?m.y:min.y;
min.z = m.z<min.z?m.z:min.z;
}, 100);
return new Promise((resolve) => {
setTimeout(()=>{
if(ref) clearInterval(ref);
var offset = {x:(max.x+min.x)/2,y:(max.y+min.y)/2,z:(max.z+min.z)/2};
var delta = {x:(max.x-min.x)/2,y:(max.y-min.y)/2,z:(max.z-min.z)/2};
var avg = (delta.x+delta.y+delta.z)/3;
var scale = {x:avg/delta.x, y:avg/delta.y, z:avg/delta.z};
resolve({offset:offset,scale:scale});
},20000);
});
}
var calibrating=false;
function docalibrate(first){
calibrating=true;
@ -137,7 +96,7 @@ function docalibrate(first){
g.drawString("Fig 8s to",88,Ypos);
g.drawString("Calibrate",88,Ypos+18);
g.flip();
calibrate().then((r)=>{
require("magnav").calibrate().then((r)=>{
CALIBDATA=r;
require("Storage").write("magnav.json",r);
restart();