Add PID Error calculations

Add new graph fields representing the PID Error; This requires the
calculation of rcCommand[] to actual deg/s for comparison with gyro
values; Only works for ACRO mode at the moment and the calc is
hardcoded for ReWrite PID controller; with fixed rates; Need to make
the values dynamic….

Author: Gary Keeble

js/flightlog.js

@@ -199,6 +199,7 @@ function FlightLog(logData) {
         // Add names for our ADDITIONAL_COMPUTED_FIELDS 
         fieldNames.push("heading[0]", "heading[1]", "heading[2]");
         fieldNames.push("axisSum[0]", "axisSum[1]", "axisSum[2]");
+        fieldNames.push("axisError[0]", "axisError[1]", "axisError[2]"); // Custom calculated error field
 
         fieldNameToIndex = {};
         for (i = 0; i < fieldNames.length; i++) {
@@ -469,7 +470,8 @@ function FlightLog(logData) {
             gyroADC = [fieldNameToIndex["gyroADC[0]"], fieldNameToIndex["gyroADC[1]"], fieldNameToIndex["gyroADC[2]"]], 
             accSmooth = [fieldNameToIndex["accSmooth[0]"], fieldNameToIndex["accSmooth[1]"], fieldNameToIndex["accSmooth[2]"]],
             magADC = [fieldNameToIndex["magADC[0]"], fieldNameToIndex["magADC[1]"], fieldNameToIndex["magADC[2]"]],
-            
+            rcCommand = [fieldNameToIndex["rcCommand[0]"], fieldNameToIndex["rcCommand[1]"], fieldNameToIndex["rcCommand[2]"]],
+
             sourceChunkIndex, destChunkIndex,
 
             sysConfig,
@@ -534,6 +536,15 @@ function FlightLog(logData) {
                             (axisPID[axis][1] !== undefined ? srcFrame[axisPID[axis][1]] : 0) +
                             (axisPID[axis][2] !== undefined ? srcFrame[axisPID[axis][2]] : 0);
                     }
+
+                    // Calculate the PID Error
+                    for (var axis = 0; axis < 3; axis++) {
+                        destFrame[fieldIndex++] =  
+                            (gyroADC[axis] !== undefined ? that.gyroRawToDegreesPerSecond(srcFrame[gyroADC[axis]]) : 0) - 
+                            (rcCommand[axis] !== undefined ? that.rcCommandRawToDegreesPerSecond(srcFrame[rcCommand[axis]], axis) : 0);
+                        console.log()
+                    }
+
                 }
             }
         }
@@ -881,6 +892,43 @@ FlightLog.prototype.gyroRawToDegreesPerSecond = function(value) {
     return this.getSysConfig().gyroScale * 1000000 / (Math.PI / 180.0) * value;
 };
 
+// Convert rcCommand to degrees per second
+FlightLog.prototype.rcCommandRawToDegreesPerSecond = function(value, axis) {
+
+    // Axis 0,1 refers to Roll and Pitch
+    // Axis 2 refers to Yaw.
+
+    var rRate  = 65;  // Roll Rate from configuration
+    var pRate  = 65;  // Pitch Rate from configuration
+    var yRate  = 65;  // Yaw Rate from configuration
+
+    var REWRITE = true; // Define that this is for the Rewrite Data - need to make dynamic!
+
+    if(REWRITE) // PID is ReWrite 
+    {
+        if(axis==0 /*ROLL*/) { 
+            return ((rRate + 27) * value ) >> 6;
+        }
+        if(axis==1 /*PITCH*/) {
+            return ((pRate + 27) * value ) >> 6;
+        }
+        if(axis==2 /*YAW*/) { 
+            return ((yRate + 27) * value ) >> 7;
+        }
+    } else { // PID is Luxfloat 
+        if(axis==0 /*ROLL*/) { 
+            return ((rRate+20)*value) / 50;
+        }
+        if(axis==1 /*PITCH*/) {
+            return ((pRate+20)*value) / 50;
+        }
+        if(axis==2 /*YAW*/) { 
+            return ((yRate+10)*value) / 50;
+        }
+    }
+};
+
+
 FlightLog.prototype.getReferenceVoltageMillivolts = function() {
     return this.vbatADCToMillivolts(this.getSysConfig().vbatref);
 };

js/flightlog_fields_presenter.js

@@ -59,7 +59,14 @@ function FlightLogFieldPresenter() {
         'axisSum[all]': 'PID_sum',
         'axisSum[0]' : 'PID_sum[roll]',
         'axisSum[1]' : 'PID_sum[pitch]',
-        'axisSum[2]' : 'PID_sum[yaw]'
+        'axisSum[2]' : 'PID_sum[yaw]',
+
+        //Virtual fields - Add the Error fields
+        'axisError[all]': 'PID_Error',
+        'axisError[0]' : 'PID_Error[roll]',
+        'axisError[1]' : 'PID_Error[pitch]',
+        'axisError[2]' : 'PID_Error[yaw]'
+
     };
 
     function presentFlags(flags, flagNames) {
@@ -119,6 +126,18 @@ function FlightLogFieldPresenter() {
             case 'gyroADC[2]':
                 return Math.round(flightLog.gyroRawToDegreesPerSecond(value)) + " deg/s";
 
+            case 'axisError[0]':
+            case 'axisError[1]':
+            case 'axisError[2]':
+                return Math.round(value) + " deg/s";
+
+            case 'rcCommand[0]':
+                return Math.round(flightLog.rcCommandRawToDegreesPerSecond(value,0)) + " deg/s";
+            case 'rcCommand[1]':
+                return Math.round(flightLog.rcCommandRawToDegreesPerSecond(value,1)) + " deg/s";
+            case 'rcCommand[2]':
+                return Math.round(flightLog.rcCommandRawToDegreesPerSecond(value,2)) + " deg/s";
+
             case 'accSmooth[0]':
             case 'accSmooth[1]':
             case 'accSmooth[2]':

js/graph_config.js

@@ -173,6 +173,10 @@ GraphConfig.load = function(config) {
                 label: "PIDs",
                 fields: ["axisSum[all]"]
             },
+            {
+                label: "Gyro Error",
+                fields: ["axisError[all]"]
+            },             
             {
                 label: "Gyro + PID roll",
                 fields: ["axisP[0]", "axisI[0]", "axisD[0]", "gyroADC[0]"]
@@ -239,6 +243,13 @@ GraphConfig.load = function(config) {
                 inputRange: sysConfig.acc_1G * 3.0, /* Reasonable typical maximum for acc */
                 outputRange: 1.0
             };
+        } else if (fieldName.match(/^axisError\[/)) { // new PID error field
+            return {
+                offset: 0,
+                power: 0.8, /* Make this 1.0 to scale linearly */
+                inputRange: 1000, // Maximum error is hard coded to 1000 deg/s
+                outputRange: 1.0
+            };
         } else if (fieldName.match(/^axis.+\[/)) {
             return {
                 offset: 0,