|
|
|
'use strict';
|
|
|
|
|
|
|
|
var helper = helper || {};
|
|
|
|
|
|
|
|
helper.mspQueue = (function (serial, MSP) {
|
|
|
|
|
|
|
|
var publicScope = {},
|
|
|
|
privateScope = {};
|
|
|
|
|
|
|
|
privateScope.handlerFrequency = 100;
|
|
|
|
privateScope.balancerFrequency = 10;
|
|
|
|
|
|
|
|
privateScope.loadFilter = new classes.SimpleSmoothFilter(0.5, 0.996);
|
|
|
|
privateScope.roundtripFilter = new classes.SimpleSmoothFilter(20, 0.996);
|
|
|
|
privateScope.hardwareRoundtripFilter = new classes.SimpleSmoothFilter(5, 0.996);
|
|
|
|
|
|
|
|
privateScope.targetLoad = 1.5;
|
|
|
|
privateScope.statusDropFactor = 0.75;
|
|
|
|
|
|
|
|
privateScope.currentLoad = 0;
|
|
|
|
|
|
|
|
privateScope.loadPid = {
|
|
|
|
gains: {
|
|
|
|
P: 10,
|
|
|
|
I: 4,
|
|
|
|
D: 2
|
|
|
|
},
|
|
|
|
Iterm: 0,
|
|
|
|
ItermLimit: 85,
|
|
|
|
previousError: 0,
|
|
|
|
output: {
|
|
|
|
min: 0,
|
|
|
|
max: 97,
|
|
|
|
minThreshold: 0
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
privateScope.dropRatio = 0;
|
|
|
|
|
|
|
|
publicScope.computeDropRatio = function () {
|
|
|
|
var error = privateScope.currentLoad - privateScope.targetLoad;
|
|
|
|
|
|
|
|
var Pterm = error * privateScope.loadPid.gains.P,
|
|
|
|
Dterm = (error - privateScope.loadPid.previousError) * privateScope.loadPid.gains.D;
|
|
|
|
|
|
|
|
privateScope.loadPid.previousError = error;
|
|
|
|
|
|
|
|
privateScope.loadPid.Iterm += error * privateScope.loadPid.gains.I;
|
|
|
|
if (privateScope.loadPid.Iterm > privateScope.loadPid.ItermLimit) {
|
|
|
|
privateScope.loadPid.Iterm = privateScope.loadPid.ItermLimit;
|
|
|
|
} else if (privateScope.loadPid.Iterm < -privateScope.loadPid.ItermLimit) {
|
|
|
|
privateScope.loadPid.Iterm = -privateScope.loadPid.ItermLimit;
|
|
|
|
}
|
|
|
|
|
|
|
|
privateScope.dropRatio = Pterm + privateScope.loadPid.Iterm + Dterm;
|
|
|
|
if (privateScope.dropRatio < privateScope.loadPid.output.minThreshold) {
|
|
|
|
privateScope.dropRatio = privateScope.loadPid.output.min;
|
|
|
|
}
|
|
|
|
if (privateScope.dropRatio > privateScope.loadPid.output.max) {
|
|
|
|
privateScope.dropRatio = privateScope.loadPid.output.max;
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.getDropRatio = function () {
|
|
|
|
return privateScope.dropRatio;
|
|
|
|
};
|
|
|
|
|
|
|
|
privateScope.queue = [];
|
|
|
|
|
|
|
|
privateScope.portInUse = false;
|
|
|
|
|
|
|
|
/**
|
|
|
|
* This method is periodically executed and moves MSP request
|
|
|
|
* from a queue to serial port. This allows to throttle requests,
|
|
|
|
* adjust rate of new frames being sent and prohibit situation in which
|
|
|
|
* serial port is saturated, virtually overloaded, with outgoing data
|
|
|
|
*
|
|
|
|
* This also implements serial port sharing problem: only 1 frame can be transmitted
|
|
|
|
* at once
|
|
|
|
*
|
|
|
|
* MSP class no longer implements blocking, it is queue responsibility
|
|
|
|
*/
|
|
|
|
publicScope.executor = function () {
|
|
|
|
|
|
|
|
privateScope.loadFilter.apply(privateScope.queue.length);
|
|
|
|
|
|
|
|
/*
|
|
|
|
* if port is blocked or there is no connection, do not process the queue
|
|
|
|
*/
|
|
|
|
if (privateScope.portInUse || serial.connectionId === false) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
var request = privateScope.get();
|
|
|
|
|
|
|
|
if (request !== undefined) {
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Lock serial port as being in use right now
|
|
|
|
*/
|
|
|
|
privateScope.portInUse = true;
|
|
|
|
|
|
|
|
request.timer = setTimeout(function () {
|
|
|
|
console.log('MSP data request timed-out: ' + request.code);
|
|
|
|
/*
|
|
|
|
* Remove current callback
|
|
|
|
*/
|
|
|
|
MSP.removeCallback(request.code);
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Create new entry in the queue
|
|
|
|
*/
|
|
|
|
publicScope.put(request);
|
|
|
|
}, serial.getTimeout());
|
|
|
|
|
|
|
|
if (request.sentOn === null) {
|
|
|
|
request.sentOn = new Date().getTime();
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Set receive callback here
|
|
|
|
*/
|
|
|
|
MSP.putCallback(request);
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Send data to serial port
|
|
|
|
*/
|
|
|
|
serial.send(request.messageBody, function (sendInfo) {
|
|
|
|
if (sendInfo.bytesSent == request.messageBody.byteLength) {
|
|
|
|
/*
|
|
|
|
* message has been sent, check callbacks and free resource
|
|
|
|
*/
|
|
|
|
if (request.onSend) {
|
|
|
|
request.onSend();
|
|
|
|
}
|
|
|
|
privateScope.portInUse = false;
|
|
|
|
}
|
|
|
|
});
|
|
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
privateScope.get = function () {
|
|
|
|
return privateScope.queue.shift();
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.flush = function () {
|
|
|
|
privateScope.queue = [];
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.freeSerialPort = function () {
|
|
|
|
privateScope.portInUse = false;
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.put = function (mspRequest) {
|
|
|
|
privateScope.queue.push(mspRequest);
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.getLength = function () {
|
|
|
|
return privateScope.queue.length;
|
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
* 1s MSP load computed as number of messages in a queue in given period
|
|
|
|
* @returns {number}
|
|
|
|
*/
|
|
|
|
publicScope.getLoad = function () {
|
|
|
|
return privateScope.loadFilter.get();
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.getRoundtrip = function () {
|
|
|
|
return privateScope.roundtripFilter.get();
|
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
*
|
|
|
|
* @param {number} number
|
|
|
|
*/
|
|
|
|
publicScope.putRoundtrip = function (number) {
|
|
|
|
privateScope.roundtripFilter.apply(number);
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.getHardwareRoundtrip = function () {
|
|
|
|
return privateScope.hardwareRoundtripFilter.get();
|
|
|
|
};
|
|
|
|
|
|
|
|
/**
|
|
|
|
*
|
|
|
|
* @param {number} number
|
|
|
|
*/
|
|
|
|
publicScope.putHardwareRoundtrip = function (number) {
|
|
|
|
privateScope.hardwareRoundtripFilter.apply(number);
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.balancer = function () {
|
|
|
|
privateScope.currentLoad = privateScope.loadFilter.get();
|
|
|
|
helper.mspQueue.computeDropRatio();
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.shouldDrop = function () {
|
|
|
|
return (Math.round(Math.random()*100) < privateScope.dropRatio);
|
|
|
|
};
|
|
|
|
|
|
|
|
publicScope.shouldDropStatus = function () {
|
|
|
|
return (Math.round(Math.random()*100) < (privateScope.dropRatio * privateScope.statusDropFactor));
|
|
|
|
};
|
|
|
|
|
|
|
|
setInterval(publicScope.executor, Math.round(1000 / privateScope.handlerFrequency));
|
|
|
|
setInterval(publicScope.balancer, Math.round(1000 / privateScope.balancerFrequency));
|
|
|
|
|
|
|
|
return publicScope;
|
|
|
|
})(serial, MSP);
|