mirror of https://github.com/espruino/BangleApps
555 lines
15 KiB
JavaScript
555 lines
15 KiB
JavaScript
// Bike Speedometer by https://github.com/HilmarSt
|
|
// Big parts of this software are based on https://github.com/espruino/BangleApps/tree/master/apps/speedalt
|
|
// Compass and Compass Calibration based on https://github.com/espruino/BangleApps/tree/master/apps/magnav
|
|
|
|
const BANGLEJS2 = 1;
|
|
const screenH = g.getHeight();
|
|
const screenYstart = 24; // 0..23 for widgets
|
|
const screenY_Half = screenH / 2 + screenYstart;
|
|
const screenW = g.getWidth();
|
|
const screenW_Half = screenW / 2;
|
|
const fontFactorB2 = 2/3;
|
|
const colfg=g.theme.fg, colbg=g.theme.bg;
|
|
const col1=colfg, colUncertain="#88f"; // if (lf.fix) g.setColor(col1); else g.setColor(colUncertain);
|
|
|
|
var altiGPS=0, altiBaro=0;
|
|
var hdngGPS=0, hdngCompass=0, calibrateCompass=false;
|
|
|
|
/*kalmanjs, Wouter Bulten, MIT, https://github.com/wouterbulten/kalmanjs */
|
|
var KalmanFilter = (function () {
|
|
'use strict';
|
|
|
|
function _classCallCheck(instance, Constructor) {
|
|
if (!(instance instanceof Constructor)) {
|
|
throw new TypeError("Cannot call a class as a function");
|
|
}
|
|
}
|
|
|
|
function _defineProperties(target, props) {
|
|
for (var i = 0; i < props.length; i++) {
|
|
var descriptor = props[i];
|
|
descriptor.enumerable = descriptor.enumerable || false;
|
|
descriptor.configurable = true;
|
|
if ("value" in descriptor) descriptor.writable = true;
|
|
Object.defineProperty(target, descriptor.key, descriptor);
|
|
}
|
|
}
|
|
|
|
function _createClass(Constructor, protoProps, staticProps) {
|
|
if (protoProps) _defineProperties(Constructor.prototype, protoProps);
|
|
if (staticProps) _defineProperties(Constructor, staticProps);
|
|
return Constructor;
|
|
}
|
|
|
|
/**
|
|
* KalmanFilter
|
|
* @class
|
|
* @author Wouter Bulten
|
|
* @see {@link http://github.com/wouterbulten/kalmanjs}
|
|
* @version Version: 1.0.0-beta
|
|
* @copyright Copyright 2015-2018 Wouter Bulten
|
|
* @license MIT License
|
|
* @preserve
|
|
*/
|
|
var KalmanFilter =
|
|
/*#__PURE__*/
|
|
function () {
|
|
/**
|
|
* Create 1-dimensional kalman filter
|
|
* @param {Number} options.R Process noise
|
|
* @param {Number} options.Q Measurement noise
|
|
* @param {Number} options.A State vector
|
|
* @param {Number} options.B Control vector
|
|
* @param {Number} options.C Measurement vector
|
|
* @return {KalmanFilter}
|
|
*/
|
|
function KalmanFilter() {
|
|
var _ref = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : {},
|
|
_ref$R = _ref.R,
|
|
R = _ref$R === void 0 ? 1 : _ref$R,
|
|
_ref$Q = _ref.Q,
|
|
Q = _ref$Q === void 0 ? 1 : _ref$Q,
|
|
_ref$A = _ref.A,
|
|
A = _ref$A === void 0 ? 1 : _ref$A,
|
|
_ref$B = _ref.B,
|
|
B = _ref$B === void 0 ? 0 : _ref$B,
|
|
_ref$C = _ref.C,
|
|
C = _ref$C === void 0 ? 1 : _ref$C;
|
|
|
|
_classCallCheck(this, KalmanFilter);
|
|
|
|
this.R = R; // noise power desirable
|
|
|
|
this.Q = Q; // noise power estimated
|
|
|
|
this.A = A;
|
|
this.C = C;
|
|
this.B = B;
|
|
this.cov = NaN;
|
|
this.x = NaN; // estimated signal without noise
|
|
}
|
|
/**
|
|
* Filter a new value
|
|
* @param {Number} z Measurement
|
|
* @param {Number} u Control
|
|
* @return {Number}
|
|
*/
|
|
|
|
|
|
_createClass(KalmanFilter, [{
|
|
key: "filter",
|
|
value: function filter(z) {
|
|
var u = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : 0;
|
|
|
|
if (isNaN(this.x)) {
|
|
this.x = 1 / this.C * z;
|
|
this.cov = 1 / this.C * this.Q * (1 / this.C);
|
|
} else {
|
|
// Compute prediction
|
|
var predX = this.predict(u);
|
|
var predCov = this.uncertainty(); // Kalman gain
|
|
|
|
var K = predCov * this.C * (1 / (this.C * predCov * this.C + this.Q)); // Correction
|
|
|
|
this.x = predX + K * (z - this.C * predX);
|
|
this.cov = predCov - K * this.C * predCov;
|
|
}
|
|
|
|
return this.x;
|
|
}
|
|
/**
|
|
* Predict next value
|
|
* @param {Number} [u] Control
|
|
* @return {Number}
|
|
*/
|
|
|
|
}, {
|
|
key: "predict",
|
|
value: function predict() {
|
|
var u = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : 0;
|
|
return this.A * this.x + this.B * u;
|
|
}
|
|
/**
|
|
* Return uncertainty of filter
|
|
* @return {Number}
|
|
*/
|
|
|
|
}, {
|
|
key: "uncertainty",
|
|
value: function uncertainty() {
|
|
return this.A * this.cov * this.A + this.R;
|
|
}
|
|
/**
|
|
* Return the last filtered measurement
|
|
* @return {Number}
|
|
*/
|
|
|
|
}, {
|
|
key: "lastMeasurement",
|
|
value: function lastMeasurement() {
|
|
return this.x;
|
|
}
|
|
/**
|
|
* Set measurement noise Q
|
|
* @param {Number} noise
|
|
*/
|
|
|
|
}, {
|
|
key: "setMeasurementNoise",
|
|
value: function setMeasurementNoise(noise) {
|
|
this.Q = noise;
|
|
}
|
|
/**
|
|
* Set the process noise R
|
|
* @param {Number} noise
|
|
*/
|
|
|
|
}, {
|
|
key: "setProcessNoise",
|
|
value: function setProcessNoise(noise) {
|
|
this.R = noise;
|
|
}
|
|
}]);
|
|
|
|
return KalmanFilter;
|
|
}();
|
|
|
|
return KalmanFilter;
|
|
|
|
}());
|
|
|
|
|
|
//==================================== MAIN ====================================
|
|
|
|
var lf = {fix:0,satellites:0};
|
|
var showMax = 0; // 1 = display the max values. 0 = display the cur fix
|
|
var canDraw = 1;
|
|
var time = ''; // Last time string displayed. Re displayed in background colour to remove before drawing new time.
|
|
var sec; // actual seconds for testing purposes
|
|
|
|
var max = {};
|
|
max.spd = 0;
|
|
max.alt = 0;
|
|
max.n = 0; // counter. Only start comparing for max after a certain number of fixes to allow kalman filter to have smoohed the data.
|
|
|
|
var emulator = (process.env.BOARD=="EMSCRIPTEN" || process.env.BOARD=="EMSCRIPTEN2")?1:0; // 1 = running in emulator. Supplies test values;
|
|
|
|
var wp = {}; // Waypoint to use for distance from cur position.
|
|
var SATinView = 0;
|
|
|
|
function radians(a) {
|
|
return a*Math.PI/180;
|
|
}
|
|
|
|
function distance(a,b){
|
|
var x = radians(a.lon-b.lon) * Math.cos(radians((a.lat+b.lat)/2));
|
|
var y = radians(b.lat-a.lat);
|
|
|
|
// Distance in selected units
|
|
var d = Math.sqrt(x*x + y*y) * 6371000;
|
|
d = (d/parseFloat(cfg.dist)).toFixed(2);
|
|
if ( d >= 100 ) d = parseFloat(d).toFixed(1);
|
|
if ( d >= 1000 ) d = parseFloat(d).toFixed(0);
|
|
|
|
return d;
|
|
}
|
|
|
|
function drawFix(dat) {
|
|
|
|
if (!canDraw) return;
|
|
|
|
g.clearRect(0,screenYstart,screenW,screenH);
|
|
|
|
var v = '';
|
|
var u='';
|
|
|
|
// Primary Display
|
|
v = (cfg.primSpd)?dat.speed.toString():dat.alt.toString();
|
|
|
|
// Primary Units
|
|
u = (cfg.primSpd)?cfg.spd_unit:dat.alt_units;
|
|
|
|
drawPrimary(v,u);
|
|
|
|
// Secondary Display
|
|
v = (cfg.primSpd)?dat.alt.toString():dat.speed.toString();
|
|
|
|
// Secondary Units
|
|
u = (cfg.primSpd)?dat.alt_units:cfg.spd_unit;
|
|
|
|
drawSecondary(v,u);
|
|
|
|
// Time
|
|
drawTime();
|
|
|
|
//Sats
|
|
if ( dat.age > 10 ) {
|
|
if ( dat.age > 90 ) dat.age = '>90';
|
|
drawSats('Age:'+dat.age);
|
|
}
|
|
else if (!BANGLEJS2) {
|
|
drawSats('Sats:'+dat.sats);
|
|
} else {
|
|
if (lf.fix) {
|
|
drawSats('Sats:'+dat.sats);
|
|
} else {
|
|
drawSats('View:' + SATinView);
|
|
}
|
|
}
|
|
g.reset();
|
|
}
|
|
|
|
|
|
function drawClock() {
|
|
if (!canDraw) return;
|
|
g.clearRect(0,screenYstart,screenW,screenH);
|
|
drawTime();
|
|
g.reset();
|
|
}
|
|
|
|
|
|
function drawPrimary(n,u) {
|
|
//if(emulator)console.log("\n1: " + n +" "+ u);
|
|
var s=40; // Font size
|
|
var l=n.length;
|
|
|
|
if ( l <= 7 ) s=48;
|
|
if ( l <= 6 ) s=55;
|
|
if ( l <= 5 ) s=66;
|
|
if ( l <= 4 ) s=85;
|
|
if ( l <= 3 ) s=110;
|
|
|
|
// X -1=left (default), 0=center, 1=right
|
|
// Y -1=top (default), 0=center, 1=bottom
|
|
g.setFontAlign(0,-1); // center, top
|
|
if (lf.fix) g.setColor(col1); else g.setColor(colUncertain);
|
|
if (BANGLEJS2) s *= fontFactorB2;
|
|
g.setFontVector(s);
|
|
g.drawString(n, screenW_Half - 10, screenYstart);
|
|
|
|
// Primary Units
|
|
s = 35; // Font size
|
|
g.setFontAlign(1,-1,3); // right, top, rotate
|
|
g.setColor(col1);
|
|
if (BANGLEJS2) s = 20;
|
|
g.setFontVector(s);
|
|
g.drawString(u, screenW - 20, screenYstart + 2);
|
|
}
|
|
|
|
|
|
function drawSecondary(n,u) {
|
|
//if(emulator)console.log("2: " + n +" "+ u);
|
|
|
|
if (calibrateCompass) hdngCompass = "CALIB!";
|
|
else hdngCompass +="°";
|
|
|
|
g.setFontAlign(0,1);
|
|
g.setColor(col1);
|
|
|
|
g.setFontVector(12).drawString("Altitude GPS / Barometer", screenW_Half - 5, screenY_Half - 10);
|
|
g.setFontVector(20);
|
|
g.drawString(n+" "+u+" / "+altiBaro+" "+u, screenW_Half, screenY_Half + 11);
|
|
|
|
g.setFontVector(12).drawString("Heading GPS / Compass", screenW_Half - 10, screenY_Half + 26);
|
|
g.setFontVector(20);
|
|
g.drawString(hdngGPS+"° / "+hdngCompass, screenW_Half, screenY_Half + 47);
|
|
}
|
|
|
|
|
|
function drawTime() {
|
|
var x = 0, y = screenH;
|
|
g.setFontAlign(-1,1); // left, bottom
|
|
g.setFont("6x8", 2);
|
|
|
|
g.setColor(colbg);
|
|
g.drawString(time,x+1,y); // clear old time
|
|
|
|
time = require("locale").time(new Date(),1);
|
|
|
|
g.setColor(colfg); // draw new time
|
|
g.drawString(time,x+2,y);
|
|
}
|
|
|
|
|
|
function drawSats(sats) {
|
|
|
|
g.setColor(col1);
|
|
g.setFont("6x8", 2);
|
|
g.setFontAlign(1,1); //right, bottom
|
|
g.drawString(sats,screenW,screenH);
|
|
|
|
g.setFontVector(18);
|
|
g.setColor(col1);
|
|
|
|
if ( cfg.modeA == 1 ) {
|
|
if ( showMax ) {
|
|
g.setFontAlign(0,1); //centre, bottom
|
|
g.drawString('MAX',120,164);
|
|
}
|
|
}
|
|
}
|
|
|
|
function onGPS(fix) {
|
|
|
|
if ( emulator ) {
|
|
fix.fix = 1;
|
|
fix.speed = Math.random()*30; // calmed by Kalman filter if cfg.spdFilt
|
|
fix.alt = Math.random()*200 -20; // calmed by Kalman filter if cfg.altFilt
|
|
fix.lat = 50.59; // google.de/maps/@50.59,8.53,17z
|
|
fix.lon = 8.53;
|
|
fix.course = 365;
|
|
fix.satellites = sec;
|
|
fix.time = new Date();
|
|
fix.smoothed = 0;
|
|
}
|
|
|
|
var m;
|
|
|
|
var sp = '---';
|
|
var al = '---';
|
|
var di = '---';
|
|
var age = '---';
|
|
|
|
if (fix.fix) lf = fix;
|
|
|
|
hdngGPS = lf.course;
|
|
if (isNaN(hdngGPS)) hdngGPS = "---";
|
|
else if (0 == hdngGPS) hdngGPS = "0?";
|
|
else hdngGPS = hdngGPS.toFixed(0);
|
|
|
|
if (emulator) hdngCompass = hdngGPS;
|
|
if (emulator) altiBaro = lf.alt.toFixed(0);
|
|
|
|
if (lf.fix) {
|
|
|
|
if (BANGLEJS2 && !emulator) Bangle.removeListener('GPS-raw', onGPSraw);
|
|
|
|
// Smooth data
|
|
if ( lf.smoothed !== 1 ) {
|
|
if ( cfg.spdFilt ) lf.speed = spdFilter.filter(lf.speed);
|
|
if ( cfg.altFilt ) lf.alt = altFilter.filter(lf.alt);
|
|
lf.smoothed = 1;
|
|
if ( max.n <= 15 ) max.n++;
|
|
}
|
|
|
|
|
|
// Speed
|
|
if ( cfg.spd == 0 ) {
|
|
m = require("locale").speed(lf.speed).match(/([0-9,\.]+)(.*)/); // regex splits numbers from units
|
|
sp = parseFloat(m[1]);
|
|
cfg.spd_unit = m[2];
|
|
}
|
|
else sp = parseFloat(lf.speed)/parseFloat(cfg.spd); // Calculate for selected units
|
|
|
|
if ( sp < 10 ) sp = sp.toFixed(1);
|
|
else sp = Math.round(sp);
|
|
if (parseFloat(sp) > parseFloat(max.spd) && max.n > 15 ) max.spd = parseFloat(sp);
|
|
|
|
// Altitude
|
|
al = lf.alt;
|
|
al = Math.round(parseFloat(al)/parseFloat(cfg.alt));
|
|
if (parseFloat(al) > parseFloat(max.alt) && max.n > 15 ) max.alt = parseFloat(al);
|
|
|
|
// Distance to waypoint
|
|
di = distance(lf,wp);
|
|
if (isNaN(di)) di = 0;
|
|
|
|
// Age of last fix (secs)
|
|
age = Math.max(0,Math.round(getTime())-(lf.time.getTime()/1000));
|
|
}
|
|
|
|
if ( cfg.modeA == 1 ) {
|
|
if ( showMax )
|
|
drawFix({
|
|
speed:max.spd,
|
|
sats:lf.satellites,
|
|
alt:max.alt,
|
|
alt_units:cfg.alt_unit,
|
|
age:age,
|
|
fix:lf.fix
|
|
}); // Speed and alt maximums
|
|
else
|
|
drawFix({
|
|
speed:sp,
|
|
sats:lf.satellites,
|
|
alt:al,
|
|
alt_units:cfg.alt_unit,
|
|
age:age,
|
|
fix:lf.fix
|
|
}); // Show speed/altitude
|
|
}
|
|
}
|
|
|
|
function setButtons(){
|
|
setWatch(_=>load(), BTN1);
|
|
|
|
onGPS(lf);
|
|
}
|
|
|
|
|
|
function updateClock() {
|
|
if (!canDraw) return;
|
|
drawTime();
|
|
g.reset();
|
|
|
|
if ( emulator ) {
|
|
max.spd++; max.alt++;
|
|
d=new Date(); sec=d.getSeconds();
|
|
onGPS(lf);
|
|
}
|
|
}
|
|
|
|
|
|
// =Main Prog
|
|
|
|
// Read settings.
|
|
let cfg = require('Storage').readJSON('bikespeedo.json',1)||{};
|
|
|
|
cfg.spd = 1; // Multiplier for speed unit conversions. 0 = use the locale values for speed
|
|
cfg.spd_unit = 'km/h'; // Displayed speed unit
|
|
cfg.alt = 1; // Multiplier for altitude unit conversions. (feet:'0.3048')
|
|
cfg.alt_unit = 'm'; // Displayed altitude units ('feet')
|
|
cfg.dist = 1000; // Multiplier for distnce unit conversions.
|
|
cfg.dist_unit = 'km'; // Displayed distnce units
|
|
cfg.modeA = 1;
|
|
cfg.primSpd = 1; // 1 = Spd in primary, 0 = Spd in secondary
|
|
|
|
cfg.altDiff = cfg.altDiff==undefined?100:cfg.altDiff;
|
|
cfg.spdFilt = cfg.spdFilt==undefined?true:cfg.spdFilt;
|
|
cfg.altFilt = cfg.altFilt==undefined?false:cfg.altFilt;
|
|
// console.log("cfg.altDiff: " + cfg.altDiff);
|
|
// console.log("cfg.spdFilt: " + cfg.spdFilt);
|
|
// console.log("cfg.altFilt: " + cfg.altFilt);
|
|
|
|
if ( cfg.spdFilt ) var spdFilter = new KalmanFilter({R: 0.1 , Q: 1 });
|
|
if ( cfg.altFilt ) var altFilter = new KalmanFilter({R: 0.01, Q: 2 });
|
|
|
|
function onGPSraw(nmea) {
|
|
var nofGP = 0, nofBD = 0, nofGL = 0;
|
|
if (nmea.slice(3,6) == "GSV") {
|
|
// console.log(nmea.slice(1,3) + " " + nmea.slice(11,13));
|
|
if (nmea.slice(0,7) == "$GPGSV,") nofGP = Number(nmea.slice(11,13));
|
|
if (nmea.slice(0,7) == "$BDGSV,") nofBD = Number(nmea.slice(11,13));
|
|
if (nmea.slice(0,7) == "$GLGSV,") nofGL = Number(nmea.slice(11,13));
|
|
SATinView = nofGP + nofBD + nofGL;
|
|
} }
|
|
if(BANGLEJS2) Bangle.on('GPS-raw', onGPSraw);
|
|
|
|
function onPressure(dat) {
|
|
altiBaro = Number(dat.altitude.toFixed(0)) + Number(cfg.altDiff);
|
|
}
|
|
|
|
Bangle.setBarometerPower(1); // needs some time...
|
|
g.clearRect(0,screenYstart,screenW,screenH);
|
|
onGPS(lf);
|
|
Bangle.setGPSPower(1);
|
|
Bangle.on('GPS', onGPS);
|
|
Bangle.on('pressure', onPressure);
|
|
|
|
Bangle.setCompassPower(1);
|
|
var CALIBDATA = require("Storage").readJSON("magnav.json",1)||null;
|
|
if (!CALIBDATA) calibrateCompass = true;
|
|
function Compass_tiltfixread(O,S){
|
|
"ram";
|
|
//console.log(O.x+" "+O.y+" "+O.z);
|
|
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;
|
|
}
|
|
var Compass_heading = 0;
|
|
function Compass_newHeading(m,h){
|
|
var s = Math.abs(m - h);
|
|
var delta = (m>h)?1:-1;
|
|
if (s>=180){s=360-s; delta = -delta;}
|
|
if (s<2) return h;
|
|
var hd = h + delta*(1 + Math.round(s/5));
|
|
if (hd<0) hd+=360;
|
|
if (hd>360)hd-= 360;
|
|
return hd;
|
|
}
|
|
function Compass_reading() {
|
|
"ram";
|
|
var d = Compass_tiltfixread(CALIBDATA.offset,CALIBDATA.scale);
|
|
Compass_heading = Compass_newHeading(d,Compass_heading);
|
|
hdngCompass = Compass_heading.toFixed(0);
|
|
}
|
|
if (!calibrateCompass) setInterval(Compass_reading,200);
|
|
|
|
setButtons();
|
|
if (emulator) setInterval(updateClock, 2000);
|
|
else setInterval(updateClock, 10000);
|
|
|
|
Bangle.loadWidgets();
|
|
Bangle.drawWidgets();
|