Rev 1564 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1564 | Rev 1565 | ||
---|---|---|---|
1 | package dongfang.mkt.frames; |
1 | package dongfang.mkt.frames; |
2 | 2 | ||
3 | import dongfang.mkt.datatype.GPSBearingAndRange; |
3 | import dongfang.mkt.datatype.GPSBearingAndRange; |
4 | import dongfang.mkt.datatype.GPSPosition; |
4 | import dongfang.mkt.datatype.GPSPosition; |
5 | 5 | ||
6 | public class OSDDataResponseFrame extends ResponseFrame { |
6 | public class OSDDataResponseFrame extends ResponseFrame { |
7 | 7 | ||
8 | private int version; |
8 | private int version; |
9 | private GPSPosition currentPosition; |
9 | private GPSPosition currentPosition; |
10 | private GPSPosition targetPosition; |
10 | private GPSPosition targetPosition; |
11 | private GPSBearingAndRange currentToTarget; |
11 | private GPSBearingAndRange currentToTarget; |
12 | private GPSPosition homePosition; |
12 | private GPSPosition homePosition; |
13 | private GPSBearingAndRange currentToHome; |
13 | private GPSBearingAndRange currentToHome; |
14 | 14 | ||
15 | private int waypointIndex; |
15 | private int waypointIndex; |
16 | private int waypointCount; |
16 | private int waypointCount; |
17 | 17 | ||
18 | private int numberOfSatellites; |
18 | private int numberOfSatellites; |
19 | private int heightByPressure; // 16 bit signed. |
19 | private double heightByPressure; // in m. |
20 | private int verticalVelocityByPressure; // 16 bit signed. |
20 | private int verticalVelocityByPressure; // 16 bit signed. |
21 | 21 | ||
22 | private int flightTime; // in secs. 16 bit unsigned. |
22 | private int flightTime; // in secs. 16 bit unsigned. |
23 | private int batteryVoltage; // in 0.1 volts. |
23 | private int batteryVoltage; // in 0.1 volts. |
24 | 24 | ||
25 | private int groundSpeed; // in cm/s. 16 bit unsigned. |
25 | private int groundSpeed; // in cm/s. 16 bit unsigned. |
26 | private int directionOfFlight; // of movement. 16 bit signed. |
26 | private int directionOfFlight; // of movement. 16 bit signed. |
27 | private int compassHeading;// 16 bit signed. |
27 | private int compassHeading;// 16 bit signed. |
28 | 28 | ||
29 | private int pitchAngle; |
29 | private int pitchAngle; |
30 | private int rollAngle; |
30 | private int rollAngle; |
31 | 31 | ||
32 | private int rcQuality; |
32 | private int rcQuality; |
33 | private int fcFlags; |
33 | private int fcFlags; |
34 | private int ncFlags; |
34 | private int ncFlags; |
35 | private int errorCode; |
35 | private int errorCode; |
36 | 36 | ||
37 | private int operatingRadius; |
37 | private int operatingRadius; |
38 | private int verticalVelocityByGPS; // 16 bit signed. |
38 | private int verticalVelocityByGPS; // 16 bit signed. |
39 | 39 | ||
40 | private int targetLoiterTime; |
40 | private int targetLoiterTime; |
41 | private int fcFlags2; |
41 | private int fcFlags2; |
42 | private int setpointForAltitude; // 16 bit signed. |
42 | private double setpointForAltitude; // in m. |
43 | 43 | ||
44 | private int throttle; |
44 | private int throttle; |
45 | private int current; // 16 bit unsigned. |
45 | private int current; // 16 bit unsigned. |
46 | private int capacityUsed; // 16 bit unsigned. |
46 | private int capacityUsed; // 16 bit unsigned. |
47 | 47 | ||
48 | public OSDDataResponseFrame(int address) { |
48 | public OSDDataResponseFrame(int address) { |
49 | super(address); |
49 | super(address); |
50 | } |
50 | } |
51 | 51 | ||
52 | public int getVersion() { |
52 | public int getVersion() { |
53 | return version; |
53 | return version; |
54 | } |
54 | } |
55 | public GPSPosition getCurrentPosition() { |
55 | public GPSPosition getCurrentPosition() { |
56 | return currentPosition; |
56 | return currentPosition; |
57 | } |
57 | } |
58 | public GPSPosition getTargetPosition() { |
58 | public GPSPosition getTargetPosition() { |
59 | return targetPosition; |
59 | return targetPosition; |
60 | } |
60 | } |
61 | public GPSBearingAndRange getCurrentToTarget() { |
61 | public GPSBearingAndRange getCurrentToTarget() { |
62 | return currentToTarget; |
62 | return currentToTarget; |
63 | } |
63 | } |
64 | public GPSPosition getHomePosition() { |
64 | public GPSPosition getHomePosition() { |
65 | return homePosition; |
65 | return homePosition; |
66 | } |
66 | } |
67 | public GPSBearingAndRange getCurrentToHome() { |
67 | public GPSBearingAndRange getCurrentToHome() { |
68 | return currentToHome; |
68 | return currentToHome; |
69 | } |
69 | } |
70 | public int getWaypointIndex() { |
70 | public int getWaypointIndex() { |
71 | return waypointIndex; |
71 | return waypointIndex; |
72 | } |
72 | } |
73 | public int getWaypointCount() { |
73 | public int getWaypointCount() { |
74 | return waypointCount; |
74 | return waypointCount; |
75 | } |
75 | } |
76 | public int getNumberOfSatellites() { |
76 | public int getNumberOfSatellites() { |
77 | return numberOfSatellites; |
77 | return numberOfSatellites; |
78 | } |
78 | } |
79 | public int getHeightByPressure() { |
79 | public double getHeightByPressure() { |
80 | return heightByPressure; |
80 | return heightByPressure; |
81 | } |
81 | } |
82 | public int getVerticalVelocityByPressure() { |
82 | public int getVerticalVelocityByPressure() { |
83 | return verticalVelocityByPressure; |
83 | return verticalVelocityByPressure; |
84 | } |
84 | } |
85 | public int getFlightTime() { |
85 | public int getFlightTime() { |
86 | return flightTime; |
86 | return flightTime; |
87 | } |
87 | } |
88 | public int getBatteryVoltage() { |
88 | public int getBatteryVoltage() { |
89 | return batteryVoltage; |
89 | return batteryVoltage; |
90 | } |
90 | } |
91 | public int getGroundSpeed() { |
91 | public int getGroundSpeed() { |
92 | return groundSpeed; |
92 | return groundSpeed; |
93 | } |
93 | } |
94 | public int getDirectionOfFlight() { |
94 | public int getDirectionOfFlight() { |
95 | return directionOfFlight; |
95 | return directionOfFlight; |
96 | } |
96 | } |
97 | public int getCompassHeading() { |
97 | public int getCompassHeading() { |
98 | return compassHeading; |
98 | return compassHeading; |
99 | } |
99 | } |
100 | public int getPitchAngle() { |
100 | public int getPitchAngle() { |
101 | return pitchAngle; |
101 | return pitchAngle; |
102 | } |
102 | } |
103 | public int getRollAngle() { |
103 | public int getRollAngle() { |
104 | return rollAngle; |
104 | return rollAngle; |
105 | } |
105 | } |
106 | public int getRcQuality() { |
106 | public int getRcQuality() { |
107 | return rcQuality; |
107 | return rcQuality; |
108 | } |
108 | } |
109 | public int getFcFlags() { |
109 | public int getFcFlags() { |
110 | return fcFlags; |
110 | return fcFlags; |
111 | } |
111 | } |
112 | public int getNcFlags() { |
112 | public int getNcFlags() { |
113 | return ncFlags; |
113 | return ncFlags; |
114 | } |
114 | } |
115 | public int getErrorCode() { |
115 | public int getErrorCode() { |
116 | return errorCode; |
116 | return errorCode; |
117 | } |
117 | } |
118 | public int getOperatingRadius() { |
118 | public int getOperatingRadius() { |
119 | return operatingRadius; |
119 | return operatingRadius; |
120 | } |
120 | } |
121 | public int getVerticalVelocityByGPS() { |
121 | public int getVerticalVelocityByGPS() { |
122 | return verticalVelocityByGPS; |
122 | return verticalVelocityByGPS; |
123 | } |
123 | } |
124 | public int getTargetLoiterTime() { |
124 | public int getTargetLoiterTime() { |
125 | return targetLoiterTime; |
125 | return targetLoiterTime; |
126 | } |
126 | } |
127 | public int getFcFlags2() { |
127 | public int getFcFlags2() { |
128 | return fcFlags2; |
128 | return fcFlags2; |
129 | } |
129 | } |
130 | public int getSetpointForAltitude() { |
130 | public double getSetpointForAltitude() { |
131 | return setpointForAltitude; |
131 | return setpointForAltitude; |
132 | } |
132 | } |
133 | public int getThrottle() { |
133 | public int getThrottle() { |
134 | return throttle; |
134 | return throttle; |
135 | } |
135 | } |
136 | public int getCurrent() { |
136 | public int getCurrent() { |
137 | return current; |
137 | return current; |
138 | } |
138 | } |
139 | public int getCapacityUsed() { |
139 | public int getCapacityUsed() { |
140 | return capacityUsed; |
140 | return capacityUsed; |
141 | } |
141 | } |
142 | public void setVersion(int version) { |
142 | public void setVersion(int version) { |
143 | this.version = version; |
143 | this.version = version; |
144 | } |
144 | } |
145 | public void setCurrentPosition(GPSPosition currentPosition) { |
145 | public void setCurrentPosition(GPSPosition currentPosition) { |
146 | this.currentPosition = currentPosition; |
146 | this.currentPosition = currentPosition; |
147 | } |
147 | } |
148 | public void setTargetPosition(GPSPosition targetPosition) { |
148 | public void setTargetPosition(GPSPosition targetPosition) { |
149 | this.targetPosition = targetPosition; |
149 | this.targetPosition = targetPosition; |
150 | } |
150 | } |
151 | public void setCurrentToTarget(GPSBearingAndRange currentToTarget) { |
151 | public void setCurrentToTarget(GPSBearingAndRange currentToTarget) { |
152 | this.currentToTarget = currentToTarget; |
152 | this.currentToTarget = currentToTarget; |
153 | } |
153 | } |
154 | public void setHomePosition(GPSPosition homePosition) { |
154 | public void setHomePosition(GPSPosition homePosition) { |
155 | this.homePosition = homePosition; |
155 | this.homePosition = homePosition; |
156 | } |
156 | } |
157 | public void setCurrentToHome(GPSBearingAndRange currentToHome) { |
157 | public void setCurrentToHome(GPSBearingAndRange currentToHome) { |
158 | this.currentToHome = currentToHome; |
158 | this.currentToHome = currentToHome; |
159 | } |
159 | } |
160 | public void setWaypointIndex(int waypointIndex) { |
160 | public void setWaypointIndex(int waypointIndex) { |
161 | this.waypointIndex = waypointIndex; |
161 | this.waypointIndex = waypointIndex; |
162 | } |
162 | } |
163 | public void setWaypointCount(int waypointCount) { |
163 | public void setWaypointCount(int waypointCount) { |
164 | this.waypointCount = waypointCount; |
164 | this.waypointCount = waypointCount; |
165 | } |
165 | } |
166 | public void setNumberOfSatellites(int numberOfSatellites) { |
166 | public void setNumberOfSatellites(int numberOfSatellites) { |
167 | this.numberOfSatellites = numberOfSatellites; |
167 | this.numberOfSatellites = numberOfSatellites; |
168 | } |
168 | } |
169 | public void setHeightByPressure(int heightByPressure) { |
169 | public void _setHeightByPressure(double heightByPressure) { |
170 | this.heightByPressure = heightByPressure; |
170 | this.heightByPressure = heightByPressure; |
171 | } |
171 | } |
172 | public void setVerticalVelocityByPressure(int verticalVelocityByPressure) { |
172 | public void setVerticalVelocityByPressure(int verticalVelocityByPressure) { |
173 | this.verticalVelocityByPressure = verticalVelocityByPressure; |
173 | this.verticalVelocityByPressure = verticalVelocityByPressure; |
174 | } |
174 | } |
175 | public void setFlightTime(int flightTime) { |
175 | public void setFlightTime(int flightTime) { |
176 | this.flightTime = flightTime; |
176 | this.flightTime = flightTime; |
177 | } |
177 | } |
178 | public void setBatteryVoltage(int batteryVoltage) { |
178 | public void setBatteryVoltage(int batteryVoltage) { |
179 | this.batteryVoltage = batteryVoltage; |
179 | this.batteryVoltage = batteryVoltage; |
180 | } |
180 | } |
181 | public void setGroundSpeed(int groundSpeed) { |
181 | public void setGroundSpeed(int groundSpeed) { |
182 | this.groundSpeed = groundSpeed; |
182 | this.groundSpeed = groundSpeed; |
183 | } |
183 | } |
184 | public void setDirectionOfFlight(int heading) { |
184 | public void setDirectionOfFlight(int heading) { |
185 | this.directionOfFlight = heading; |
185 | this.directionOfFlight = heading; |
186 | } |
186 | } |
187 | public void setCompassHeading(int compassHeading) { |
187 | public void setCompassHeading(int compassHeading) { |
188 | this.compassHeading = compassHeading; |
188 | this.compassHeading = compassHeading; |
189 | } |
189 | } |
190 | public void setPitchAngle(int pitchAngle) { |
190 | public void setPitchAngle(int pitchAngle) { |
191 | this.pitchAngle = pitchAngle; |
191 | this.pitchAngle = pitchAngle; |
192 | } |
192 | } |
193 | public void setRollAngle(int rollAngle) { |
193 | public void setRollAngle(int rollAngle) { |
194 | this.rollAngle = rollAngle; |
194 | this.rollAngle = rollAngle; |
195 | } |
195 | } |
196 | public void setRcQuality(int rcQuality) { |
196 | public void setRcQuality(int rcQuality) { |
197 | this.rcQuality = rcQuality; |
197 | this.rcQuality = rcQuality; |
198 | } |
198 | } |
199 | public void setFcFlags(int fcFlags) { |
199 | public void setFcFlags(int fcFlags) { |
200 | this.fcFlags = fcFlags; |
200 | this.fcFlags = fcFlags; |
201 | } |
201 | } |
202 | public void setNcFlags(int ncFlags) { |
202 | public void setNcFlags(int ncFlags) { |
203 | this.ncFlags = ncFlags; |
203 | this.ncFlags = ncFlags; |
204 | } |
204 | } |
205 | public void setErrorCode(int errorCode) { |
205 | public void setErrorCode(int errorCode) { |
206 | this.errorCode = errorCode; |
206 | this.errorCode = errorCode; |
207 | } |
207 | } |
208 | public void setOperatingRadius(int operatingRadius) { |
208 | public void setOperatingRadius(int operatingRadius) { |
209 | this.operatingRadius = operatingRadius; |
209 | this.operatingRadius = operatingRadius; |
210 | } |
210 | } |
211 | public void setVerticalVelocityByGPS(int verticalVelocityByGPS) { |
211 | public void setVerticalVelocityByGPS(int verticalVelocityByGPS) { |
212 | this.verticalVelocityByGPS = verticalVelocityByGPS; |
212 | this.verticalVelocityByGPS = verticalVelocityByGPS; |
213 | } |
213 | } |
214 | public void setTargetLoiterTime(int targetLoiterTime) { |
214 | public void setTargetLoiterTime(int targetLoiterTime) { |
215 | this.targetLoiterTime = targetLoiterTime; |
215 | this.targetLoiterTime = targetLoiterTime; |
216 | } |
216 | } |
217 | public void setFcFlags2(int fcFlags2) { |
217 | public void setFcFlags2(int fcFlags2) { |
218 | this.fcFlags2 = fcFlags2; |
218 | this.fcFlags2 = fcFlags2; |
219 | } |
219 | } |
220 | public void setSetpointForAltitude(int setpointForAltitude) { |
220 | public void setSetpointForAltitude(double setpointForAltitude) { |
221 | this.setpointForAltitude = setpointForAltitude; |
221 | this.setpointForAltitude = setpointForAltitude; |
222 | } |
222 | } |
223 | public void setThrottle(int throttle) { |
223 | public void setThrottle(int throttle) { |
224 | this.throttle = throttle; |
224 | this.throttle = throttle; |
225 | } |
225 | } |
226 | public void setCurrent(int current) { |
226 | public void setCurrent(int current) { |
227 | this.current = current; |
227 | this.current = current; |
228 | } |
228 | } |
229 | public void setCapacityUsed(int capacityUsed) { |
229 | public void setCapacityUsed(int capacityUsed) { |
230 | this.capacityUsed = capacityUsed; |
230 | this.capacityUsed = capacityUsed; |
231 | } |
231 | } |
232 | 232 | ||
233 | @Override |
233 | @Override |
234 | public boolean isResponseTo(RequestFrame r) { |
234 | public boolean isResponseTo(RequestFrame r) { |
235 | return r instanceof OSDDataRequestFrame; |
235 | return r instanceof OSDDataRequestFrame; |
236 | } |
236 | } |
237 | 237 | ||
238 | public String toString() { |
238 | public String toString() { |
239 | StringBuilder result = new StringBuilder(); |
239 | StringBuilder result = new StringBuilder(); |
240 | result.append("OSDData version=" + this.version + "\n"); |
240 | result.append("OSDData version=" + this.version + "\n"); |
241 | result.append("currentPosition: " + this.currentPosition + "\n"); |
241 | result.append("currentPosition: " + this.currentPosition + "\n"); |
242 | result.append("targetPosition: " + this.targetPosition + " (" + this.currentToTarget + ")" + "\n"); |
242 | result.append("targetPosition: " + this.targetPosition + " (" + this.currentToTarget + ")" + "\n"); |
243 | result.append("homePosition: " + this.homePosition + " (" + this.currentToHome + ")" + "\n"); |
243 | result.append("homePosition: " + this.homePosition + " (" + this.currentToHome + ")" + "\n"); |
244 | result.append("heightByPressure: " + this.heightByPressure + "\n"); |
244 | result.append("heightByPressure: " + this.heightByPressure + "\n"); |
245 | result.append("heightByPressureReal: " + this.heightByPressure / 1.26 + "\n"); |
245 | result.append("heightByPressureReal: " + this.heightByPressure / 1.26 + "\n"); |
246 | result.append("verticalVelocityByPressure: " + this.verticalVelocityByPressure + "\n"); |
246 | result.append("verticalVelocityByPressure: " + this.verticalVelocityByPressure + "\n"); |
247 | result.append("verticalVelocityByGPS: " + this.verticalVelocityByGPS + "\n"); |
247 | result.append("verticalVelocityByGPS: " + this.verticalVelocityByGPS + "\n"); |
248 | result.append("direction of flight: " + this.directionOfFlight + "\n"); |
248 | result.append("direction of flight: " + this.directionOfFlight + "\n"); |
249 | result.append("compassHeading: " + this.compassHeading + "\n"); |
249 | result.append("compassHeading: " + this.compassHeading + "\n"); |
250 | result.append("pitchAngle: " + this.pitchAngle + "\n"); |
250 | result.append("pitchAngle: " + this.pitchAngle + "\n"); |
251 | result.append("rollAngle: " + this.rollAngle + "\n"); |
251 | result.append("rollAngle: " + this.rollAngle + "\n"); |
252 | result.append("battery voltage: " + ((double)this.batteryVoltage)/10 + "\n"); |
252 | result.append("battery voltage: " + ((double)this.batteryVoltage)/10 + "\n"); |
253 | result.append("throttle: " + this.throttle + "\n"); |
253 | result.append("throttle: " + this.throttle + "\n"); |
254 | return result.toString(); |
254 | return result.toString(); |
255 | } |
255 | } |
256 | } |
256 | } |
257 | 257 | ||
258 | 258 |