mirror of
https://github.com/betaflight/betaflight-configurator.git
synced 2025-07-26 09:45:28 +03:00
Fix handling of MSP_SERVO and MSP_MOTOR. Previously the code assumed
there was always 8 servos and 8 motors.
This commit is contained in:
parent
774fcf5f68
commit
8d39b3f8cc
1 changed files with 4 additions and 2 deletions
|
@ -240,16 +240,18 @@ var MSP = {
|
||||||
SENSOR_DATA.magnetometer[2] = data.getInt16(16, 1) / 1090;
|
SENSOR_DATA.magnetometer[2] = data.getInt16(16, 1) / 1090;
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_SERVO:
|
case MSP_codes.MSP_SERVO:
|
||||||
|
var servoCount = message_length / 2;
|
||||||
var needle = 0;
|
var needle = 0;
|
||||||
for (var i = 0; i < 8; i++) {
|
for (var i = 0; i < servoCount; i++) {
|
||||||
SERVO_DATA[i] = data.getUint16(needle, 1);
|
SERVO_DATA[i] = data.getUint16(needle, 1);
|
||||||
|
|
||||||
needle += 2;
|
needle += 2;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
case MSP_codes.MSP_MOTOR:
|
case MSP_codes.MSP_MOTOR:
|
||||||
|
var motorCount = message_length / 2;
|
||||||
var needle = 0;
|
var needle = 0;
|
||||||
for (var i = 0; i < 8; i++) {
|
for (var i = 0; i < motorCount; i++) {
|
||||||
MOTOR_DATA[i] = data.getUint16(needle, 1);
|
MOTOR_DATA[i] = data.getUint16(needle, 1);
|
||||||
|
|
||||||
needle += 2;
|
needle += 2;
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue