mirror of
https://github.com/iNavFlight/inav-configurator.git
synced 2025-07-23 00:05:19 +03:00
rates in dps for iNav > 1.1.0, see iNavFlight#204
This commit is contained in:
parent
61ebb7a455
commit
e9d2563dad
5 changed files with 216 additions and 142 deletions
69
js/fc.js
69
js/fc.js
|
@ -37,6 +37,13 @@ var FAILSAFE_CONFIG;
|
||||||
var RXFAIL_CONFIG;
|
var RXFAIL_CONFIG;
|
||||||
|
|
||||||
var FC = {
|
var FC = {
|
||||||
|
isRatesInDps: function () {
|
||||||
|
if (typeof CONFIG != "undefined" && CONFIG.flightControllerIdentifier == "INAV" && semver.gt(CONFIG.flightControllerVersion, "1.1.0")) {
|
||||||
|
return true;
|
||||||
|
} else {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
},
|
||||||
resetState: function() {
|
resetState: function() {
|
||||||
CONFIG = {
|
CONFIG = {
|
||||||
apiVersion: "0.0.0",
|
apiVersion: "0.0.0",
|
||||||
|
@ -55,7 +62,7 @@ var FC = {
|
||||||
uid: [0, 0, 0],
|
uid: [0, 0, 0],
|
||||||
accelerometerTrims: [0, 0]
|
accelerometerTrims: [0, 0]
|
||||||
};
|
};
|
||||||
|
|
||||||
BF_CONFIG = {
|
BF_CONFIG = {
|
||||||
mixerConfiguration: 0,
|
mixerConfiguration: 0,
|
||||||
features: 0,
|
features: 0,
|
||||||
|
@ -66,33 +73,33 @@ var FC = {
|
||||||
currentscale: 0,
|
currentscale: 0,
|
||||||
currentoffset: 0
|
currentoffset: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
LED_STRIP = [];
|
LED_STRIP = [];
|
||||||
|
|
||||||
PID = {
|
PID = {
|
||||||
controller: 0
|
controller: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
PID_names = [];
|
PID_names = [];
|
||||||
PIDs = new Array(10);
|
PIDs = new Array(10);
|
||||||
for (var i = 0; i < 10; i++) {
|
for (var i = 0; i < 10; i++) {
|
||||||
PIDs[i] = new Array(3);
|
PIDs[i] = new Array(3);
|
||||||
}
|
}
|
||||||
|
|
||||||
RC_MAP = [];
|
RC_MAP = [];
|
||||||
|
|
||||||
// defaults
|
// defaults
|
||||||
// roll, pitch, yaw, throttle, aux 1, ... aux n
|
// roll, pitch, yaw, throttle, aux 1, ... aux n
|
||||||
RC = {
|
RC = {
|
||||||
active_channels: 0,
|
active_channels: 0,
|
||||||
channels: new Array(32)
|
channels: new Array(32)
|
||||||
};
|
};
|
||||||
|
|
||||||
RC_tuning = {
|
RC_tuning = {
|
||||||
RC_RATE: 0,
|
RC_RATE: 0,
|
||||||
RC_EXPO: 0,
|
RC_EXPO: 0,
|
||||||
roll_pitch_rate: 0, // pre 1.7 api only
|
roll_pitch_rate: 0, // pre 1.7 api only
|
||||||
roll_rate: 0,
|
roll_rate: 0,
|
||||||
pitch_rate: 0,
|
pitch_rate: 0,
|
||||||
yaw_rate: 0,
|
yaw_rate: 0,
|
||||||
dynamic_THR_PID: 0,
|
dynamic_THR_PID: 0,
|
||||||
|
@ -101,26 +108,26 @@ var FC = {
|
||||||
dynamic_THR_breakpoint: 0,
|
dynamic_THR_breakpoint: 0,
|
||||||
RC_YAW_EXPO: 0
|
RC_YAW_EXPO: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
AUX_CONFIG = [];
|
AUX_CONFIG = [];
|
||||||
AUX_CONFIG_IDS = [];
|
AUX_CONFIG_IDS = [];
|
||||||
|
|
||||||
MODE_RANGES = [];
|
MODE_RANGES = [];
|
||||||
ADJUSTMENT_RANGES = [];
|
ADJUSTMENT_RANGES = [];
|
||||||
|
|
||||||
SERVO_CONFIG = [];
|
SERVO_CONFIG = [];
|
||||||
SERVO_RULES = [];
|
SERVO_RULES = [];
|
||||||
|
|
||||||
SERIAL_CONFIG = {
|
SERIAL_CONFIG = {
|
||||||
ports: [],
|
ports: [],
|
||||||
|
|
||||||
// pre 1.6 settings
|
// pre 1.6 settings
|
||||||
mspBaudRate: 0,
|
mspBaudRate: 0,
|
||||||
gpsBaudRate: 0,
|
gpsBaudRate: 0,
|
||||||
gpsPassthroughBaudRate: 0,
|
gpsPassthroughBaudRate: 0,
|
||||||
cliBaudRate: 0,
|
cliBaudRate: 0,
|
||||||
};
|
};
|
||||||
|
|
||||||
SENSOR_DATA = {
|
SENSOR_DATA = {
|
||||||
gyroscope: [0, 0, 0],
|
gyroscope: [0, 0, 0],
|
||||||
accelerometer: [0, 0, 0],
|
accelerometer: [0, 0, 0],
|
||||||
|
@ -130,10 +137,10 @@ var FC = {
|
||||||
kinematics: [0.0, 0.0, 0.0],
|
kinematics: [0.0, 0.0, 0.0],
|
||||||
debug: [0, 0, 0, 0]
|
debug: [0, 0, 0, 0]
|
||||||
};
|
};
|
||||||
|
|
||||||
MOTOR_DATA = new Array(8);
|
MOTOR_DATA = new Array(8);
|
||||||
SERVO_DATA = new Array(8);
|
SERVO_DATA = new Array(8);
|
||||||
|
|
||||||
GPS_DATA = {
|
GPS_DATA = {
|
||||||
fix: 0,
|
fix: 0,
|
||||||
numSat: 0,
|
numSat: 0,
|
||||||
|
@ -153,23 +160,23 @@ var FC = {
|
||||||
timeouts: 0,
|
timeouts: 0,
|
||||||
packetCount: 0
|
packetCount: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
ANALOG = {
|
ANALOG = {
|
||||||
voltage: 0,
|
voltage: 0,
|
||||||
mAhdrawn: 0,
|
mAhdrawn: 0,
|
||||||
rssi: 0,
|
rssi: 0,
|
||||||
amperage: 0
|
amperage: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
ARMING_CONFIG = {
|
ARMING_CONFIG = {
|
||||||
auto_disarm_delay: 0,
|
auto_disarm_delay: 0,
|
||||||
disarm_kill_switch: 0
|
disarm_kill_switch: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
FC_CONFIG = {
|
FC_CONFIG = {
|
||||||
loopTime: 0
|
loopTime: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
MISC = {
|
MISC = {
|
||||||
midrc: 0,
|
midrc: 0,
|
||||||
minthrottle: 0,
|
minthrottle: 0,
|
||||||
|
@ -188,14 +195,14 @@ var FC = {
|
||||||
vbatmaxcellvoltage: 0,
|
vbatmaxcellvoltage: 0,
|
||||||
vbatwarningcellvoltage: 0
|
vbatwarningcellvoltage: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
_3D = {
|
_3D = {
|
||||||
deadband3d_low: 0,
|
deadband3d_low: 0,
|
||||||
deadband3d_high: 0,
|
deadband3d_high: 0,
|
||||||
neutral3d: 0,
|
neutral3d: 0,
|
||||||
deadband3d_throttle: 0
|
deadband3d_throttle: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
DATAFLASH = {
|
DATAFLASH = {
|
||||||
ready: false,
|
ready: false,
|
||||||
supported: false,
|
supported: false,
|
||||||
|
@ -203,7 +210,7 @@ var FC = {
|
||||||
totalSize: 0,
|
totalSize: 0,
|
||||||
usedSize: 0
|
usedSize: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
SDCARD = {
|
SDCARD = {
|
||||||
supported: false,
|
supported: false,
|
||||||
state: 0,
|
state: 0,
|
||||||
|
@ -211,31 +218,31 @@ var FC = {
|
||||||
freeSizeKB: 0,
|
freeSizeKB: 0,
|
||||||
totalSizeKB: 0,
|
totalSizeKB: 0,
|
||||||
};
|
};
|
||||||
|
|
||||||
BLACKBOX = {
|
BLACKBOX = {
|
||||||
supported: false,
|
supported: false,
|
||||||
blackboxDevice: 0,
|
blackboxDevice: 0,
|
||||||
blackboxRateNum: 1,
|
blackboxRateNum: 1,
|
||||||
blackboxRateDenom: 1
|
blackboxRateDenom: 1
|
||||||
};
|
};
|
||||||
|
|
||||||
TRANSPONDER = {
|
TRANSPONDER = {
|
||||||
supported: false,
|
supported: false,
|
||||||
data: []
|
data: []
|
||||||
};
|
};
|
||||||
|
|
||||||
RC_deadband = {
|
RC_deadband = {
|
||||||
deadband: 0,
|
deadband: 0,
|
||||||
yaw_deadband: 0,
|
yaw_deadband: 0,
|
||||||
alt_hold_deadband: 0
|
alt_hold_deadband: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
SENSOR_ALIGNMENT = {
|
SENSOR_ALIGNMENT = {
|
||||||
align_gyro: 0,
|
align_gyro: 0,
|
||||||
align_acc: 0,
|
align_acc: 0,
|
||||||
align_mag: 0
|
align_mag: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
RX_CONFIG = {
|
RX_CONFIG = {
|
||||||
serialrx_provider: 0,
|
serialrx_provider: 0,
|
||||||
maxcheck: 0,
|
maxcheck: 0,
|
||||||
|
@ -245,7 +252,7 @@ var FC = {
|
||||||
rx_min_usec: 0,
|
rx_min_usec: 0,
|
||||||
rx_max_usec: 0
|
rx_max_usec: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
FAILSAFE_CONFIG = {
|
FAILSAFE_CONFIG = {
|
||||||
failsafe_delay: 0,
|
failsafe_delay: 0,
|
||||||
failsafe_off_delay: 0,
|
failsafe_off_delay: 0,
|
||||||
|
@ -254,7 +261,7 @@ var FC = {
|
||||||
failsafe_throttle_low_delay: 0,
|
failsafe_throttle_low_delay: 0,
|
||||||
failsafe_procedure: 0
|
failsafe_procedure: 0
|
||||||
};
|
};
|
||||||
|
|
||||||
RXFAIL_CONFIG = [];
|
RXFAIL_CONFIG = [];
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
185
js/msp.js
185
js/msp.js
|
@ -7,7 +7,7 @@ var MSP_codes = {
|
||||||
MSP_FC_VERSION: 3,
|
MSP_FC_VERSION: 3,
|
||||||
MSP_BOARD_INFO: 4,
|
MSP_BOARD_INFO: 4,
|
||||||
MSP_BUILD_INFO: 5,
|
MSP_BUILD_INFO: 5,
|
||||||
|
|
||||||
// MSP commands for Cleanflight original features
|
// MSP commands for Cleanflight original features
|
||||||
MSP_CHANNEL_FORWARDING: 32,
|
MSP_CHANNEL_FORWARDING: 32,
|
||||||
MSP_SET_CHANNEL_FORWARDING: 33,
|
MSP_SET_CHANNEL_FORWARDING: 33,
|
||||||
|
@ -66,7 +66,7 @@ var MSP_codes = {
|
||||||
MSP_3D: 124,
|
MSP_3D: 124,
|
||||||
MSP_RC_DEADBAND: 125,
|
MSP_RC_DEADBAND: 125,
|
||||||
MSP_SENSOR_ALIGNMENT: 126,
|
MSP_SENSOR_ALIGNMENT: 126,
|
||||||
|
|
||||||
MSP_SET_RAW_RC: 200,
|
MSP_SET_RAW_RC: 200,
|
||||||
MSP_SET_RAW_GPS: 201,
|
MSP_SET_RAW_GPS: 201,
|
||||||
MSP_SET_PID: 202,
|
MSP_SET_PID: 202,
|
||||||
|
@ -85,9 +85,9 @@ var MSP_codes = {
|
||||||
MSP_SET_RC_DEADBAND: 218,
|
MSP_SET_RC_DEADBAND: 218,
|
||||||
MSP_SET_RESET_CURR_PID: 219,
|
MSP_SET_RESET_CURR_PID: 219,
|
||||||
MSP_SET_SENSOR_ALIGNMENT: 220,
|
MSP_SET_SENSOR_ALIGNMENT: 220,
|
||||||
|
|
||||||
// MSP_BIND: 240,
|
// MSP_BIND: 240,
|
||||||
|
|
||||||
MSP_SERVO_MIX_RULES: 241,
|
MSP_SERVO_MIX_RULES: 241,
|
||||||
MSP_SET_SERVO_MIX_RULE: 242,
|
MSP_SET_SERVO_MIX_RULE: 242,
|
||||||
|
|
||||||
|
@ -102,7 +102,7 @@ var MSP_codes = {
|
||||||
MSP_SET_ACC_TRIM: 239, // set acc angle trim values
|
MSP_SET_ACC_TRIM: 239, // set acc angle trim values
|
||||||
MSP_GPS_SV_INFO: 164, // get Signal Strength
|
MSP_GPS_SV_INFO: 164, // get Signal Strength
|
||||||
MSP_GPSSTATISTICS: 166, // GPS statistics
|
MSP_GPSSTATISTICS: 166, // GPS statistics
|
||||||
|
|
||||||
// Additional private MSP for baseflight configurator (yes thats us \o/)
|
// Additional private MSP for baseflight configurator (yes thats us \o/)
|
||||||
MSP_RX_MAP: 64, // get channel map (also returns number of channels total)
|
MSP_RX_MAP: 64, // get channel map (also returns number of channels total)
|
||||||
MSP_SET_RX_MAP: 65, // set rc map, numchannels to set comes from MSP_RX_MAP
|
MSP_SET_RX_MAP: 65, // set rc map, numchannels to set comes from MSP_RX_MAP
|
||||||
|
@ -139,14 +139,14 @@ var MSP = {
|
||||||
'230400',
|
'230400',
|
||||||
'250000',
|
'250000',
|
||||||
],
|
],
|
||||||
|
|
||||||
serialPortFunctions: {
|
serialPortFunctions: {
|
||||||
'MSP': 0,
|
'MSP': 0,
|
||||||
'GPS': 1,
|
'GPS': 1,
|
||||||
'TELEMETRY_FRSKY': 2,
|
'TELEMETRY_FRSKY': 2,
|
||||||
'TELEMETRY_HOTT': 3,
|
'TELEMETRY_HOTT': 3,
|
||||||
'TELEMETRY_MSP': 4,
|
'TELEMETRY_MSP': 4,
|
||||||
'TELEMETRY_LTM': 4, // LTM replaced MSP
|
'TELEMETRY_LTM': 4, // LTM replaced MSP
|
||||||
'TELEMETRY_SMARTPORT': 5,
|
'TELEMETRY_SMARTPORT': 5,
|
||||||
'RX_SERIAL': 6,
|
'RX_SERIAL': 6,
|
||||||
'BLACKBOX': 7,
|
'BLACKBOX': 7,
|
||||||
|
@ -351,12 +351,19 @@ var MSP = {
|
||||||
RC_tuning.roll_pitch_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.roll_pitch_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
RC_tuning.pitch_rate = 0;
|
RC_tuning.pitch_rate = 0;
|
||||||
RC_tuning.roll_rate = 0;
|
RC_tuning.roll_rate = 0;
|
||||||
|
RC_tuning.yaw_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
|
} else if (FC.isRatesInDps()) {
|
||||||
|
RC_tuning.roll_pitch_rate = 0;
|
||||||
|
RC_tuning.roll_rate = parseFloat((data.getUint8(offset++) * 10));
|
||||||
|
RC_tuning.pitch_rate = parseFloat((data.getUint8(offset++) * 10));
|
||||||
|
RC_tuning.yaw_rate = parseFloat((data.getUint8(offset++) * 10));
|
||||||
} else {
|
} else {
|
||||||
RC_tuning.roll_pitch_rate = 0;
|
RC_tuning.roll_pitch_rate = 0;
|
||||||
RC_tuning.roll_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.roll_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
RC_tuning.pitch_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.pitch_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
|
RC_tuning.yaw_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
}
|
}
|
||||||
RC_tuning.yaw_rate = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
|
||||||
RC_tuning.dynamic_THR_PID = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.dynamic_THR_PID = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
RC_tuning.throttle_MID = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.throttle_MID = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
RC_tuning.throttle_EXPO = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
RC_tuning.throttle_EXPO = parseFloat((data.getUint8(offset++) / 100).toFixed(2));
|
||||||
|
@ -456,7 +463,7 @@ var MSP = {
|
||||||
_3D.deadband3d_high = data.getUint16(offset, 1);
|
_3D.deadband3d_high = data.getUint16(offset, 1);
|
||||||
offset += 2;
|
offset += 2;
|
||||||
_3D.neutral3d = data.getUint16(offset, 1);
|
_3D.neutral3d = data.getUint16(offset, 1);
|
||||||
|
|
||||||
if (semver.lt(CONFIG.apiVersion, "1.17.0")) {
|
if (semver.lt(CONFIG.apiVersion, "1.17.0")) {
|
||||||
offset += 2;
|
offset += 2;
|
||||||
_3D.deadband3d_throttle = data.getUint16(offset, 1);
|
_3D.deadband3d_throttle = data.getUint16(offset, 1);
|
||||||
|
@ -545,12 +552,12 @@ var MSP = {
|
||||||
SERVO_CONFIG.push(arr);
|
SERVO_CONFIG.push(arr);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if (semver.eq(CONFIG.apiVersion, '1.10.0')) {
|
if (semver.eq(CONFIG.apiVersion, '1.10.0')) {
|
||||||
// drop two unused servo configurations due to MSP rx buffer to small)
|
// drop two unused servo configurations due to MSP rx buffer to small)
|
||||||
while (SERVO_CONFIG.length > 8) {
|
while (SERVO_CONFIG.length > 8) {
|
||||||
SERVO_CONFIG.pop();
|
SERVO_CONFIG.pop();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -652,12 +659,12 @@ var MSP = {
|
||||||
break;
|
break;
|
||||||
|
|
||||||
//
|
//
|
||||||
// Cleanflight specific
|
// Cleanflight specific
|
||||||
//
|
//
|
||||||
|
|
||||||
case MSP_codes.MSP_API_VERSION:
|
case MSP_codes.MSP_API_VERSION:
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
CONFIG.mspProtocolVersion = data.getUint8(offset++);
|
CONFIG.mspProtocolVersion = data.getUint8(offset++);
|
||||||
CONFIG.apiVersion = data.getUint8(offset++) + '.' + data.getUint8(offset++) + '.0';
|
CONFIG.apiVersion = data.getUint8(offset++) + '.' + data.getUint8(offset++) + '.0';
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -677,14 +684,14 @@ var MSP = {
|
||||||
|
|
||||||
case MSP_codes.MSP_BUILD_INFO:
|
case MSP_codes.MSP_BUILD_INFO:
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
|
|
||||||
var dateLength = 11;
|
var dateLength = 11;
|
||||||
var buff = [];
|
var buff = [];
|
||||||
for (var i = 0; i < dateLength; i++) {
|
for (var i = 0; i < dateLength; i++) {
|
||||||
buff.push(data.getUint8(offset++));
|
buff.push(data.getUint8(offset++));
|
||||||
}
|
}
|
||||||
buff.push(32); // ascii space
|
buff.push(32); // ascii space
|
||||||
|
|
||||||
var timeLength = 8;
|
var timeLength = 8;
|
||||||
for (var i = 0; i < timeLength; i++) {
|
for (var i = 0; i < timeLength; i++) {
|
||||||
buff.push(data.getUint8(offset++));
|
buff.push(data.getUint8(offset++));
|
||||||
|
@ -708,7 +715,7 @@ var MSP = {
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_codes.MSP_CF_SERIAL_CONFIG:
|
case MSP_codes.MSP_CF_SERIAL_CONFIG:
|
||||||
|
|
||||||
if (semver.lt(CONFIG.apiVersion, "1.6.0")) {
|
if (semver.lt(CONFIG.apiVersion, "1.6.0")) {
|
||||||
SERIAL_CONFIG.ports = [];
|
SERIAL_CONFIG.ports = [];
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
|
@ -718,7 +725,7 @@ var MSP = {
|
||||||
identifier: data.getUint8(offset++, 1),
|
identifier: data.getUint8(offset++, 1),
|
||||||
scenario: data.getUint8(offset++, 1)
|
scenario: data.getUint8(offset++, 1)
|
||||||
}
|
}
|
||||||
SERIAL_CONFIG.ports.push(serialPort);
|
SERIAL_CONFIG.ports.push(serialPort);
|
||||||
}
|
}
|
||||||
SERIAL_CONFIG.mspBaudRate = data.getUint32(offset, 1);
|
SERIAL_CONFIG.mspBaudRate = data.getUint32(offset, 1);
|
||||||
offset+= 4;
|
offset+= 4;
|
||||||
|
@ -733,7 +740,7 @@ var MSP = {
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
var bytesPerPort = 1 + 2 + (1 * 4);
|
var bytesPerPort = 1 + 2 + (1 * 4);
|
||||||
var serialPortCount = data.byteLength / bytesPerPort;
|
var serialPortCount = data.byteLength / bytesPerPort;
|
||||||
|
|
||||||
for (var i = 0; i < serialPortCount; i++) {
|
for (var i = 0; i < serialPortCount; i++) {
|
||||||
var serialPort = {
|
var serialPort = {
|
||||||
identifier: data.getUint8(offset, 1),
|
identifier: data.getUint8(offset, 1),
|
||||||
|
@ -743,7 +750,7 @@ var MSP = {
|
||||||
telemetry_baudrate: MSP.supportedBaudRates[data.getUint8(offset + 5, 1)],
|
telemetry_baudrate: MSP.supportedBaudRates[data.getUint8(offset + 5, 1)],
|
||||||
blackbox_baudrate: MSP.supportedBaudRates[data.getUint8(offset + 6, 1)]
|
blackbox_baudrate: MSP.supportedBaudRates[data.getUint8(offset + 6, 1)]
|
||||||
}
|
}
|
||||||
|
|
||||||
offset += bytesPerPort;
|
offset += bytesPerPort;
|
||||||
SERIAL_CONFIG.ports.push(serialPort);
|
SERIAL_CONFIG.ports.push(serialPort);
|
||||||
}
|
}
|
||||||
|
@ -758,7 +765,7 @@ var MSP = {
|
||||||
MODE_RANGES = []; // empty the array as new data is coming in
|
MODE_RANGES = []; // empty the array as new data is coming in
|
||||||
|
|
||||||
var modeRangeCount = data.byteLength / 4; // 4 bytes per item.
|
var modeRangeCount = data.byteLength / 4; // 4 bytes per item.
|
||||||
|
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
for (var i = 0; offset < data.byteLength && i < modeRangeCount; i++) {
|
for (var i = 0; offset < data.byteLength && i < modeRangeCount; i++) {
|
||||||
var modeRange = {
|
var modeRange = {
|
||||||
|
@ -777,7 +784,7 @@ var MSP = {
|
||||||
ADJUSTMENT_RANGES = []; // empty the array as new data is coming in
|
ADJUSTMENT_RANGES = []; // empty the array as new data is coming in
|
||||||
|
|
||||||
var adjustmentRangeCount = data.byteLength / 6; // 6 bytes per item.
|
var adjustmentRangeCount = data.byteLength / 6; // 6 bytes per item.
|
||||||
|
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
for (var i = 0; offset < data.byteLength && i < adjustmentRangeCount; i++) {
|
for (var i = 0; offset < data.byteLength && i < adjustmentRangeCount; i++) {
|
||||||
var adjustmentRange = {
|
var adjustmentRange = {
|
||||||
|
@ -859,15 +866,15 @@ var MSP = {
|
||||||
|
|
||||||
case MSP_codes.MSP_LED_STRIP_CONFIG:
|
case MSP_codes.MSP_LED_STRIP_CONFIG:
|
||||||
LED_STRIP = [];
|
LED_STRIP = [];
|
||||||
|
|
||||||
var ledCount = data.byteLength / 7; // v1.4.0 and below incorrectly reported 4 bytes per led.
|
var ledCount = data.byteLength / 7; // v1.4.0 and below incorrectly reported 4 bytes per led.
|
||||||
|
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
for (var i = 0; offset < data.byteLength && i < ledCount; i++) {
|
for (var i = 0; offset < data.byteLength && i < ledCount; i++) {
|
||||||
|
|
||||||
var directionMask = data.getUint16(offset, 1);
|
var directionMask = data.getUint16(offset, 1);
|
||||||
offset += 2;
|
offset += 2;
|
||||||
|
|
||||||
var directions = [];
|
var directions = [];
|
||||||
for (var directionLetterIndex = 0; directionLetterIndex < MSP.ledDirectionLetters.length; directionLetterIndex++) {
|
for (var directionLetterIndex = 0; directionLetterIndex < MSP.ledDirectionLetters.length; directionLetterIndex++) {
|
||||||
if (bit_check(directionMask, directionLetterIndex)) {
|
if (bit_check(directionMask, directionLetterIndex)) {
|
||||||
|
@ -884,7 +891,7 @@ var MSP = {
|
||||||
functions.push(MSP.ledFunctionLetters[functionLetterIndex]);
|
functions.push(MSP.ledFunctionLetters[functionLetterIndex]);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
var led = {
|
var led = {
|
||||||
directions: directions,
|
directions: directions,
|
||||||
functions: functions,
|
functions: functions,
|
||||||
|
@ -892,10 +899,10 @@ var MSP = {
|
||||||
y: data.getUint8(offset++, 1),
|
y: data.getUint8(offset++, 1),
|
||||||
color: data.getUint8(offset++, 1)
|
color: data.getUint8(offset++, 1)
|
||||||
};
|
};
|
||||||
|
|
||||||
LED_STRIP.push(led);
|
LED_STRIP.push(led);
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_SET_LED_STRIP_CONFIG:
|
case MSP_codes.MSP_SET_LED_STRIP_CONFIG:
|
||||||
console.log('Led strip config saved');
|
console.log('Led strip config saved');
|
||||||
|
@ -926,8 +933,8 @@ var MSP = {
|
||||||
console.log("Data flash erase begun...");
|
console.log("Data flash erase begun...");
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_SDCARD_SUMMARY:
|
case MSP_codes.MSP_SDCARD_SUMMARY:
|
||||||
var flags = data.getUint8(0);
|
var flags = data.getUint8(0);
|
||||||
|
|
||||||
SDCARD.supported = (flags & 0x01) != 0;
|
SDCARD.supported = (flags & 0x01) != 0;
|
||||||
SDCARD.state = data.getUint8(1);
|
SDCARD.state = data.getUint8(1);
|
||||||
SDCARD.filesystemLastError = data.getUint8(2);
|
SDCARD.filesystemLastError = data.getUint8(2);
|
||||||
|
@ -947,7 +954,7 @@ var MSP = {
|
||||||
var offset = 0;
|
var offset = 0;
|
||||||
TRANSPONDER.supported = (data.getUint8(offset++) & 1) != 0;
|
TRANSPONDER.supported = (data.getUint8(offset++) & 1) != 0;
|
||||||
TRANSPONDER.data = [];
|
TRANSPONDER.data = [];
|
||||||
var bytesRemaining = data.byteLength - offset;
|
var bytesRemaining = data.byteLength - offset;
|
||||||
for (var i = 0; i < bytesRemaining; i++) {
|
for (var i = 0; i < bytesRemaining; i++) {
|
||||||
TRANSPONDER.data.push(data.getUint8(offset++));
|
TRANSPONDER.data.push(data.getUint8(offset++));
|
||||||
}
|
}
|
||||||
|
@ -961,7 +968,7 @@ var MSP = {
|
||||||
case MSP_codes.MSP_SET_ADJUSTMENT_RANGE:
|
case MSP_codes.MSP_SET_ADJUSTMENT_RANGE:
|
||||||
console.log('Adjustment range saved');
|
console.log('Adjustment range saved');
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_codes.MSP_PID_CONTROLLER:
|
case MSP_codes.MSP_PID_CONTROLLER:
|
||||||
PID.controller = data.getUint8(0, 1);
|
PID.controller = data.getUint8(0, 1);
|
||||||
break;
|
break;
|
||||||
|
@ -985,7 +992,7 @@ var MSP = {
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_SET_SENSOR_ALIGNMENT:
|
case MSP_codes.MSP_SET_SENSOR_ALIGNMENT:
|
||||||
console.log('Sensor alignment saved');
|
console.log('Sensor alignment saved');
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_SET_RX_CONFIG:
|
case MSP_codes.MSP_SET_RX_CONFIG:
|
||||||
console.log('Rx config saved');
|
console.log('Rx config saved');
|
||||||
break;
|
break;
|
||||||
|
@ -1168,11 +1175,17 @@ MSP.crunch = function (code) {
|
||||||
buffer.push(Math.round(RC_tuning.RC_EXPO * 100));
|
buffer.push(Math.round(RC_tuning.RC_EXPO * 100));
|
||||||
if (semver.lt(CONFIG.apiVersion, "1.7.0")) {
|
if (semver.lt(CONFIG.apiVersion, "1.7.0")) {
|
||||||
buffer.push(Math.round(RC_tuning.roll_pitch_rate * 100));
|
buffer.push(Math.round(RC_tuning.roll_pitch_rate * 100));
|
||||||
|
buffer.push(Math.round(RC_tuning.yaw_rate * 100));
|
||||||
|
} else if (FC.isRatesInDps()) {
|
||||||
|
buffer.push(Math.round(RC_tuning.roll_rate / 10));
|
||||||
|
buffer.push(Math.round(RC_tuning.pitch_rate / 10));
|
||||||
|
buffer.push(Math.round(RC_tuning.yaw_rate / 10));
|
||||||
} else {
|
} else {
|
||||||
buffer.push(Math.round(RC_tuning.roll_rate * 100));
|
buffer.push(Math.round(RC_tuning.roll_rate * 100));
|
||||||
buffer.push(Math.round(RC_tuning.pitch_rate * 100));
|
buffer.push(Math.round(RC_tuning.pitch_rate * 100));
|
||||||
|
buffer.push(Math.round(RC_tuning.yaw_rate * 100));
|
||||||
}
|
}
|
||||||
buffer.push(Math.round(RC_tuning.yaw_rate * 100));
|
|
||||||
buffer.push(Math.round(RC_tuning.dynamic_THR_PID * 100));
|
buffer.push(Math.round(RC_tuning.dynamic_THR_PID * 100));
|
||||||
buffer.push(Math.round(RC_tuning.throttle_MID * 100));
|
buffer.push(Math.round(RC_tuning.throttle_MID * 100));
|
||||||
buffer.push(Math.round(RC_tuning.throttle_EXPO * 100));
|
buffer.push(Math.round(RC_tuning.throttle_EXPO * 100));
|
||||||
|
@ -1290,17 +1303,17 @@ MSP.crunch = function (code) {
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 1));
|
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 1));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 2));
|
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 2));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 3));
|
buffer.push(specificByte(SERIAL_CONFIG.mspBaudRate, 3));
|
||||||
|
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 0));
|
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 0));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 1));
|
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 1));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 2));
|
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 2));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 3));
|
buffer.push(specificByte(SERIAL_CONFIG.cliBaudRate, 3));
|
||||||
|
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 0));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 0));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 1));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 1));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 2));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 2));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 3));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsBaudRate, 3));
|
||||||
|
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 0));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 0));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 1));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 1));
|
||||||
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 2));
|
buffer.push(specificByte(SERIAL_CONFIG.gpsPassthroughBaudRate, 2));
|
||||||
|
@ -1308,13 +1321,13 @@ MSP.crunch = function (code) {
|
||||||
} else {
|
} else {
|
||||||
for (var i = 0; i < SERIAL_CONFIG.ports.length; i++) {
|
for (var i = 0; i < SERIAL_CONFIG.ports.length; i++) {
|
||||||
var serialPort = SERIAL_CONFIG.ports[i];
|
var serialPort = SERIAL_CONFIG.ports[i];
|
||||||
|
|
||||||
buffer.push(serialPort.identifier);
|
buffer.push(serialPort.identifier);
|
||||||
|
|
||||||
var functionMask = MSP.serialPortFunctionsToMask(serialPort.functions);
|
var functionMask = MSP.serialPortFunctionsToMask(serialPort.functions);
|
||||||
buffer.push(specificByte(functionMask, 0));
|
buffer.push(specificByte(functionMask, 0));
|
||||||
buffer.push(specificByte(functionMask, 1));
|
buffer.push(specificByte(functionMask, 1));
|
||||||
|
|
||||||
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.msp_baudrate));
|
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.msp_baudrate));
|
||||||
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.gps_baudrate));
|
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.gps_baudrate));
|
||||||
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.telemetry_baudrate));
|
buffer.push(MSP.supportedBaudRates.indexOf(serialPort.telemetry_baudrate));
|
||||||
|
@ -1334,11 +1347,11 @@ MSP.crunch = function (code) {
|
||||||
buffer.push(lowByte(_3D.deadband3d_throttle));
|
buffer.push(lowByte(_3D.deadband3d_throttle));
|
||||||
buffer.push(highByte(_3D.deadband3d_throttle));
|
buffer.push(highByte(_3D.deadband3d_throttle));
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_codes.MSP_SET_RC_DEADBAND:
|
case MSP_codes.MSP_SET_RC_DEADBAND:
|
||||||
buffer.push(RC_deadband.deadband);
|
buffer.push(RC_deadband.deadband);
|
||||||
buffer.push(RC_deadband.yaw_deadband);
|
buffer.push(RC_deadband.yaw_deadband);
|
||||||
buffer.push(RC_deadband.alt_hold_deadband);
|
buffer.push(RC_deadband.alt_hold_deadband);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
@ -1357,28 +1370,28 @@ MSP.crunch = function (code) {
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Set raw Rx values over MSP protocol.
|
* Set raw Rx values over MSP protocol.
|
||||||
*
|
*
|
||||||
* Channels is an array of 16-bit unsigned integer channel values to be sent. 8 channels is probably the maximum.
|
* Channels is an array of 16-bit unsigned integer channel values to be sent. 8 channels is probably the maximum.
|
||||||
*/
|
*/
|
||||||
MSP.setRawRx = function(channels) {
|
MSP.setRawRx = function(channels) {
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
|
|
||||||
for (var i = 0; i < channels.length; i++) {
|
for (var i = 0; i < channels.length; i++) {
|
||||||
buffer.push(specificByte(channels[i], 0));
|
buffer.push(specificByte(channels[i], 0));
|
||||||
buffer.push(specificByte(channels[i], 1));
|
buffer.push(specificByte(channels[i], 1));
|
||||||
}
|
}
|
||||||
|
|
||||||
MSP.send_message(MSP_codes.MSP_SET_RAW_RC, buffer, false);
|
MSP.send_message(MSP_codes.MSP_SET_RAW_RC, buffer, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
MSP.sendBlackboxConfiguration = function(onDataCallback) {
|
MSP.sendBlackboxConfiguration = function(onDataCallback) {
|
||||||
var
|
var
|
||||||
message = [
|
message = [
|
||||||
BLACKBOX.blackboxDevice & 0xFF,
|
BLACKBOX.blackboxDevice & 0xFF,
|
||||||
BLACKBOX.blackboxRateNum & 0xFF,
|
BLACKBOX.blackboxRateNum & 0xFF,
|
||||||
BLACKBOX.blackboxRateDenom & 0xFF
|
BLACKBOX.blackboxRateDenom & 0xFF
|
||||||
];
|
];
|
||||||
|
|
||||||
MSP.send_message(MSP_codes.MSP_SET_BLACKBOX_CONFIG, message, false, function(response) {
|
MSP.send_message(MSP_codes.MSP_SET_BLACKBOX_CONFIG, message, false, function(response) {
|
||||||
onDataCallback();
|
onDataCallback();
|
||||||
});
|
});
|
||||||
|
@ -1389,10 +1402,10 @@ MSP.sendBlackboxConfiguration = function(onDataCallback) {
|
||||||
* of the returned data to the given callback (or null for the data if an error occured).
|
* of the returned data to the given callback (or null for the data if an error occured).
|
||||||
*/
|
*/
|
||||||
MSP.dataflashRead = function(address, onDataCallback) {
|
MSP.dataflashRead = function(address, onDataCallback) {
|
||||||
MSP.send_message(MSP_codes.MSP_DATAFLASH_READ, [address & 0xFF, (address >> 8) & 0xFF, (address >> 16) & 0xFF, (address >> 24) & 0xFF],
|
MSP.send_message(MSP_codes.MSP_DATAFLASH_READ, [address & 0xFF, (address >> 8) & 0xFF, (address >> 16) & 0xFF, (address >> 24) & 0xFF],
|
||||||
false, function(response) {
|
false, function(response) {
|
||||||
var chunkAddress = response.data.getUint32(0, 1);
|
var chunkAddress = response.data.getUint32(0, 1);
|
||||||
|
|
||||||
// Verify that the address of the memory returned matches what the caller asked for
|
// Verify that the address of the memory returned matches what the caller asked for
|
||||||
if (chunkAddress == address) {
|
if (chunkAddress == address) {
|
||||||
/* Strip that address off the front of the reply and deliver it separately so the caller doesn't have to
|
/* Strip that address off the front of the reply and deliver it separately so the caller doesn't have to
|
||||||
|
@ -1412,8 +1425,8 @@ MSP.sendServoMixRules = function(onCompleteCallback) {
|
||||||
};
|
};
|
||||||
|
|
||||||
MSP.sendServoConfigurations = function(onCompleteCallback) {
|
MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
var nextFunction = send_next_servo_configuration;
|
var nextFunction = send_next_servo_configuration;
|
||||||
|
|
||||||
var servoIndex = 0;
|
var servoIndex = 0;
|
||||||
|
|
||||||
if (SERVO_CONFIG.length == 0) {
|
if (SERVO_CONFIG.length == 0) {
|
||||||
|
@ -1424,7 +1437,7 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
|
|
||||||
|
|
||||||
function send_next_servo_configuration() {
|
function send_next_servo_configuration() {
|
||||||
|
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
|
|
||||||
if (semver.lt(CONFIG.apiVersion, "1.12.0")) {
|
if (semver.lt(CONFIG.apiVersion, "1.12.0")) {
|
||||||
|
@ -1442,15 +1455,15 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
|
|
||||||
buffer.push(lowByte(SERVO_CONFIG[i].rate));
|
buffer.push(lowByte(SERVO_CONFIG[i].rate));
|
||||||
}
|
}
|
||||||
|
|
||||||
nextFunction = send_channel_forwarding;
|
nextFunction = send_channel_forwarding;
|
||||||
} else {
|
} else {
|
||||||
// send one at a time, with index
|
// send one at a time, with index
|
||||||
|
|
||||||
var servoConfiguration = SERVO_CONFIG[servoIndex];
|
var servoConfiguration = SERVO_CONFIG[servoIndex];
|
||||||
|
|
||||||
buffer.push(servoIndex);
|
buffer.push(servoIndex);
|
||||||
|
|
||||||
buffer.push(lowByte(servoConfiguration.min));
|
buffer.push(lowByte(servoConfiguration.min));
|
||||||
buffer.push(highByte(servoConfiguration.min));
|
buffer.push(highByte(servoConfiguration.min));
|
||||||
|
|
||||||
|
@ -1461,7 +1474,7 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
buffer.push(highByte(servoConfiguration.middle));
|
buffer.push(highByte(servoConfiguration.middle));
|
||||||
|
|
||||||
buffer.push(lowByte(servoConfiguration.rate));
|
buffer.push(lowByte(servoConfiguration.rate));
|
||||||
|
|
||||||
buffer.push(servoConfiguration.angleAtMin);
|
buffer.push(servoConfiguration.angleAtMin);
|
||||||
buffer.push(servoConfiguration.angleAtMax);
|
buffer.push(servoConfiguration.angleAtMax);
|
||||||
|
|
||||||
|
@ -1475,7 +1488,7 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
buffer.push(specificByte(servoConfiguration.reversedInputSources, 1));
|
buffer.push(specificByte(servoConfiguration.reversedInputSources, 1));
|
||||||
buffer.push(specificByte(servoConfiguration.reversedInputSources, 2));
|
buffer.push(specificByte(servoConfiguration.reversedInputSources, 2));
|
||||||
buffer.push(specificByte(servoConfiguration.reversedInputSources, 3));
|
buffer.push(specificByte(servoConfiguration.reversedInputSources, 3));
|
||||||
|
|
||||||
// prepare for next iteration
|
// prepare for next iteration
|
||||||
servoIndex++;
|
servoIndex++;
|
||||||
if (servoIndex == SERVO_CONFIG.length) {
|
if (servoIndex == SERVO_CONFIG.length) {
|
||||||
|
@ -1484,7 +1497,7 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
}
|
}
|
||||||
MSP.send_message(MSP_codes.MSP_SET_SERVO_CONFIGURATION, buffer, false, nextFunction);
|
MSP.send_message(MSP_codes.MSP_SET_SERVO_CONFIGURATION, buffer, false, nextFunction);
|
||||||
}
|
}
|
||||||
|
|
||||||
function send_channel_forwarding() {
|
function send_channel_forwarding() {
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
|
|
||||||
|
@ -1503,8 +1516,8 @@ MSP.sendServoConfigurations = function(onCompleteCallback) {
|
||||||
};
|
};
|
||||||
|
|
||||||
MSP.sendModeRanges = function(onCompleteCallback) {
|
MSP.sendModeRanges = function(onCompleteCallback) {
|
||||||
var nextFunction = send_next_mode_range;
|
var nextFunction = send_next_mode_range;
|
||||||
|
|
||||||
var modeRangeIndex = 0;
|
var modeRangeIndex = 0;
|
||||||
|
|
||||||
if (MODE_RANGES.length == 0) {
|
if (MODE_RANGES.length == 0) {
|
||||||
|
@ -1514,29 +1527,29 @@ MSP.sendModeRanges = function(onCompleteCallback) {
|
||||||
}
|
}
|
||||||
|
|
||||||
function send_next_mode_range() {
|
function send_next_mode_range() {
|
||||||
|
|
||||||
var modeRange = MODE_RANGES[modeRangeIndex];
|
var modeRange = MODE_RANGES[modeRangeIndex];
|
||||||
|
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
buffer.push(modeRangeIndex);
|
buffer.push(modeRangeIndex);
|
||||||
buffer.push(modeRange.id);
|
buffer.push(modeRange.id);
|
||||||
buffer.push(modeRange.auxChannelIndex);
|
buffer.push(modeRange.auxChannelIndex);
|
||||||
buffer.push((modeRange.range.start - 900) / 25);
|
buffer.push((modeRange.range.start - 900) / 25);
|
||||||
buffer.push((modeRange.range.end - 900) / 25);
|
buffer.push((modeRange.range.end - 900) / 25);
|
||||||
|
|
||||||
// prepare for next iteration
|
// prepare for next iteration
|
||||||
modeRangeIndex++;
|
modeRangeIndex++;
|
||||||
if (modeRangeIndex == MODE_RANGES.length) {
|
if (modeRangeIndex == MODE_RANGES.length) {
|
||||||
nextFunction = onCompleteCallback;
|
nextFunction = onCompleteCallback;
|
||||||
|
|
||||||
}
|
}
|
||||||
MSP.send_message(MSP_codes.MSP_SET_MODE_RANGE, buffer, false, nextFunction);
|
MSP.send_message(MSP_codes.MSP_SET_MODE_RANGE, buffer, false, nextFunction);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
MSP.sendAdjustmentRanges = function(onCompleteCallback) {
|
MSP.sendAdjustmentRanges = function(onCompleteCallback) {
|
||||||
var nextFunction = send_next_adjustment_range;
|
var nextFunction = send_next_adjustment_range;
|
||||||
|
|
||||||
var adjustmentRangeIndex = 0;
|
var adjustmentRangeIndex = 0;
|
||||||
|
|
||||||
if (ADJUSTMENT_RANGES.length == 0) {
|
if (ADJUSTMENT_RANGES.length == 0) {
|
||||||
|
@ -1547,9 +1560,9 @@ MSP.sendAdjustmentRanges = function(onCompleteCallback) {
|
||||||
|
|
||||||
|
|
||||||
function send_next_adjustment_range() {
|
function send_next_adjustment_range() {
|
||||||
|
|
||||||
var adjustmentRange = ADJUSTMENT_RANGES[adjustmentRangeIndex];
|
var adjustmentRange = ADJUSTMENT_RANGES[adjustmentRangeIndex];
|
||||||
|
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
buffer.push(adjustmentRangeIndex);
|
buffer.push(adjustmentRangeIndex);
|
||||||
buffer.push(adjustmentRange.slotIndex);
|
buffer.push(adjustmentRange.slotIndex);
|
||||||
|
@ -1558,21 +1571,21 @@ MSP.sendAdjustmentRanges = function(onCompleteCallback) {
|
||||||
buffer.push((adjustmentRange.range.end - 900) / 25);
|
buffer.push((adjustmentRange.range.end - 900) / 25);
|
||||||
buffer.push(adjustmentRange.adjustmentFunction);
|
buffer.push(adjustmentRange.adjustmentFunction);
|
||||||
buffer.push(adjustmentRange.auxSwitchChannelIndex);
|
buffer.push(adjustmentRange.auxSwitchChannelIndex);
|
||||||
|
|
||||||
// prepare for next iteration
|
// prepare for next iteration
|
||||||
adjustmentRangeIndex++;
|
adjustmentRangeIndex++;
|
||||||
if (adjustmentRangeIndex == ADJUSTMENT_RANGES.length) {
|
if (adjustmentRangeIndex == ADJUSTMENT_RANGES.length) {
|
||||||
nextFunction = onCompleteCallback;
|
nextFunction = onCompleteCallback;
|
||||||
|
|
||||||
}
|
}
|
||||||
MSP.send_message(MSP_codes.MSP_SET_ADJUSTMENT_RANGE, buffer, false, nextFunction);
|
MSP.send_message(MSP_codes.MSP_SET_ADJUSTMENT_RANGE, buffer, false, nextFunction);
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
MSP.sendLedStripConfig = function(onCompleteCallback) {
|
MSP.sendLedStripConfig = function(onCompleteCallback) {
|
||||||
|
|
||||||
var nextFunction = send_next_led_strip_config;
|
var nextFunction = send_next_led_strip_config;
|
||||||
|
|
||||||
var ledIndex = 0;
|
var ledIndex = 0;
|
||||||
|
|
||||||
if (LED_STRIP.length == 0) {
|
if (LED_STRIP.length == 0) {
|
||||||
|
@ -1582,11 +1595,11 @@ MSP.sendLedStripConfig = function(onCompleteCallback) {
|
||||||
}
|
}
|
||||||
|
|
||||||
function send_next_led_strip_config() {
|
function send_next_led_strip_config() {
|
||||||
|
|
||||||
var led = LED_STRIP[ledIndex];
|
var led = LED_STRIP[ledIndex];
|
||||||
|
|
||||||
var buffer = [];
|
var buffer = [];
|
||||||
|
|
||||||
buffer.push(ledIndex);
|
buffer.push(ledIndex);
|
||||||
|
|
||||||
var directionMask = 0;
|
var directionMask = 0;
|
||||||
|
@ -1614,20 +1627,20 @@ MSP.sendLedStripConfig = function(onCompleteCallback) {
|
||||||
|
|
||||||
buffer.push(led.color);
|
buffer.push(led.color);
|
||||||
|
|
||||||
|
|
||||||
// prepare for next iteration
|
// prepare for next iteration
|
||||||
ledIndex++;
|
ledIndex++;
|
||||||
if (ledIndex == LED_STRIP.length) {
|
if (ledIndex == LED_STRIP.length) {
|
||||||
nextFunction = onCompleteCallback;
|
nextFunction = onCompleteCallback;
|
||||||
}
|
}
|
||||||
|
|
||||||
MSP.send_message(MSP_codes.MSP_SET_LED_STRIP_CONFIG, buffer, false, nextFunction);
|
MSP.send_message(MSP_codes.MSP_SET_LED_STRIP_CONFIG, buffer, false, nextFunction);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
MSP.serialPortFunctionMaskToFunctions = function(functionMask) {
|
MSP.serialPortFunctionMaskToFunctions = function(functionMask) {
|
||||||
var functions = [];
|
var functions = [];
|
||||||
|
|
||||||
var keys = Object.keys(MSP.serialPortFunctions);
|
var keys = Object.keys(MSP.serialPortFunctions);
|
||||||
for (var index = 0; index < keys.length; index++) {
|
for (var index = 0; index < keys.length; index++) {
|
||||||
var key = keys[index];
|
var key = keys[index];
|
||||||
|
@ -1688,5 +1701,3 @@ MSP.SDCARD_STATE_FATAL = 1;
|
||||||
MSP.SDCARD_STATE_CARD_INIT = 2;
|
MSP.SDCARD_STATE_CARD_INIT = 2;
|
||||||
MSP.SDCARD_STATE_FS_INIT = 3;
|
MSP.SDCARD_STATE_FS_INIT = 3;
|
||||||
MSP.SDCARD_STATE_READY = 4;
|
MSP.SDCARD_STATE_READY = 4;
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -1,3 +1,25 @@
|
||||||
|
.rate-tpa.rate-tpa--inav input[type="number"] {
|
||||||
|
margin: 4px;
|
||||||
|
width: auto;
|
||||||
|
border: 1px solid silver;
|
||||||
|
border-radius: 3px;
|
||||||
|
display: inline-block;
|
||||||
|
}
|
||||||
|
|
||||||
|
.rate-tpa.rate-tpa--inav td {
|
||||||
|
background-color: #DEDEDE;
|
||||||
|
width: auto;
|
||||||
|
border-bottom: 1px solid #ccc;
|
||||||
|
}
|
||||||
|
|
||||||
|
.tab-pid_tuning .rate-tpa.rate-tpa--inav th:first-child {
|
||||||
|
border-top-left-radius: 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
.tab-pid_tuning .rate-tpa.rate-tpa--inav .roll {
|
||||||
|
border-bottom-left-radius: 0;
|
||||||
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .rate-tpa th {
|
.tab-pid_tuning .rate-tpa th {
|
||||||
background-color: #828885;
|
background-color: #828885;
|
||||||
padding: 4px;
|
padding: 4px;
|
||||||
|
@ -153,7 +175,7 @@
|
||||||
font-weight: normal;
|
font-weight: normal;
|
||||||
padding: 2px;
|
padding: 2px;
|
||||||
padding-left: 6px;
|
padding-left: 6px;
|
||||||
border-top-left-radius: 3px;
|
border-top-left-radius: 3px;
|
||||||
border-top-right-radius: 3px;
|
border-top-right-radius: 3px;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -258,11 +280,11 @@
|
||||||
}
|
}
|
||||||
|
|
||||||
.show {
|
.show {
|
||||||
width:110px;
|
width:110px;
|
||||||
float:right;
|
float:right;
|
||||||
margin-right:3px;
|
margin-right:3px;
|
||||||
}
|
}
|
||||||
|
|
||||||
.show a {
|
.show a {
|
||||||
margin-left: 10px;
|
margin-left: 10px;
|
||||||
width: calc(100% - 10px);
|
width: calc(100% - 10px);
|
||||||
|
@ -282,7 +304,7 @@
|
||||||
margin-right: 5px;
|
margin-right: 5px;
|
||||||
}
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .spacer_left {
|
.tab-pid_tuning .spacer_left {
|
||||||
padding-left: 15px;
|
padding-left: 15px;
|
||||||
float: right;
|
float: right;
|
||||||
width: calc(100% - 20px)
|
width: calc(100% - 20px)
|
||||||
|
@ -327,7 +349,7 @@
|
||||||
color: #7d7d7d;
|
color: #7d7d7d;
|
||||||
font-size: 11px;
|
font-size: 11px;
|
||||||
}
|
}
|
||||||
|
|
||||||
#content-watermark {
|
#content-watermark {
|
||||||
position: absolute;
|
position: absolute;
|
||||||
bottom: 40px;
|
bottom: 40px;
|
||||||
|
@ -344,7 +366,7 @@
|
||||||
}
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .resetbt {
|
.tab-pid_tuning .resetbt {
|
||||||
width: 140px;
|
width: 140px;
|
||||||
float: right;
|
float: right;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -374,16 +396,16 @@
|
||||||
}
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .borderleft {
|
.tab-pid_tuning .borderleft {
|
||||||
border-top-left-radius: 3px;
|
border-top-left-radius: 3px;
|
||||||
border-top-right-radius: 3px;
|
border-top-right-radius: 3px;
|
||||||
}
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .textleft {
|
.tab-pid_tuning .textleft {
|
||||||
width: 25%;
|
width: 25%;
|
||||||
float: left;
|
float: left;
|
||||||
text-align: left;
|
text-align: left;
|
||||||
}
|
}
|
||||||
|
|
||||||
.tab-pid_tuning .topspacer {
|
.tab-pid_tuning .topspacer {
|
||||||
margin-top:15px;
|
margin-top:15px;
|
||||||
}
|
}
|
||||||
|
|
|
@ -7,7 +7,7 @@
|
||||||
</div>
|
</div>
|
||||||
<div class="cf_column half">
|
<div class="cf_column half">
|
||||||
<div class="controller">
|
<div class="controller">
|
||||||
<span class="head" i18n="pidTuningControllerHead"></span>
|
<span class="head" i18n="pidTuningControllerHead"></span>
|
||||||
<select name="controller">
|
<select name="controller">
|
||||||
<!-- list generated here -->
|
<!-- list generated here -->
|
||||||
</select>
|
</select>
|
||||||
|
@ -154,7 +154,29 @@
|
||||||
</div>
|
</div>
|
||||||
<div class="cf_column half">
|
<div class="cf_column half">
|
||||||
<div class="spacer_left">
|
<div class="spacer_left">
|
||||||
<table class="rate-tpa cf">
|
<table class="rate-tpa rate-tpa--inav">
|
||||||
|
<tbody>
|
||||||
|
<tr>
|
||||||
|
<th class="roll" i18n="pidTuningRollRate"></th>
|
||||||
|
<td class="roll">
|
||||||
|
<input type="number" name="roll" step="10" min="60" max="1800" /> degrees per second
|
||||||
|
</td>
|
||||||
|
</tr>
|
||||||
|
<tr>
|
||||||
|
<th class="pitch" i18n="pidTuningPitchRate"></th>
|
||||||
|
<td class="pitch">
|
||||||
|
<input type="number" name="pitch" step="10" min="60" max="1800" /> degrees per second
|
||||||
|
</td>
|
||||||
|
</tr>
|
||||||
|
<tr>
|
||||||
|
<th class="yaw" i18n="pidTuningYawRate"></th>
|
||||||
|
<td class="yaw">
|
||||||
|
<input type="number" name="yaw" step="10" min="20" max="1800" /> degrees per second
|
||||||
|
</td>
|
||||||
|
</tr>
|
||||||
|
</tbody>
|
||||||
|
</table>
|
||||||
|
<table class="rate-tpa rate-tpa--no-dps cf">
|
||||||
<thead>
|
<thead>
|
||||||
<tr>
|
<tr>
|
||||||
<th class="roll-pitch" i18n="pidTuningRollPitchRate"></th>
|
<th class="roll-pitch" i18n="pidTuningRollPitchRate"></th>
|
||||||
|
@ -203,4 +225,4 @@
|
||||||
<a class="refresh" href="#" i18n="pidTuningButtonRefresh"></a>
|
<a class="refresh" href="#" i18n="pidTuningButtonRefresh"></a>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
|
@ -173,9 +173,17 @@ TABS.pid_tuning.initialize = function (callback) {
|
||||||
|
|
||||||
// Fill in data from RC_tuning object
|
// Fill in data from RC_tuning object
|
||||||
$('.rate-tpa input[name="roll-pitch"]').val(RC_tuning.roll_pitch_rate.toFixed(2));
|
$('.rate-tpa input[name="roll-pitch"]').val(RC_tuning.roll_pitch_rate.toFixed(2));
|
||||||
$('.rate-tpa input[name="roll"]').val(RC_tuning.roll_rate.toFixed(2));
|
|
||||||
$('.rate-tpa input[name="pitch"]').val(RC_tuning.pitch_rate.toFixed(2));
|
if (FC.isRatesInDps()) {
|
||||||
$('.rate-tpa input[name="yaw"]').val(RC_tuning.yaw_rate.toFixed(2));
|
$('.rate-tpa input[name="roll"]').val(RC_tuning.roll_rate);
|
||||||
|
$('.rate-tpa input[name="pitch"]').val(RC_tuning.pitch_rate);
|
||||||
|
$('.rate-tpa input[name="yaw"]').val(RC_tuning.yaw_rate);
|
||||||
|
} else {
|
||||||
|
$('.rate-tpa input[name="roll"]').val(RC_tuning.roll_rate.toFixed(2));
|
||||||
|
$('.rate-tpa input[name="pitch"]').val(RC_tuning.pitch_rate.toFixed(2));
|
||||||
|
$('.rate-tpa input[name="yaw"]').val(RC_tuning.yaw_rate.toFixed(2));
|
||||||
|
}
|
||||||
|
|
||||||
$('.rate-tpa input[name="tpa"]').val(RC_tuning.dynamic_THR_PID.toFixed(2));
|
$('.rate-tpa input[name="tpa"]').val(RC_tuning.dynamic_THR_PID.toFixed(2));
|
||||||
$('.rate-tpa input[name="tpa-breakpoint"]').val(RC_tuning.dynamic_THR_breakpoint);
|
$('.rate-tpa input[name="tpa-breakpoint"]').val(RC_tuning.dynamic_THR_breakpoint);
|
||||||
}
|
}
|
||||||
|
@ -234,9 +242,9 @@ TABS.pid_tuning.initialize = function (callback) {
|
||||||
|
|
||||||
// catch RC_tuning changes
|
// catch RC_tuning changes
|
||||||
RC_tuning.roll_pitch_rate = parseFloat($('.rate-tpa input[name="roll-pitch"]').val());
|
RC_tuning.roll_pitch_rate = parseFloat($('.rate-tpa input[name="roll-pitch"]').val());
|
||||||
RC_tuning.roll_rate = parseFloat($('.rate-tpa input[name="roll"]').val());
|
RC_tuning.roll_rate = parseFloat($('.rate-tpa input[name="roll"]:visible').val());
|
||||||
RC_tuning.pitch_rate = parseFloat($('.rate-tpa input[name="pitch"]').val());
|
RC_tuning.pitch_rate = parseFloat($('.rate-tpa input[name="pitch"]:visible').val());
|
||||||
RC_tuning.yaw_rate = parseFloat($('.rate-tpa input[name="yaw"]').val());
|
RC_tuning.yaw_rate = parseFloat($('.rate-tpa input[name="yaw"]:visible').val());
|
||||||
RC_tuning.dynamic_THR_PID = parseFloat($('.rate-tpa input[name="tpa"]').val());
|
RC_tuning.dynamic_THR_PID = parseFloat($('.rate-tpa input[name="tpa"]').val());
|
||||||
RC_tuning.dynamic_THR_breakpoint = parseInt($('.rate-tpa input[name="tpa-breakpoint"]').val());
|
RC_tuning.dynamic_THR_breakpoint = parseInt($('.rate-tpa input[name="tpa-breakpoint"]').val());
|
||||||
}
|
}
|
||||||
|
@ -313,12 +321,12 @@ TABS.pid_tuning.initialize = function (callback) {
|
||||||
{ name: "LuxFloat"},
|
{ name: "LuxFloat"},
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
|
|
||||||
for (var i = 0; i < pidControllerList.length; i++) {
|
for (var i = 0; i < pidControllerList.length; i++) {
|
||||||
pidController_e.append('<option value="' + (i) + '">' + pidControllerList[i].name + '</option>');
|
pidController_e.append('<option value="' + (i) + '">' + pidControllerList[i].name + '</option>');
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
var form_e = $('#pid-tuning');
|
var form_e = $('#pid-tuning');
|
||||||
|
|
||||||
if (GUI.canChangePidController) {
|
if (GUI.canChangePidController) {
|
||||||
|
@ -336,8 +344,12 @@ TABS.pid_tuning.initialize = function (callback) {
|
||||||
$('.rate-tpa .tpa-breakpoint').hide();
|
$('.rate-tpa .tpa-breakpoint').hide();
|
||||||
$('.rate-tpa .roll').hide();
|
$('.rate-tpa .roll').hide();
|
||||||
$('.rate-tpa .pitch').hide();
|
$('.rate-tpa .pitch').hide();
|
||||||
|
$('.rate-tpa--inav').hide();
|
||||||
|
} else if (FC.isRatesInDps()) {
|
||||||
|
$('.rate-tpa--no-dps').hide();
|
||||||
} else {
|
} else {
|
||||||
$('.rate-tpa .roll-pitch').hide();
|
$('.rate-tpa .roll-pitch').hide();
|
||||||
|
$('.rate-tpa--inav').hide();
|
||||||
}
|
}
|
||||||
|
|
||||||
// UI Hooks
|
// UI Hooks
|
||||||
|
@ -412,4 +424,4 @@ TABS.pid_tuning.cleanup = function (callback) {
|
||||||
if (callback) {
|
if (callback) {
|
||||||
callback();
|
callback();
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue