Rev 1562 | Rev 1564 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1562 | Rev 1563 | ||
---|---|---|---|
1 | package dongfang.mkt.frames; |
1 | package dongfang.mkt.frames; |
2 | - | ||
3 | public class OSDDataResponseFrame extends ResponseFrame { |
- | |
4 | - | ||
5 | public static class GPSPosition { |
- | |
6 | int longitude; // in 1E-7 degrees. 32 bit signed. |
- | |
7 | int latitude; // in 1E-7 degrees. 32 bit signed. |
- | |
8 | long altitude; // in mm. 32 bit signed. |
- | |
9 | int status; |
- | |
10 | public int getLongitude() { |
- | |
11 | return longitude; |
- | |
12 | } |
- | |
13 | public int getLatitude() { |
- | |
14 | return latitude; |
- | |
15 | } |
- | |
16 | public long getAltitude() { |
- | |
17 | return altitude; |
- | |
18 | } |
- | |
19 | public int getStatus() { |
- | |
20 | return status; |
- | |
21 | } |
- | |
22 | public void setLongitude(int longitude) { |
- | |
23 | this.longitude = longitude; |
- | |
24 | } |
- | |
25 | public void setLatitude(int latitude) { |
- | |
26 | this.latitude = latitude; |
- | |
27 | } |
- | |
28 | public void setAltitude(long altitude) { |
- | |
29 | this.altitude = altitude; |
- | |
30 | } |
- | |
31 | public void setStatus(int status) { |
- | |
32 | this.status = status; |
- | |
33 | } |
- | |
34 | - | ||
35 | public String toXML() { |
- | |
36 | double latitude = Math.abs(this.latitude); |
2 | |
37 | String result = "latitude=\"" + latitude/1E7; |
- | |
38 | if (this.latitude < 0) result+= "S"; else result+="N"; |
- | |
39 | result += "\""; |
3 | import dongfang.mkt.datatype.GPSBearingAndRange; |
40 | double longitude= Math.abs(this.longitude); |
- | |
41 | result += " longitude=\"" + longitude/1E7; |
- | |
42 | if (this.longitude < 0) result+= "W"; else result+="E"; |
- | |
43 | result += "\""; |
- | |
44 | return result; |
- | |
45 | } |
- | |
46 | - | ||
47 | public String toString() { |
- | |
48 | double latitude = Math.abs(this.latitude); |
- | |
49 | String result = "" + latitude/1E7; |
- | |
50 | if (this.latitude < 0) result+= "S"; else result+="N"; |
- | |
51 | result += " "; |
- | |
52 | double longitude= Math.abs(this.longitude); |
- | |
53 | result += "" + longitude/1E7; |
- | |
54 | if (this.longitude < 0) result+= "W"; else result+="E"; |
- | |
55 | return result; |
- | |
56 | } |
- | |
57 | } |
4 | import dongfang.mkt.datatype.GPSPosition; |
58 | - | ||
59 | public static class GPSDistanceAndBearing { |
- | |
60 | int distance; // in m/10. 16 bit unsigned. |
- | |
61 | int bearing; // in degrees. 16 bit signed. |
- | |
62 | public int getDistance() { |
- | |
63 | return distance; |
- | |
64 | } |
- | |
65 | public int getBearing() { |
- | |
66 | return bearing; |
- | |
67 | } |
- | |
68 | public void setDistance(int distance) { |
- | |
69 | this.distance = distance; |
- | |
70 | } |
- | |
71 | public void setBearing(int bearing) { |
- | |
72 | this.bearing = bearing; |
- | |
73 | } |
- | |
74 | public String toXML() { |
- | |
75 | String result = "distance=\"" + ((double)this.distance)/10; |
- | |
76 | result += "\" bearing=\""; |
- | |
77 | result += this.bearing; |
- | |
78 | result += "\""; |
- | |
79 | return result; |
- | |
80 | } |
- | |
81 | public String toString() { |
- | |
82 | String result = "" + ((double)this.distance)/10; |
- | |
83 | result += "m @"; |
- | |
84 | result += this.bearing; |
- | |
85 | result += "deg"; |
- | |
86 | return result; |
- | |
87 | } |
5 | |
88 | } |
6 | public class OSDDataResponseFrame extends ResponseFrame { |
89 | 7 | ||
90 | private int version; |
8 | private int version; |
91 | private GPSPosition currentPosition; |
9 | private GPSPosition currentPosition; |
92 | private GPSPosition targetPosition; |
10 | private GPSPosition targetPosition; |
93 | private GPSDistanceAndBearing currentToTarget; |
11 | private GPSBearingAndRange currentToTarget; |
94 | private GPSPosition homePosition; |
12 | private GPSPosition homePosition; |
95 | private GPSDistanceAndBearing currentToHome; |
13 | private GPSBearingAndRange currentToHome; |
96 | 14 | ||
97 | private int waypointIndex; |
15 | private int waypointIndex; |
98 | private int waypointCount; |
16 | private int waypointCount; |
99 | 17 | ||
100 | private int numberOfSatellites; |
18 | private int numberOfSatellites; |
101 | private int heightByPressure; // 16 bit signed. |
19 | private int heightByPressure; // 16 bit signed. |
102 | private int verticalVelocityByPressure; // 16 bit signed. |
20 | private int verticalVelocityByPressure; // 16 bit signed. |
103 | 21 | ||
104 | private int flightTime; // in secs. 16 bit unsigned. |
22 | private int flightTime; // in secs. 16 bit unsigned. |
105 | private int batteryVoltage; // in 0.1 volts. |
23 | private int batteryVoltage; // in 0.1 volts. |
106 | 24 | ||
107 | private int groundSpeed; // in cm/s. 16 bit unsigned. |
25 | private int groundSpeed; // in cm/s. 16 bit unsigned. |
108 | private int directionOfFlight; // of movement. 16 bit signed. |
26 | private int directionOfFlight; // of movement. 16 bit signed. |
109 | private int compassHeading;// 16 bit signed. |
27 | private int compassHeading;// 16 bit signed. |
110 | 28 | ||
111 | private int pitchAngle; |
29 | private int pitchAngle; |
112 | private int rollAngle; |
30 | private int rollAngle; |
113 | 31 | ||
114 | private int rcQuality; |
32 | private int rcQuality; |
115 | private int fcFlags; |
33 | private int fcFlags; |
116 | private int ncFlags; |
34 | private int ncFlags; |
117 | private int errorCode; |
35 | private int errorCode; |
118 | 36 | ||
119 | private int operatingRadius; |
37 | private int operatingRadius; |
120 | private int verticalVelocityByGPS; // 16 bit signed. |
38 | private int verticalVelocityByGPS; // 16 bit signed. |
121 | 39 | ||
122 | private int targetLoiterTime; |
40 | private int targetLoiterTime; |
123 | private int fcFlags2; |
41 | private int fcFlags2; |
124 | private int setpointForAltitude; // 16 bit signed. |
42 | private int setpointForAltitude; // 16 bit signed. |
125 | 43 | ||
126 | private int throttle; |
44 | private int throttle; |
127 | private int current; // 16 bit unsigned. |
45 | private int current; // 16 bit unsigned. |
128 | private int capacityUsed; // 16 bit unsigned. |
46 | private int capacityUsed; // 16 bit unsigned. |
129 | 47 | ||
130 | public OSDDataResponseFrame(int address) { |
48 | public OSDDataResponseFrame(int address) { |
131 | super(address); |
49 | super(address); |
132 | } |
50 | } |
133 | 51 | ||
134 | public int getVersion() { |
52 | public int getVersion() { |
135 | return version; |
53 | return version; |
136 | } |
54 | } |
137 | public GPSPosition getCurrentPosition() { |
55 | public GPSPosition getCurrentPosition() { |
138 | return currentPosition; |
56 | return currentPosition; |
139 | } |
57 | } |
140 | public GPSPosition getTargetPosition() { |
58 | public GPSPosition getTargetPosition() { |
141 | return targetPosition; |
59 | return targetPosition; |
142 | } |
60 | } |
143 | public GPSDistanceAndBearing getCurrentToTarget() { |
61 | public GPSBearingAndRange getCurrentToTarget() { |
144 | return currentToTarget; |
62 | return currentToTarget; |
145 | } |
63 | } |
146 | public GPSPosition getHomePosition() { |
64 | public GPSPosition getHomePosition() { |
147 | return homePosition; |
65 | return homePosition; |
148 | } |
66 | } |
149 | public GPSDistanceAndBearing getCurrentToHome() { |
67 | public GPSBearingAndRange getCurrentToHome() { |
150 | return currentToHome; |
68 | return currentToHome; |
151 | } |
69 | } |
152 | public int getWaypointIndex() { |
70 | public int getWaypointIndex() { |
153 | return waypointIndex; |
71 | return waypointIndex; |
154 | } |
72 | } |
155 | public int getWaypointCount() { |
73 | public int getWaypointCount() { |
156 | return waypointCount; |
74 | return waypointCount; |
157 | } |
75 | } |
158 | public int getNumberOfSatellites() { |
76 | public int getNumberOfSatellites() { |
159 | return numberOfSatellites; |
77 | return numberOfSatellites; |
160 | } |
78 | } |
161 | public int getHeightByPressure() { |
79 | public int getHeightByPressure() { |
162 | return heightByPressure; |
80 | return heightByPressure; |
163 | } |
81 | } |
164 | public int getVerticalVelocityByPressure() { |
82 | public int getVerticalVelocityByPressure() { |
165 | return verticalVelocityByPressure; |
83 | return verticalVelocityByPressure; |
166 | } |
84 | } |
167 | public int getFlightTime() { |
85 | public int getFlightTime() { |
168 | return flightTime; |
86 | return flightTime; |
169 | } |
87 | } |
170 | public int getBatteryVoltage() { |
88 | public int getBatteryVoltage() { |
171 | return batteryVoltage; |
89 | return batteryVoltage; |
172 | } |
90 | } |
173 | public int getGroundSpeed() { |
91 | public int getGroundSpeed() { |
174 | return groundSpeed; |
92 | return groundSpeed; |
175 | } |
93 | } |
176 | public int getDirectionOfFlight() { |
94 | public int getDirectionOfFlight() { |
177 | return directionOfFlight; |
95 | return directionOfFlight; |
178 | } |
96 | } |
179 | public int getCompassHeading() { |
97 | public int getCompassHeading() { |
180 | return compassHeading; |
98 | return compassHeading; |
181 | } |
99 | } |
182 | public int getPitchAngle() { |
100 | public int getPitchAngle() { |
183 | return pitchAngle; |
101 | return pitchAngle; |
184 | } |
102 | } |
185 | public int getRollAngle() { |
103 | public int getRollAngle() { |
186 | return rollAngle; |
104 | return rollAngle; |
187 | } |
105 | } |
188 | public int getRcQuality() { |
106 | public int getRcQuality() { |
189 | return rcQuality; |
107 | return rcQuality; |
190 | } |
108 | } |
191 | public int getFcFlags() { |
109 | public int getFcFlags() { |
192 | return fcFlags; |
110 | return fcFlags; |
193 | } |
111 | } |
194 | public int getNcFlags() { |
112 | public int getNcFlags() { |
195 | return ncFlags; |
113 | return ncFlags; |
196 | } |
114 | } |
197 | public int getErrorCode() { |
115 | public int getErrorCode() { |
198 | return errorCode; |
116 | return errorCode; |
199 | } |
117 | } |
200 | public int getOperatingRadius() { |
118 | public int getOperatingRadius() { |
201 | return operatingRadius; |
119 | return operatingRadius; |
202 | } |
120 | } |
203 | public int getVerticalVelocityByGPS() { |
121 | public int getVerticalVelocityByGPS() { |
204 | return verticalVelocityByGPS; |
122 | return verticalVelocityByGPS; |
205 | } |
123 | } |
206 | public int getTargetLoiterTime() { |
124 | public int getTargetLoiterTime() { |
207 | return targetLoiterTime; |
125 | return targetLoiterTime; |
208 | } |
126 | } |
209 | public int getFcFlags2() { |
127 | public int getFcFlags2() { |
210 | return fcFlags2; |
128 | return fcFlags2; |
211 | } |
129 | } |
212 | public int getSetpointForAltitude() { |
130 | public int getSetpointForAltitude() { |
213 | return setpointForAltitude; |
131 | return setpointForAltitude; |
214 | } |
132 | } |
215 | public int getThrottle() { |
133 | public int getThrottle() { |
216 | return throttle; |
134 | return throttle; |
217 | } |
135 | } |
218 | public int getCurrent() { |
136 | public int getCurrent() { |
219 | return current; |
137 | return current; |
220 | } |
138 | } |
221 | public int getCapacityUsed() { |
139 | public int getCapacityUsed() { |
222 | return capacityUsed; |
140 | return capacityUsed; |
223 | } |
141 | } |
224 | public void setVersion(int version) { |
142 | public void setVersion(int version) { |
225 | this.version = version; |
143 | this.version = version; |
226 | } |
144 | } |
227 | public void setCurrentPosition(GPSPosition currentPosition) { |
145 | public void setCurrentPosition(GPSPosition currentPosition) { |
228 | this.currentPosition = currentPosition; |
146 | this.currentPosition = currentPosition; |
229 | } |
147 | } |
230 | public void setTargetPosition(GPSPosition targetPosition) { |
148 | public void setTargetPosition(GPSPosition targetPosition) { |
231 | this.targetPosition = targetPosition; |
149 | this.targetPosition = targetPosition; |
232 | } |
150 | } |
233 | public void setCurrentToTarget(GPSDistanceAndBearing currentToTarget) { |
151 | public void setCurrentToTarget(GPSBearingAndRange currentToTarget) { |
234 | this.currentToTarget = currentToTarget; |
152 | this.currentToTarget = currentToTarget; |
235 | } |
153 | } |
236 | public void setHomePosition(GPSPosition homePosition) { |
154 | public void setHomePosition(GPSPosition homePosition) { |
237 | this.homePosition = homePosition; |
155 | this.homePosition = homePosition; |
238 | } |
156 | } |
239 | public void setCurrentToHome(GPSDistanceAndBearing currentToHome) { |
157 | public void setCurrentToHome(GPSBearingAndRange currentToHome) { |
240 | this.currentToHome = currentToHome; |
158 | this.currentToHome = currentToHome; |
241 | } |
159 | } |
242 | public void setWaypointIndex(int waypointIndex) { |
160 | public void setWaypointIndex(int waypointIndex) { |
243 | this.waypointIndex = waypointIndex; |
161 | this.waypointIndex = waypointIndex; |
244 | } |
162 | } |
245 | public void setWaypointCount(int waypointCount) { |
163 | public void setWaypointCount(int waypointCount) { |
246 | this.waypointCount = waypointCount; |
164 | this.waypointCount = waypointCount; |
247 | } |
165 | } |
248 | public void setNumberOfSatellites(int numberOfSatellites) { |
166 | public void setNumberOfSatellites(int numberOfSatellites) { |
249 | this.numberOfSatellites = numberOfSatellites; |
167 | this.numberOfSatellites = numberOfSatellites; |
250 | } |
168 | } |
251 | public void setHeightByPressure(int heightByPressure) { |
169 | public void setHeightByPressure(int heightByPressure) { |
252 | this.heightByPressure = heightByPressure; |
170 | this.heightByPressure = heightByPressure; |
253 | } |
171 | } |
254 | public void setVerticalVelocityByPressure(int verticalVelocityByPressure) { |
172 | public void setVerticalVelocityByPressure(int verticalVelocityByPressure) { |
255 | this.verticalVelocityByPressure = verticalVelocityByPressure; |
173 | this.verticalVelocityByPressure = verticalVelocityByPressure; |
256 | } |
174 | } |
257 | public void setFlightTime(int flightTime) { |
175 | public void setFlightTime(int flightTime) { |
258 | this.flightTime = flightTime; |
176 | this.flightTime = flightTime; |
259 | } |
177 | } |
260 | public void setBatteryVoltage(int batteryVoltage) { |
178 | public void setBatteryVoltage(int batteryVoltage) { |
261 | this.batteryVoltage = batteryVoltage; |
179 | this.batteryVoltage = batteryVoltage; |
262 | } |
180 | } |
263 | public void setGroundSpeed(int groundSpeed) { |
181 | public void setGroundSpeed(int groundSpeed) { |
264 | this.groundSpeed = groundSpeed; |
182 | this.groundSpeed = groundSpeed; |
265 | } |
183 | } |
266 | public void setDirectionOfFlight(int heading) { |
184 | public void setDirectionOfFlight(int heading) { |
267 | this.directionOfFlight = heading; |
185 | this.directionOfFlight = heading; |
268 | } |
186 | } |
269 | public void setCompassHeading(int compassHeading) { |
187 | public void setCompassHeading(int compassHeading) { |
270 | this.compassHeading = compassHeading; |
188 | this.compassHeading = compassHeading; |
271 | } |
189 | } |
272 | public void setPitchAngle(int pitchAngle) { |
190 | public void setPitchAngle(int pitchAngle) { |
273 | this.pitchAngle = pitchAngle; |
191 | this.pitchAngle = pitchAngle; |
274 | } |
192 | } |
275 | public void setRollAngle(int rollAngle) { |
193 | public void setRollAngle(int rollAngle) { |
276 | this.rollAngle = rollAngle; |
194 | this.rollAngle = rollAngle; |
277 | } |
195 | } |
278 | public void setRcQuality(int rcQuality) { |
196 | public void setRcQuality(int rcQuality) { |
279 | this.rcQuality = rcQuality; |
197 | this.rcQuality = rcQuality; |
280 | } |
198 | } |
281 | public void setFcFlags(int fcFlags) { |
199 | public void setFcFlags(int fcFlags) { |
282 | this.fcFlags = fcFlags; |
200 | this.fcFlags = fcFlags; |
283 | } |
201 | } |
284 | public void setNcFlags(int ncFlags) { |
202 | public void setNcFlags(int ncFlags) { |
285 | this.ncFlags = ncFlags; |
203 | this.ncFlags = ncFlags; |
286 | } |
204 | } |
287 | public void setErrorCode(int errorCode) { |
205 | public void setErrorCode(int errorCode) { |
288 | this.errorCode = errorCode; |
206 | this.errorCode = errorCode; |
289 | } |
207 | } |
290 | public void setOperatingRadius(int operatingRadius) { |
208 | public void setOperatingRadius(int operatingRadius) { |
291 | this.operatingRadius = operatingRadius; |
209 | this.operatingRadius = operatingRadius; |
292 | } |
210 | } |
293 | public void setVerticalVelocityByGPS(int verticalVelocityByGPS) { |
211 | public void setVerticalVelocityByGPS(int verticalVelocityByGPS) { |
294 | this.verticalVelocityByGPS = verticalVelocityByGPS; |
212 | this.verticalVelocityByGPS = verticalVelocityByGPS; |
295 | } |
213 | } |
296 | public void setTargetLoiterTime(int targetLoiterTime) { |
214 | public void setTargetLoiterTime(int targetLoiterTime) { |
297 | this.targetLoiterTime = targetLoiterTime; |
215 | this.targetLoiterTime = targetLoiterTime; |
298 | } |
216 | } |
299 | public void setFcFlags2(int fcFlags2) { |
217 | public void setFcFlags2(int fcFlags2) { |
300 | this.fcFlags2 = fcFlags2; |
218 | this.fcFlags2 = fcFlags2; |
301 | } |
219 | } |
302 | public void setSetpointForAltitude(int setpointForAltitude) { |
220 | public void setSetpointForAltitude(int setpointForAltitude) { |
303 | this.setpointForAltitude = setpointForAltitude; |
221 | this.setpointForAltitude = setpointForAltitude; |
304 | } |
222 | } |
305 | public void setThrottle(int throttle) { |
223 | public void setThrottle(int throttle) { |
306 | this.throttle = throttle; |
224 | this.throttle = throttle; |
307 | } |
225 | } |
308 | public void setCurrent(int current) { |
226 | public void setCurrent(int current) { |
309 | this.current = current; |
227 | this.current = current; |
310 | } |
228 | } |
311 | public void setCapacityUsed(int capacityUsed) { |
229 | public void setCapacityUsed(int capacityUsed) { |
312 | this.capacityUsed = capacityUsed; |
230 | this.capacityUsed = capacityUsed; |
313 | } |
231 | } |
314 | 232 | ||
315 | @Override |
233 | @Override |
316 | public boolean isResponseTo(RequestFrame r) { |
234 | public boolean isResponseTo(RequestFrame r) { |
317 | return r instanceof OSDDataRequestFrame; |
235 | return r instanceof OSDDataRequestFrame; |
318 | } |
236 | } |
319 | 237 | ||
320 | public String toString() { |
238 | public String toString() { |
321 | StringBuilder result = new StringBuilder(); |
239 | StringBuilder result = new StringBuilder(); |
322 | result.append("OSDData version=" + this.version + "\n"); |
240 | result.append("OSDData version=" + this.version + "\n"); |
323 | result.append("currentPosition: " + this.currentPosition + "\n"); |
241 | result.append("currentPosition: " + this.currentPosition + "\n"); |
324 | result.append("targetPosition: " + this.targetPosition + " (" + this.currentToTarget + ")" + "\n"); |
242 | result.append("targetPosition: " + this.targetPosition + " (" + this.currentToTarget + ")" + "\n"); |
325 | result.append("homePosition: " + this.homePosition + " (" + this.currentToHome + ")" + "\n"); |
243 | result.append("homePosition: " + this.homePosition + " (" + this.currentToHome + ")" + "\n"); |
326 | result.append("heightByPressure: " + this.heightByPressure + "\n"); |
244 | result.append("heightByPressure: " + this.heightByPressure + "\n"); |
327 | result.append("verticalVelocityByPressure: " + this.verticalVelocityByPressure + "\n"); |
245 | result.append("verticalVelocityByPressure: " + this.verticalVelocityByPressure + "\n"); |
328 | result.append("verticalVelocityByGPS: " + this.verticalVelocityByGPS + "\n"); |
246 | result.append("verticalVelocityByGPS: " + this.verticalVelocityByGPS + "\n"); |
329 | result.append("direction of flight: " + this.directionOfFlight + "\n"); |
247 | result.append("direction of flight: " + this.directionOfFlight + "\n"); |
330 | result.append("compassHeading: " + this.compassHeading + "\n"); |
248 | result.append("compassHeading: " + this.compassHeading + "\n"); |
331 | result.append("pitchAngle: " + this.pitchAngle + "\n"); |
249 | result.append("pitchAngle: " + this.pitchAngle + "\n"); |
332 | result.append("rollAngle: " + this.rollAngle + "\n"); |
250 | result.append("rollAngle: " + this.rollAngle + "\n"); |
333 | result.append("battery voltage: " + ((double)this.batteryVoltage)/10 + "\n"); |
251 | result.append("battery voltage: " + ((double)this.batteryVoltage)/10 + "\n"); |
334 | result.append("throttle: " + this.throttle + "\n"); |
252 | result.append("throttle: " + this.throttle + "\n"); |
335 | return result.toString(); |
253 | return result.toString(); |
336 | } |
254 | } |
337 | } |
255 | } |
338 | 256 | ||
339 | 257 |