diff --git a/src/js/RateCurve.js b/src/js/RateCurve.js index 46791441..f9d71ff0 100644 --- a/src/js/RateCurve.js +++ b/src/js/RateCurve.js @@ -24,7 +24,7 @@ var RateCurve = function (useLegacyCurve) { return result; }; - this.drawRateCurve = function (rate, rcRate, rcExpo, superExpoActive, deadband, maxAngularVel, context, width, height) { + this.drawRateCurve = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context, width, height) { var canvasHeightScale = height / (2 * maxAngularVel); var stepWidth = context.lineWidth; @@ -34,10 +34,10 @@ var RateCurve = function (useLegacyCurve) { context.beginPath(); var rcData = minRc; - context.moveTo(-500, -canvasHeightScale * this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband)); + context.moveTo(-500, -canvasHeightScale * this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit)); rcData = rcData + stepWidth; while (rcData <= maxRc) { - context.lineTo(rcData - midRc, -canvasHeightScale * this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband)); + context.lineTo(rcData - midRc, -canvasHeightScale * this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit)); rcData = rcData + stepWidth; } @@ -58,12 +58,12 @@ var RateCurve = function (useLegacyCurve) { context.stroke(); } - this.drawStickPosition = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband, maxAngularVel, context, stickColor) { + this.drawStickPosition = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context, stickColor) { const DEFAULT_SIZE = 60; // canvas units, relative size of the stick indicator (larger value is smaller indicator) const rateScaling = (context.canvas.height / 2) / maxAngularVel; - var currentValue = this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband); + var currentValue = this.rcCommandRawToDegreesPerSecond(rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit); if(rcData!=undefined) { context.save(); @@ -80,7 +80,7 @@ var RateCurve = function (useLegacyCurve) { }; -RateCurve.prototype.rcCommandRawToDegreesPerSecond = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband) { +RateCurve.prototype.rcCommandRawToDegreesPerSecond = function (rcData, rate, rcRate, rcExpo, superExpoActive, deadband, limit) { var angleRate; if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) { if (rcRate > 2) { @@ -113,16 +113,16 @@ RateCurve.prototype.rcCommandRawToDegreesPerSecond = function (rcData, rate, rcR angleRate = (((rate * 100) + 27) * rcCommandf / 16) / 4.1; // Only applies to old versions ? } - angleRate = this.constrain(angleRate, -1998, 1998); // Rate limit protection + angleRate = this.constrain(angleRate, -1 * limit, limit); // Rate limit from profile } return angleRate; }; -RateCurve.prototype.getMaxAngularVel = function (rate, rcRate, rcExpo, superExpoActive, deadband) { +RateCurve.prototype.getMaxAngularVel = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit) { var maxAngularVel; if (!this.useLegacyCurve) { - maxAngularVel = this.rcCommandRawToDegreesPerSecond(maxRc, rate, rcRate, rcExpo, superExpoActive, deadband); + maxAngularVel = this.rcCommandRawToDegreesPerSecond(maxRc, rate, rcRate, rcExpo, superExpoActive, deadband, limit); } return maxAngularVel; @@ -134,7 +134,7 @@ RateCurve.prototype.setMaxAngularVel = function (value) { }; -RateCurve.prototype.draw = function (rate, rcRate, rcExpo, superExpoActive, deadband, maxAngularVel, context) { +RateCurve.prototype.draw = function (rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context) { if (rate !== undefined && rcRate !== undefined && rcExpo !== undefined) { var height = context.canvas.height; var width = context.canvas.width; @@ -142,7 +142,7 @@ RateCurve.prototype.draw = function (rate, rcRate, rcExpo, superExpoActive, dead if (this.useLegacyCurve) { this.drawLegacyRateCurve(rate, rcRate, rcExpo, context, width, height); } else { - this.drawRateCurve(rate, rcRate, rcExpo, superExpoActive, deadband, maxAngularVel, context, width, height); + this.drawRateCurve(rate, rcRate, rcExpo, superExpoActive, deadband, limit, maxAngularVel, context, width, height); } } }; diff --git a/src/js/fc.js b/src/js/fc.js index d34009e5..e7a3a320 100644 --- a/src/js/fc.js +++ b/src/js/fc.js @@ -169,6 +169,9 @@ var FC = { rcYawRate: 0, rcPitchRate: 0, RC_PITCH_EXPO: 0, + roll_rate_limit: 1998, + pitch_rate_limit: 1998, + yaw_rate_limit: 1998, }; AUX_CONFIG = []; diff --git a/src/js/msp/MSPHelper.js b/src/js/msp/MSPHelper.js index 4ca9ee49..5dbd3a5e 100644 --- a/src/js/msp/MSPHelper.js +++ b/src/js/msp/MSPHelper.js @@ -337,6 +337,11 @@ MspHelper.prototype.process_data = function(dataHandler) { RC_tuning.throttleLimitType = data.readU8(); RC_tuning.throttleLimitPercent = data.readU8(); } + if (semver.gte(CONFIG.apiVersion, "1.42.0")) { + RC_tuning.roll_rate_limit = data.readU16(); + RC_tuning.pitch_rate_limit = data.readU16(); + RC_tuning.yaw_rate_limit = data.readU16(); + } break; case MSPCodes.MSP_PID: // PID data arrived, we need to scale it and save to appropriate bank / array diff --git a/src/js/tabs/pid_tuning.js b/src/js/tabs/pid_tuning.js index 11cc0e23..319b293e 100644 --- a/src/js/tabs/pid_tuning.js +++ b/src/js/tabs/pid_tuning.js @@ -765,18 +765,18 @@ TABS.pid_tuning.initialize = function (callback) { self.rateCurve = new RateCurve(useLegacyCurve); - function printMaxAngularVel(rate, rcRate, rcExpo, useSuperExpo, deadband, maxAngularVelElement) { - var maxAngularVel = self.rateCurve.getMaxAngularVel(rate, rcRate, rcExpo, useSuperExpo, deadband).toFixed(0); + function printMaxAngularVel(rate, rcRate, rcExpo, useSuperExpo, deadband, limit, maxAngularVelElement) { + var maxAngularVel = self.rateCurve.getMaxAngularVel(rate, rcRate, rcExpo, useSuperExpo, deadband, limit).toFixed(0); maxAngularVelElement.text(maxAngularVel); return maxAngularVel; } - function drawCurve(rate, rcRate, rcExpo, useSuperExpo, deadband, maxAngularVel, colour, yOffset, context) { + function drawCurve(rate, rcRate, rcExpo, useSuperExpo, deadband, limit, maxAngularVel, colour, yOffset, context) { context.save(); context.strokeStyle = colour; context.translate(0, yOffset); - self.rateCurve.draw(rate, rcRate, rcExpo, useSuperExpo, deadband, maxAngularVel, context); + self.rateCurve.draw(rate, rcRate, rcExpo, useSuperExpo, deadband, limit, maxAngularVel, context); context.restore(); } @@ -808,7 +808,10 @@ TABS.pid_tuning.initialize = function (callback) { rc_pitch_expo: RC_tuning.RC_PITCH_EXPO, superexpo: FEATURE_CONFIG.features.isEnabled('SUPEREXPO_RATES'), deadband: RC_DEADBAND_CONFIG.deadband, - yawDeadband: RC_DEADBAND_CONFIG.yaw_deadband + yawDeadband: RC_DEADBAND_CONFIG.yaw_deadband, + roll_rate_limit: RC_tuning.roll_rate_limit, + pitch_rate_limit: RC_tuning.pitch_rate_limit, + yaw_rate_limit: RC_tuning.yaw_rate_limit }; if (semver.lt(CONFIG.apiVersion, "1.7.0")) { @@ -1201,9 +1204,9 @@ TABS.pid_tuning.initialize = function (callback) { if (!useLegacyCurve) { maxAngularVel = Math.max( - printMaxAngularVel(self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, self.maxAngularVelRollElement), - printMaxAngularVel(self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, self.maxAngularVelPitchElement), - printMaxAngularVel(self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, self.maxAngularVelYawElement)); + printMaxAngularVel(self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.roll_rate_limit, self.maxAngularVelRollElement), + printMaxAngularVel(self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.pitch_rate_limit, self.maxAngularVelPitchElement), + printMaxAngularVel(self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, self.currentRates.yaw_rate_limit, self.maxAngularVelYawElement)); // make maxAngularVel multiple of 200deg/s so that the auto-scale doesn't keep changing for small changes of the maximum curve maxAngularVel = self.rateCurve.setMaxAngularVel(maxAngularVel); @@ -1215,9 +1218,9 @@ TABS.pid_tuning.initialize = function (callback) { } curveContext.lineWidth = 2 * lineScale; - drawCurve(self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, maxAngularVel, '#ff0000', 0, curveContext); - drawCurve(self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, maxAngularVel, '#00ff00', -4, curveContext); - drawCurve(self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, maxAngularVel, '#0000ff', 4, curveContext); + drawCurve(self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.roll_rate_limit, maxAngularVel, '#ff0000', 0, curveContext); + drawCurve(self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.pitch_rate_limit, maxAngularVel, '#00ff00', -4, curveContext); + drawCurve(self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, self.currentRates.yaw_rate_limit, maxAngularVel, '#0000ff', 4, curveContext); self.updateRatesLabels(); @@ -1443,9 +1446,9 @@ TABS.pid_tuning.renderModel = function () { if (RC.channels[0] && RC.channels[1] && RC.channels[2]) { var delta = this.clock.getDelta(); - var roll = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[0], this.currentRates.roll_rate, this.currentRates.rc_rate, this.currentRates.rc_expo, this.currentRates.superexpo, this.currentRates.deadband), - pitch = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[1], this.currentRates.pitch_rate, this.currentRates.rc_rate, this.currentRates.rc_expo, this.currentRates.superexpo, this.currentRates.deadband), - yaw = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[2], this.currentRates.yaw_rate, this.currentRates.rc_rate_yaw, this.currentRates.rc_yaw_expo, this.currentRates.superexpo, this.currentRates.yawDeadband); + var roll = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[0], this.currentRates.roll_rate, this.currentRates.rc_rate, this.currentRates.rc_expo, this.currentRates.superexpo, this.currentRates.deadband, this.currentRates.roll_rate_limit), + pitch = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[1], this.currentRates.pitch_rate, this.currentRates.rc_rate_pitch, this.currentRates.rc_pitch_expo, this.currentRates.superexpo, this.currentRates.deadband, this.currentRates.pitch_rate_limit), + yaw = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[2], this.currentRates.yaw_rate, this.currentRates.rc_rate_yaw, this.currentRates.rc_yaw_expo, this.currentRates.superexpo, this.currentRates.yawDeadband, this.currentRates.yaw_rate_limit); this.model.rotateBy(-degToRad(pitch), -degToRad(yaw), -degToRad(roll)); @@ -1724,9 +1727,9 @@ TABS.pid_tuning.updateRatesLabels = function() { } if(RC.channels[0] && RC.channels[1] && RC.channels[2]) { - currentValues.push(self.rateCurve.drawStickPosition(RC.channels[0], self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, maxAngularVel, stickContext, '#FF8080') + ' deg/s'); - currentValues.push(self.rateCurve.drawStickPosition(RC.channels[1], self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, maxAngularVel, stickContext, '#80FF80') + ' deg/s'); - currentValues.push(self.rateCurve.drawStickPosition(RC.channels[2], self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, maxAngularVel, stickContext, '#8080FF') + ' deg/s'); + currentValues.push(self.rateCurve.drawStickPosition(RC.channels[0], self.currentRates.roll_rate, self.currentRates.rc_rate, self.currentRates.rc_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.roll_rate_limit, maxAngularVel, stickContext, '#FF8080') + ' deg/s'); + currentValues.push(self.rateCurve.drawStickPosition(RC.channels[1], self.currentRates.pitch_rate, self.currentRates.rc_rate_pitch, self.currentRates.rc_pitch_expo, self.currentRates.superexpo, self.currentRates.deadband, self.currentRates.pitch_rate_limit, maxAngularVel, stickContext, '#80FF80') + ' deg/s'); + currentValues.push(self.rateCurve.drawStickPosition(RC.channels[2], self.currentRates.yaw_rate, self.currentRates.rc_rate_yaw, self.currentRates.rc_yaw_expo, self.currentRates.superexpo, self.currentRates.yawDeadband, self.currentRates.yaw_rate_limit, maxAngularVel, stickContext, '#8080FF') + ' deg/s'); } else { currentValues = []; } diff --git a/src/js/tabs/receiver.js b/src/js/tabs/receiver.js index ef5de331..45d76e85 100644 --- a/src/js/tabs/receiver.js +++ b/src/js/tabs/receiver.js @@ -574,9 +574,9 @@ TABS.receiver.renderModel = function () { if (RC.channels[0] && RC.channels[1] && RC.channels[2]) { var delta = this.clock.getDelta(); - var roll = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[0], RC_tuning.roll_rate, RC_tuning.RC_RATE, RC_tuning.RC_EXPO, this.useSuperExpo, this.deadband), - pitch = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[1], RC_tuning.pitch_rate, RC_tuning.rcPitchRate, RC_tuning.RC_PITCH_EXPO, this.useSuperExpo, this.deadband), - yaw = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[2], RC_tuning.yaw_rate, RC_tuning.rcYawRate, RC_tuning.RC_YAW_EXPO, this.useSuperExpo, this.yawDeadband); + var roll = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[0], RC_tuning.roll_rate, RC_tuning.RC_RATE, RC_tuning.RC_EXPO, this.useSuperExpo, this.deadband, RC_tuning.roll_rate_limit), + pitch = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[1], RC_tuning.pitch_rate, RC_tuning.rcPitchRate, RC_tuning.RC_PITCH_EXPO, this.useSuperExpo, this.deadband, RC_tuning.pitch_rate_limit), + yaw = delta * this.rateCurve.rcCommandRawToDegreesPerSecond(RC.channels[2], RC_tuning.yaw_rate, RC_tuning.rcYawRate, RC_tuning.RC_YAW_EXPO, this.useSuperExpo, this.yawDeadband, RC_tuning.yaw_rate_limit); this.model.rotateBy(-degToRad(pitch), -degToRad(yaw), -degToRad(roll)); }