17,12 → 17,14 |
import dongfang.mkt.frames.OSDDataResponseFrame; |
import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
import dongfang.mkt.frames.ReadMotorMixerResponseFrame; |
import dongfang.mkt.frames.ResponseFrame; |
import dongfang.mkt.frames.SetCompassHeadingResponseFrame; |
import dongfang.mkt.frames.ReadParamSetResponseFrame; |
import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
import dongfang.mkt.frames.WriteMotorMixerResponseFrame; |
import dongfang.mkt.frames.WriteParamSetResponseFrame; |
import dongfang.mkt.frames.VariablesResponseFrame; |
import dongfang.mkt.frames.ReadVariablesResponseFrame; |
import dongfang.mkt.frames.VersionResponseFrame; |
|
public class MKInputStream extends InputStream { |
259,6 → 261,39 |
result = f; |
break; |
} |
case 'M': { |
WriteMotorMixerResponseFrame f = new WriteMotorMixerResponseFrame(address); |
f.setWasAccepted(base64InputStream.readByte() != 0); |
} |
case 'N': { |
int numMotors = 12; |
ReadMotorMixerResponseFrame f = new ReadMotorMixerResponseFrame(); |
f.setConfigurationVersion(base64InputStream.readByte()); |
int length = base64InputStream.readByte(); |
f.setDataLength(length); |
int testLength = 0; |
int[][] matrix = new int[numMotors][]; |
for(int i=0; i<numMotors; i++) { |
int[] row = new int[4]; |
for(int j=0; j<4; j++) { |
matrix[i][j] = base64InputStream.readByte(); |
testLength++; |
} |
} |
|
if (length != testLength) |
throw new IOException("Length of motor mixer data was not as expected."); |
/* |
int[] opposite = new int[numMotors]; |
for(int i=0; i<numMotors; i++) { |
opposite[i] = base64InputStream.readByte(); |
} |
*/ |
|
f.setMatrix(matrix); |
|
result = f; |
} |
case 'O': { |
OSDDataResponseFrame f = new OSDDataResponseFrame(address); |
f.setVersion(base64InputStream.readByte()); |
366,7 → 401,7 |
|
// This is my own creation. The ID collides with the waypoint one of FC. |
case 'X': { |
VariablesResponseFrame f = new VariablesResponseFrame(address); |
ReadVariablesResponseFrame f = new ReadVariablesResponseFrame(address); |
f.setVariables(base64InputStream.readSignedWords(8)); |
result = f; |
break; |