Subversion Repositories Projects

Rev

Rev 1690 | Rev 1696 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1690 Rev 1695
Line 10... Line 10...
10 
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
 10 
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
11 
import dongfang.mkt.frames.AttitudeDataResponseFrame;
 11 
import dongfang.mkt.frames.AttitudeDataResponseFrame;
12 
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
 12 
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
13 
import dongfang.mkt.frames.CompassHeadingResponseFrame;
 13 
import dongfang.mkt.frames.CompassHeadingResponseFrame;
14 
import dongfang.mkt.frames.ConfirmFrame;
 14 
import dongfang.mkt.frames.ConfirmFrame;
- 
 
 15 
import dongfang.mkt.frames.DCMMatrixResponseFrame;
15 
import dongfang.mkt.frames.DebugResponseFrame;
 16 
import dongfang.mkt.frames.DebugResponseFrame;
16 
import dongfang.mkt.frames.MotorTestResponseFrame;
 17 
import dongfang.mkt.frames.MotorTestResponseFrame;
17 
import dongfang.mkt.frames.OSDDataResponseFrame;
 18 
import dongfang.mkt.frames.OSDDataResponseFrame;
18 
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
 19 
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
19 
import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame;
 20 
import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame;
Line 120... Line 121...
120 
                                // of Unicode, which Java uses for chars)
 121 
                                // of Unicode, which Java uses for chars)
121 
                                result[i] = (char) readByte();
 122 
                                result[i] = (char) readByte();
122 
                        }
 123 
                        }
123 
                        return result;
 124 
                        return result;
124 
                }
 125 
                }
- 
 
 126 
               
- 
 
 127 
                public float readFloat() throws IOException {
- 
 
 128 
                        int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24);
- 
 
 129 
                        return Float.intBitsToFloat(asInt);
125 
        }
 130 
                }
- 
 
 131 
 
- 
 
 132 
                public float[] readFloats(int length) throws IOException {
- 
 
 133 
                        float[] result = new float[length];
- 
 
 134 
                        for (int i = 0; i < length; i++) {
- 
 
 135 
                                result[i] = readFloat();
- 
 
 136 
                        }
- 
 
 137 
                        return result;
- 
 
 138 
                }
- 
 
 139 
}
Line 126... Line 140...
126 
 
 140 
 
127 
        MKDataInputStream base64InputStream = new MKDataInputStream();
 141 
        MKDataInputStream base64InputStream = new MKDataInputStream();
Line 128... Line 142...
128 
        OutputStream nonPacketSpillway = null; //System.err;
 142 
        OutputStream nonPacketSpillway = null; //System.err;
Line 183... Line 197...
183 
                        result = f;
 197 
                        result = f;
184 
                        break;
 198 
                        break;
185 
                }
 199 
                }
186 
                case 'C': {
 200 
                case 'C': {
187 
                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);
 201 
                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);
188 
                        f.setPitch(base64InputStream.readSignedWord());
 202 
                        f.setAttitude(base64InputStream.readFloats(3));
189 
                        f.setRoll(base64InputStream.readSignedWord());
 203 
                        f.setRates(base64InputStream.readFloats(3));
190 
                        f.setHeading(base64InputStream.readSignedWord());
 - 
 
191 
                        f.setExpansion(base64InputStream.readBytes(8));
 204 
                        f.setAcc(base64InputStream.readFloats(3));
192 
                        result = f;
 205 
                        result = f;
193 
                        break;
 206 
                        break;
194 
                }
 207 
                }
195 
                case 'D': {
 208 
                case 'D': {
196 
                        DebugResponseFrame f = new DebugResponseFrame(address);
 209 
                        DebugResponseFrame f = new DebugResponseFrame(address);
Line 199... Line 212...
199 
                        for (int i=0; i<32; i++)
 212 
                        for (int i=0; i<32; i++)
200 
                                f.setAnalog(i, base64InputStream.readSignedWord());
 213 
                                f.setAnalog(i, base64InputStream.readSignedWord());
201 
                        result = f;
 214 
                        result = f;
202 
                        break;
 215 
                        break;
203 
                }
 216 
                }
- 
 
 217 
                case 'E': {
- 
 
 218 
                        DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address);
- 
 
 219 
                        float[][] matrix = new float[3][];
- 
 
 220 
                        for (int i=0; i<3; i++) {
- 
 
 221 
                                float[] row = base64InputStream.readFloats(3);
- 
 
 222 
                                matrix[i] = row;
- 
 
 223 
                        }
- 
 
 224 
                        f.setMatrix(matrix);
- 
 
 225 
                        result = f;
- 
 
 226 
                        break;
- 
 
 227 
                }
204 
                case 'F': {
 228 
                case 'F': {
205 
                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
 229 
                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
206 
                        f.setParameterSetNumber(base64InputStream.readByte());
 230 
                        f.setParameterSetNumber(base64InputStream.readByte());
207 
                        result = f;
 231 
                        result = f;
208 
                        break;
 232 
                        break;