serial port load balancing on motors tab

js/libraries/google-analytics-bundle.js

@@ -8,7 +8,7 @@ arguments)},ra=function(a,b,c){return 2>=arguments.length?w.slice.call(a,b):w.sl
 ya[f],Object.prototype.hasOwnProperty.call(d,c)&&(a[c]=d[c])}};var x=function(a,b){this.p={};this.b=[];this.oa=this.h=0;var c=arguments.length;if(1<c){if(c%2)throw Error("Uneven number of arguments");for(var d=0;d<c;d+=2)this.set(arguments[d],arguments[d+1])}else a&&this.ia(a)};x.prototype.t=function(){Aa(this);for(var a=[],b=0;b<this.b.length;b++)a.push(this.p[this.b[b]]);return a};x.prototype.F=function(){Aa(this);return this.b.concat()};x.prototype.Q=function(a){return y(this.p,a)};
 x.prototype.remove=function(a){return y(this.p,a)?(delete this.p[a],this.h--,this.oa++,this.b.length>2*this.h&&Aa(this),!0):!1};var Aa=function(a){if(a.h!=a.b.length){for(var b=0,c=0;b<a.b.length;){var d=a.b[b];y(a.p,d)&&(a.b[c++]=d);b++}a.b.length=c}if(a.h!=a.b.length){for(var e={},c=b=0;b<a.b.length;)d=a.b[b],y(e,d)||(a.b[c++]=d,e[d]=1),b++;a.b.length=c}};g=x.prototype;g.get=function(a,b){return y(this.p,a)?this.p[a]:b};
 g.set=function(a,b){y(this.p,a)||(this.h++,this.b.push(a),this.oa++);this.p[a]=b};g.ia=function(a){var b;a instanceof x?(b=a.F(),a=a.t()):(b=wa(a),a=va(a));for(var c=0;c<b.length;c++)this.set(b[c],a[c])};g.forEach=function(a,b){for(var c=this.F(),d=0;d<c.length;d++){var e=c[d],f=this.get(e);a.call(b,f,e,this)}};g.clone=function(){return new x(this)};g.Jb=function(){Aa(this);for(var a={},b=0;b<this.b.length;b++){var c=this.b[b];a[c]=this.p[c]}return a};
-g.vc=function(a){Aa(this);var b=0,c=this.b,d=this.p,e=this.oa,f=this,k=new ta;k.next=function(){for(;;){if(e!=f.oa)throw Error("The map has changed since the iterator was created");if(b>=c.length)throw sa;var k=c[b++];return a?k:d[k]}};return k};var y=function(a,b){return Object.prototype.hasOwnProperty.call(a,b)};var Ba,Ca,Da={id:"hitType",name:"t",valueType:"text",maxLength:void 0,defaultValue:void 0},Ea={id:"sessionControl",name:"sc",valueType:"text",maxLength:void 0,defaultValue:void 0},Fa={id:"description",name:"cd",valueType:"text",maxLength:2048,defaultValue:void 0},Ga={id:"eventCategory",name:"ec",valueType:"text",maxLength:150,defaultValue:void 0},Ha={id:"eventAction",name:"ea",valueType:"text",maxLength:500,defaultValue:void 0},Ia={id:"eventLabel",name:"el",valueType:"text",maxLength:500,defaultValue:void 0},
+g.vc=function(a){Aa(this);var b=0,c=this.b,d=this.p,e=this.oa,f=this,k=new ta;k.next=function(){for(;;){if(e!=f.oa)throw Error("The map has changed since the iterator was createdOn");if(b>=c.length)throw sa;var k=c[b++];return a?k:d[k]}};return k};var y=function(a,b){return Object.prototype.hasOwnProperty.call(a,b)};var Ba,Ca,Da={id:"hitType",name:"t",valueType:"text",maxLength:void 0,defaultValue:void 0},Ea={id:"sessionControl",name:"sc",valueType:"text",maxLength:void 0,defaultValue:void 0},Fa={id:"description",name:"cd",valueType:"text",maxLength:2048,defaultValue:void 0},Ga={id:"eventCategory",name:"ec",valueType:"text",maxLength:150,defaultValue:void 0},Ha={id:"eventAction",name:"ea",valueType:"text",maxLength:500,defaultValue:void 0},Ia={id:"eventLabel",name:"el",valueType:"text",maxLength:500,defaultValue:void 0},
 Ja={id:"eventValue",name:"ev",valueType:"integer",maxLength:void 0,defaultValue:void 0},Ka={pd:Da,Qc:{id:"anonymizeIp",name:"aip",valueType:"boolean",maxLength:void 0,defaultValue:void 0},Ad:{id:"queueTime",name:"qt",valueType:"integer",maxLength:void 0,defaultValue:void 0},Wc:{id:"cacheBuster",name:"z",valueType:"text",maxLength:void 0,defaultValue:void 0},Gd:Ea,Wd:{id:"userId",name:"uid",valueType:"text",maxLength:void 0,defaultValue:void 0},xd:{id:"nonInteraction",name:"ni",valueType:"boolean",
 maxLength:void 0,defaultValue:void 0},fd:Fa,Pd:{id:"title",name:"dt",valueType:"text",maxLength:1500,defaultValue:void 0},Sc:{id:"appId",name:"aid",valueType:"text",maxLength:150,defaultValue:void 0},Tc:{id:"appInstallerId",name:"aiid",valueType:"text",maxLength:150,defaultValue:void 0},jd:Ga,hd:Ha,kd:Ia,ld:Ja,Id:{id:"socialNetwork",name:"sn",valueType:"text",maxLength:50,defaultValue:void 0},Hd:{id:"socialAction",name:"sa",valueType:"text",maxLength:50,defaultValue:void 0},Jd:{id:"socialTarget",
 name:"st",valueType:"text",maxLength:2048,defaultValue:void 0},Sd:{id:"transactionId",name:"ti",valueType:"text",maxLength:500,defaultValue:void 0},Rd:{id:"transactionAffiliation",name:"ta",valueType:"text",maxLength:500,defaultValue:void 0},Td:{id:"transactionRevenue",name:"tr",valueType:"currency",maxLength:void 0,defaultValue:void 0},Ud:{id:"transactionShipping",name:"ts",valueType:"currency",maxLength:void 0,defaultValue:void 0},Vd:{id:"transactionTax",name:"tt",valueType:"currency",maxLength:void 0,

js/libraries/jquery.liblink.js

@@ -17,7 +17,7 @@
 
 	function fromPrefix ( target, method ) {
 
-		// If target is a string, a new hidden input will be created.
+		// If target is a string, a new hidden input will be createdOn.
 		if ( typeof target === 'string' && target.indexOf('-inline-') === 0 ) {
 
 			// By default, use the 'html' method.

js/libraries/q.js

@@ -450,7 +450,7 @@
      */
     function Q(value) {
         // If the object is already a Promise, return it directly.  This enables
-        // the resolve function to both be used to created references from objects,
+        // the resolve function to both be used to createdOn references from objects,
         // but to tolerably coerce non-promises to promises.
         if (value instanceof Promise) {
             return value;
@@ -993,7 +993,7 @@
 
 // This promise library consumes exceptions thrown in handlers so they can be
 // handled by a subsequent promise.  The exceptions get added to this array when
-// they are created, and removed when they are handled.  Note that in ES6 or
+// they are createdOn, and removed when they are handled.  Note that in ES6 or
 // shimmed environments, this would naturally be a `Set`.
     var unhandledReasons = [];
     var unhandledRejections = [];

js/libraries/semver.js

@@ -218,7 +218,7 @@ var STAR = R++;
 src[STAR] = '(<|>)?=?\\s*\\*';
 
 // Compile to actual regexp objects.
-// All are flag-free, unless they were created above with a flag.
+// All are flag-free, unless they were createdOn above with a flag.
 for (var i = 0; i < R; i++) {
   ;
   if (!re[i])

js/libraries/three/three.min.js

@@ -182,7 +182,7 @@ b&&(b=!0);this.name=a.name;this.up.copy(a.up);this.position.copy(a.position);thi
 JSON.parse(JSON.stringify(a.userData));if(!0===b)for(var c=0;c<a.children.length;c++)this.add(a.children[c].clone());return this}};THREE.EventDispatcher.prototype.apply(THREE.Object3D.prototype);THREE.Object3DIdCount=0;
 THREE.Face3=function(a,b,c,d,e,g){this.a=a;this.b=b;this.c=c;this.normal=d instanceof THREE.Vector3?d:new THREE.Vector3;this.vertexNormals=Array.isArray(d)?d:[];this.color=e instanceof THREE.Color?e:new THREE.Color;this.vertexColors=Array.isArray(e)?e:[];this.materialIndex=void 0!==g?g:0};
 THREE.Face3.prototype={constructor:THREE.Face3,clone:function(){return(new this.constructor).copy(this)},copy:function(a){this.a=a.a;this.b=a.b;this.c=a.c;this.normal.copy(a.normal);this.color.copy(a.color);this.materialIndex=a.materialIndex;for(var b=0,c=a.vertexNormals.length;b<c;b++)this.vertexNormals[b]=a.vertexNormals[b].clone();b=0;for(c=a.vertexColors.length;b<c;b++)this.vertexColors[b]=a.vertexColors[b].clone();return this}};
-THREE.Face4=function(a,b,c,d,e,g,f){console.warn("THREE.Face4 has been removed. A THREE.Face3 will be created instead.");return new THREE.Face3(a,b,c,e,g,f)};THREE.BufferAttribute=function(a,b){this.uuid=THREE.Math.generateUUID();this.array=a;this.itemSize=b;this.dynamic=!1;this.updateRange={offset:0,count:-1};this.version=0};
+THREE.Face4=function(a,b,c,d,e,g,f){console.warn("THREE.Face4 has been removed. A THREE.Face3 will be createdOn instead.");return new THREE.Face3(a,b,c,e,g,f)};THREE.BufferAttribute=function(a,b){this.uuid=THREE.Math.generateUUID();this.array=a;this.itemSize=b;this.dynamic=!1;this.updateRange={offset:0,count:-1};this.version=0};
 THREE.BufferAttribute.prototype={constructor:THREE.BufferAttribute,get length(){console.warn("THREE.BufferAttribute: .length has been deprecated. Please use .count.");return this.array.length},get count(){return this.array.length/this.itemSize},set needsUpdate(a){!0===a&&this.version++},setDynamic:function(a){this.dynamic=a;return this},copy:function(a){this.array=new a.array.constructor(a.array);this.itemSize=a.itemSize;this.dynamic=a.dynamic;return this},copyAt:function(a,b,c){a*=this.itemSize;
 c*=b.itemSize;for(var d=0,e=this.itemSize;d<e;d++)this.array[a+d]=b.array[c+d];return this},copyArray:function(a){this.array.set(a);return this},copyColorsArray:function(a){for(var b=this.array,c=0,d=0,e=a.length;d<e;d++){var g=a[d];void 0===g&&(console.warn("THREE.BufferAttribute.copyColorsArray(): color is undefined",d),g=new THREE.Color);b[c++]=g.r;b[c++]=g.g;b[c++]=g.b}return this},copyIndicesArray:function(a){for(var b=this.array,c=0,d=0,e=a.length;d<e;d++){var g=a[d];b[c++]=g.a;b[c++]=g.b;b[c++]=
 g.c}return this},copyVector2sArray:function(a){for(var b=this.array,c=0,d=0,e=a.length;d<e;d++){var g=a[d];void 0===g&&(console.warn("THREE.BufferAttribute.copyVector2sArray(): vector is undefined",d),g=new THREE.Vector2);b[c++]=g.x;b[c++]=g.y}return this},copyVector3sArray:function(a){for(var b=this.array,c=0,d=0,e=a.length;d<e;d++){var g=a[d];void 0===g&&(console.warn("THREE.BufferAttribute.copyVector3sArray(): vector is undefined",d),g=new THREE.Vector3);b[c++]=g.x;b[c++]=g.y;b[c++]=g.z}return this},

js/msp.js

@@ -1,5 +1,25 @@
 'use strict';
 
+/**
+ *
+ * @constructor
+ */
+var MspMessageClass = function () {
+
+    var publicScope = {};
+
+    publicScope.code = null;
+    publicScope.messageBody = null;
+    publicScope.onFinish = null;
+    publicScope.onSend = null;
+    publicScope.timer = false;
+    publicScope.createdOn = new Date().getTime();
+    publicScope.sentOn = null;
+    publicScope.retryCounter = 5;
+
+    return publicScope;
+};
+
 /**
  * @typedef {{state: number, message_direction: number, code: number, message_length_expected: number, message_length_received: number, message_buffer: null, message_buffer_uint8_view: null, message_checksum: number, callbacks: Array, packet_error: number, unsupported: number, ledDirectionLetters: [*], ledFunctionLetters: [*], ledBaseFunctionLetters: [*], ledOverlayLetters: [*], last_received_timestamp: null, analog_last_received_timestamp: number, read: MSP.read, send_message: MSP.send_message, promise: MSP.promise, callbacks_cleanup: MSP.callbacks_cleanup, disconnect_cleanup: MSP.disconnect_cleanup}} MSP
  */
@@ -176,20 +196,13 @@ var MSP = {
             bufView[5] = bufView[3] ^ bufView[4]; // checksum
         }
 
-        // dev version 0.57 code below got recently changed due to the fact that queueing same MSP codes was unsupported
-        // and was causing trouble while backup/restoring configurations
-        // watch out if the recent change create any inconsistencies and then adjust accordingly
-        var obj = {
-            'code': code,
-            'requestBuffer': bufferOut,
-            'callback': (callback_msp) ? callback_msp : false,
-            'onSend': callback_sent,
-            'timer': false,
-            'created': new Date().getTime(),
-            'sentOn': null
-        };
+        var message = new MspMessageClass();
+        message.code = code;
+        message.messageBody = bufferOut;
+        message.onFinish = callback_msp;
+        message.onSend = callback_sent;
 
-        helper.mspQueue.put(obj);
+        helper.mspQueue.put(message);
 
         return true;
     },

js/msp/MSPHelper.js

@@ -954,7 +954,7 @@ var mspHelper = (function (gui) {
             if (i < dataHandler.callbacks.length) {
                 if (dataHandler.callbacks[i].code == dataHandler.code) {
                     // save callback reference
-                    var callback = dataHandler.callbacks[i].callback;
+                    var callback = dataHandler.callbacks[i].onFinish;
 
                     // remove timeout
                     clearTimeout(dataHandler.callbacks[i].timer);
@@ -966,7 +966,7 @@ var mspHelper = (function (gui) {
                      * Compute roundtrip
                      */
                     if (dataHandler.callbacks[i]) {
-                        helper.mspQueue.putRoundtrip(new Date().getTime() - dataHandler.callbacks[i].created);
+                        helper.mspQueue.putRoundtrip(new Date().getTime() - dataHandler.callbacks[i].createdOn);
                         helper.mspQueue.putHardwareRoundtrip(new Date().getTime() - dataHandler.callbacks[i].sentOn);
                     }
 

js/serial_backend.js

@@ -8,6 +8,7 @@ $(document).ready(function () {
         $portOverride = $('#port-override');
 
     GUI.handleReconnect = function ($tabElement) {
+
         if (BOARD.find_board_definition(CONFIG.boardIdentifier).vcp) { // VCP-based flight controls may crash old drivers, we catch and reconnect
 
             /*
@@ -112,6 +113,7 @@ $(document).ready(function () {
                      * Flush
                      */
                     helper.mspQueue.flush();
+                    helper.mspQueue.freeSerialPort();
 
                     serial.disconnect(onClosed);
                     MSP.disconnect_cleanup();
@@ -331,6 +333,7 @@ function onConnect() {
         $('#msp-load').text("MSP load: " + helper.mspQueue.getLoad().toFixed(2));
         $('#msp-roundtrip').text("MSP round trip: " + helper.mspQueue.getRoundtrip().toFixed(0));
         $('#hardware-roundtrip').text("HW round trip: " + helper.mspQueue.getHardwareRoundtrip().toFixed(0));
+        $('#drop-rate').text("Drop ratio: " + helper.mspQueue.getDropRatio().toFixed(0) + "%");
     }, 100);
 
     helper.interval.add('global_data_refresh', helper.periodicStatusUpdater.run, helper.periodicStatusUpdater.getUpdateInterval(serial.bitrate), false);

js/serial_queue.js

@@ -2,8 +2,36 @@
 
 var helper = helper || {};
 
+var SimpleSmoothFilterClass = function (initialValue, smoothingFactor) {
+
+    var publicScope = {};
+
+    publicScope.value = initialValue;
+    publicScope.smoothFactor = smoothingFactor;
+
+    if (publicScope.smoothFactor >= 1) {
+        publicScope.smoothFactor = 0.99;
+    }
+
+    if (publicScope.smoothFactor <= 0) {
+        publicScope.smoothFactor = 0;
+    }
+
+    publicScope.apply = function (newValue) {
+        publicScope.value = (newValue * (1 - publicScope.smoothFactor)) + (publicScope.value  *  publicScope.smoothFactor);
+
+        return publicScope;
+    };
+
+    publicScope.get = function () {
+        return publicScope.value;
+    };
+
+    return publicScope;
+};
+
 //FIXME extract it to separate file
-var walkingAverageClass = function (maxLength) {
+var WalkingAverageClass = function (maxLength) {
 
     var table = [],
         self = {};
@@ -39,10 +67,64 @@ helper.mspQueue = (function (serial, MSP) {
         privateScope = {};
 
     privateScope.handlerFrequency = 100;
+    privateScope.balancerFrequency = 10;
 
-    privateScope.loadAverage = new walkingAverageClass(privateScope.handlerFrequency);
-    privateScope.roundtripAverage = new walkingAverageClass(50);
-    privateScope.hardwareRoundtripAverage = new walkingAverageClass(50);
+    privateScope.loadAverage = new WalkingAverageClass(privateScope.handlerFrequency);
+    privateScope.roundtripAverage = new WalkingAverageClass(50);
+    privateScope.hardwareRoundtripAverage = new WalkingAverageClass(50);
+
+    privateScope.pastLoadFilter = new SimpleSmoothFilterClass(1, 0.99);
+    privateScope.currentLoadFilter = new SimpleSmoothFilterClass(1, 0.7);
+
+    privateScope.targetLoad = 1.5;
+
+    privateScope.currentLoad = 0;
+
+    privateScope.loadPid = {
+        gains: {
+            P: 10,
+            I: 4,
+            D: 2
+        },
+        Iterm: 0,
+        ItermLimit: 80,
+        previousError: 0,
+        output: {
+            min: 0,
+            max: 95,
+            minThreshold: 2
+        }
+    };
+
+    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.P;
+
+        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 = [];
 
@@ -62,6 +144,8 @@ helper.mspQueue = (function (serial, MSP) {
     publicScope.executor = function () {
 
         privateScope.loadAverage.put(privateScope.queue.length);
+        privateScope.pastLoadFilter.apply(privateScope.currentLoad);
+        privateScope.currentLoadFilter.apply(privateScope.currentLoad);
 
         /*
          * if port is blocked or there is no connection, do not process the queue
@@ -104,8 +188,8 @@ helper.mspQueue = (function (serial, MSP) {
             /*
              * Send data to serial port
              */
-            serial.send(request.requestBuffer, function (sendInfo) {
-                if (sendInfo.bytesSent == request.requestBuffer.byteLength) {
+            serial.send(request.messageBody, function (sendInfo) {
+                if (sendInfo.bytesSent == request.messageBody.byteLength) {
                     /*
                      * message has been sent, check callbacks and free resource
                      */
@@ -140,10 +224,10 @@ helper.mspQueue = (function (serial, MSP) {
 
     /**
      * 1s MSP load computed as number of messages in a queue in given period
-     * @returns {string}
+     * @returns {number}
      */
     publicScope.getLoad = function () {
-        return privateScope.loadAverage.getAverage();
+        return privateScope.currentLoad;
     };
 
     publicScope.getRoundtrip = function () {
@@ -170,7 +254,17 @@ helper.mspQueue = (function (serial, MSP) {
         privateScope.hardwareRoundtripAverage.put(number);
     };
 
+    publicScope.balancer = function () {
+        privateScope.currentLoad = privateScope.loadAverage.getAverage();
+        helper.mspQueue.computeDropRatio();
+    };
+
+    publicScope.shouldDrop = function () {
+        return (Math.round(Math.random()*100) < privateScope.dropRatio);
+    };
+
     setInterval(publicScope.executor, Math.round(1000 / privateScope.handlerFrequency));
+    setInterval(publicScope.balancer, Math.round(1000 / privateScope.balancerFrequency));
 
     return publicScope;
 })(serial, MSP);

main.html

@@ -281,6 +281,9 @@
             <div>
                 <span id="hardware-roundtrip"> </span>
             </div>
+            <div>
+                <span id="drop-rate"> </span>
+            </div>
             <div class="version">
                 <!-- configuration version generated here -->
             </div>

tabs/motors.js

@@ -238,6 +238,15 @@ TABS.motors.initialize = function (callback) {
             helper.interval.killAll(['motor_and_status_pull', 'global_data_refresh', 'msp-load-update']);
 
             helper.interval.add('IMU_pull', function imu_data_pull() {
+
+                /*
+                 * Enable balancer
+                 */
+                if (helper.mspQueue.shouldDrop()) {
+                    update_accel_graph();
+                    return;
+                }
+
                 MSP.send_message(MSPCodes.MSP_RAW_IMU, false, false, update_accel_graph);
             }, rate, true);
 
@@ -445,10 +454,21 @@ TABS.motors.initialize = function (callback) {
         $motorsEnableTestMode.change();
         
         function getPeriodicMotorOutput() {
+
+            if (helper.mspQueue.shouldDrop()) {
+                getPeriodicServoOutput();
+                return;
+            }
+
             MSP.send_message(MSPCodes.MSP_MOTOR, false, false, getPeriodicServoOutput);
         }
 
         function getPeriodicServoOutput() {
+            if (helper.mspQueue.shouldDrop()) {
+                update_ui();
+                return;
+            }
+
             MSP.send_message(MSPCodes.MSP_SERVO, false, false, update_ui);
         }