/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java |
---|
286,10 → 286,10 |
// h[m] = h[int] * 0.05 / 1.24 = h[int] |
f._setHeightByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
f.setVerticalVelocityByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); // clueless! |
f.setVerticalVelocityByPressure(base64InputStream.readSignedWord()); |
f.setFlightTime(base64InputStream.readWord()); |
f.setBatteryVoltage(base64InputStream.readByte()); |
f.setGroundSpeed(((double)base64InputStream.readWord()) / 1E2); |
f.setGroundSpeed(base64InputStream.readWord()); |
f.setDirectionOfFlight(base64InputStream.readSignedWord()); |
f.setCompassHeading(base64InputStream.readSignedWord()); |
303,7 → 303,7 |
f.setErrorCode(base64InputStream.readByte()); |
f.setOperatingRadius(base64InputStream.readByte()); |
f.setVerticalVelocityByGPS(((double)base64InputStream.readSignedWord()) / 1E2); |
f.setVerticalVelocityByGPS(base64InputStream.readSignedWord()); |
f.setTargetLoiterTime(base64InputStream.readByte()); |
f.setFcFlags2(base64InputStream.readByte()); |
f.setSetpointForAltitude(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/tcp/MKTCPServer.java |
---|
0,0 → 1,148 |
package dongfang.mkt.comm.tcp; |
import java.io.IOException; |
import java.io.InputStream; |
import java.io.OutputStream; |
import java.net.ServerSocket; |
import java.net.Socket; |
import java.net.SocketAddress; |
import java.util.HashMap; |
import java.util.HashSet; |
import java.util.Map; |
import java.util.Set; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
public class MKTCPServer { |
private MKConnection mkConnection; |
private ServerSocket serverSocket; |
private Map<SocketAddress, Socket> connections = new HashMap<SocketAddress, Socket>(); |
private int port; |
public MKTCPServer(MKConnection mkConnection, int port) throws IOException { |
this.mkConnection = mkConnection; |
serverSocket = new ServerSocket(port); |
this.port = port; |
} |
public void start() { |
ClientConnectionServer svr = new ClientConnectionServer(); |
svr.setDaemon(true); |
svr.start(); |
new Manifold().start(); |
System.out.println("Waiting for connections at port " + port); |
} |
class ClientConnectionServer extends Thread { |
public void run() { |
while (true) { |
try { |
// accept() will block, preventing performance loss bc of inf looping. |
Socket clientSocket = serverSocket.accept(); |
System.out.println("Accepted a connection from " + clientSocket.getRemoteSocketAddress()); |
connections.put(clientSocket.getRemoteSocketAddress(), clientSocket); |
new Mux(clientSocket.getInputStream()).start(); |
} catch (IOException ex) { |
// ignore. |
System.err.println(ex); |
} |
} |
} |
} |
/* |
* This thread accepts new client connections but does not do anything for |
* transferring data. |
*/ |
void send(byte[] buf, int len) throws IOException { |
synchronized (mkConnection.getOutputStream()) { |
String snak = new String(buf, 0, len); |
//System.out.println("Sending on behalf of client: " + snak); |
mkConnection.getOutputStream().write(buf, 0, len); |
} |
} |
/* |
* For each client, this takes the client's output and sends frames on |
* (without mixing them up) to the common connection. An instance of this |
* must be made and started for each client. |
*/ |
class Mux extends Thread { |
InputStream input; |
byte[] buffer = new byte[1024]; |
Mux(InputStream input) { |
this.input = input; |
} |
public void run() { |
while (true) { |
try { |
int r; |
while ((r = input.read()) != '#') { |
// can't be! There should be only legal frames coming. |
} |
buffer[0] = '#'; |
int c = 1; |
while ((r = input.read()) != '\r') { |
buffer[c++] = (byte) r; |
} |
buffer[c++] = '\r'; |
send(buffer, c); |
} catch (IOException ex) { |
} |
} |
} |
} |
class Manifold extends Thread { |
public void run() { |
byte[] buf = new byte[1024]; |
int read; |
InputStream is = null; |
try { |
is = mkConnection.getInputStream(); |
} catch (IOException ex) { |
throw new RuntimeException(ex); |
} |
while(true) { |
try { |
read = is.read(buf, 0, buf.length); |
String snak = new String(buf, 0, read); |
Set<SocketAddress> keys = new HashSet<SocketAddress>(connections.keySet()); |
// Will read() block or return zero when no data is available? |
if (read > 0) { |
//System.out.println("Received: " + snak); |
for(SocketAddress a: keys) { |
try { |
Socket connection = connections.get(a); |
OutputStream os = connection.getOutputStream(); |
os.write(buf, 0, read); |
} catch (IOException ex) { |
// ignore errors writing to each client. |
} |
} |
} else { |
try { |
Thread.sleep(10); |
} catch (InterruptedException ex) {} |
} |
} catch (IOException ex) { |
// ignore. |
} |
} |
} |
} |
public static void main(String[] args) throws IOException { |
int port = Integer.parseInt(args[0]); |
MKConnection c = new RXTXSerialPort(); |
c.init(null); |
MKTCPServer server = new MKTCPServer(c, port); |
server.start(); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/datatype/GPSPosition.java |
---|
45,16 → 45,37 |
} |
public String toXML() { |
String result = "latitude=\"" + latitude + "\""; |
result += " longitude=\"" + longitude + "\""; |
result += " altitude=\"" + altitude + "\""; |
double latitude = Math.abs(this.latitude); |
String result = "latitude=\"" + latitude; |
if (this.latitude < 0) |
result += "S"; |
else |
result += "N"; |
result += "\""; |
double longitude = Math.abs(this.longitude); |
result += " longitude=\"" + longitude; |
if (this.longitude < 0) |
result += "W"; |
else |
result += "E"; |
result += "\""; |
return result; |
} |
public String toString() { |
double latitude = Math.abs(this.latitude); |
String result = "" + latitude; |
result += "," + longitude; |
result += "," + altitude; |
if (this.latitude < 0) |
result += "S"; |
else |
result += "N"; |
result += " "; |
double longitude = Math.abs(this.longitude); |
result += "" + longitude; |
if (this.longitude < 0) |
result += "W"; |
else |
result += "E"; |
return result; |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/RequestFrameVisitor.java |
---|
File deleted |
\ No newline at end of file |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/OSDDataResponseFrame.java |
---|
17,12 → 17,12 |
private int numberOfSatellites; |
private double heightByPressure; // in m. |
private double verticalVelocityByPressure; // 16 bit signed. |
private int verticalVelocityByPressure; // 16 bit signed. |
private int flightTime; // in secs. 16 bit unsigned. |
private int batteryVoltage; // in 0.1 volts. |
private double groundSpeed; // in m/s. 16 bit unsigned. |
private int groundSpeed; // in cm/s. 16 bit unsigned. |
private int directionOfFlight; // of movement. 16 bit signed. |
private int compassHeading;// 16 bit signed. |
35,7 → 35,7 |
private int errorCode; |
private int operatingRadius; |
private double verticalVelocityByGPS; // 16 bit signed. |
private int verticalVelocityByGPS; // 16 bit signed. |
private int targetLoiterTime; |
private int fcFlags2; |
79,7 → 79,7 |
public double getHeightByPressure() { |
return heightByPressure; |
} |
public double getVerticalVelocityByPressure() { |
public int getVerticalVelocityByPressure() { |
return verticalVelocityByPressure; |
} |
public int getFlightTime() { |
88,7 → 88,7 |
public int getBatteryVoltage() { |
return batteryVoltage; |
} |
public double getGroundSpeed() { |
public int getGroundSpeed() { |
return groundSpeed; |
} |
public int getDirectionOfFlight() { |
118,7 → 118,7 |
public int getOperatingRadius() { |
return operatingRadius; |
} |
public double getVerticalVelocityByGPS() { |
public int getVerticalVelocityByGPS() { |
return verticalVelocityByGPS; |
} |
public int getTargetLoiterTime() { |
169,7 → 169,7 |
public void _setHeightByPressure(double heightByPressure) { |
this.heightByPressure = heightByPressure; |
} |
public void setVerticalVelocityByPressure(double verticalVelocityByPressure) { |
public void setVerticalVelocityByPressure(int verticalVelocityByPressure) { |
this.verticalVelocityByPressure = verticalVelocityByPressure; |
} |
public void setFlightTime(int flightTime) { |
178,7 → 178,7 |
public void setBatteryVoltage(int batteryVoltage) { |
this.batteryVoltage = batteryVoltage; |
} |
public void setGroundSpeed(double groundSpeed) { |
public void setGroundSpeed(int groundSpeed) { |
this.groundSpeed = groundSpeed; |
} |
public void setDirectionOfFlight(int heading) { |
208,7 → 208,7 |
public void setOperatingRadius(int operatingRadius) { |
this.operatingRadius = operatingRadius; |
} |
public void setVerticalVelocityByGPS(double verticalVelocityByGPS) { |
public void setVerticalVelocityByGPS(int verticalVelocityByGPS) { |
this.verticalVelocityByGPS = verticalVelocityByGPS; |
} |
public void setTargetLoiterTime(int targetLoiterTime) { |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MKTCPServer.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/SimpleOSDTool.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/OSDLogger.java |
---|
15,7 → 15,6 |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.comm.tcp.MKTCPConnection; |
import dongfang.mkt.frames.OSDDataRequestFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
65,12 → 64,10 |
w.write(" <currentPosition " + f.getCurrentPosition().toXML() + "/>\n"); |
w.write(" <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n"); |
w.write(" <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n"); |
w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + f.getGroundSpeed() + "\"/>\n"); |
w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + ((double)f.getGroundSpeed())/100 + "\"/>\n"); |
w.write(" <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n"); |
double gpsHeight = f.getCurrentPosition().getAltitude() - f.getHomePosition().getAltitude(); |
w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" gps=\"" + gpsHeight + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n"); |
w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n"); |
w.write(" <status rcQuality=\"" + f.getRcQuality() + "\" fcflags=\"" + f.getFcFlags() + "\" fcflags2=\"" + f.getFcFlags2() + "\" ncflags=\"" + f.getNcFlags() + "\" errorCode=\"" + f.getErrorCode() + "\" numberOfSatellites=\"" + f.getNumberOfSatellites() + "\"/>\n"); |
w.write(" <flight time=\"" + f.getFlightTime() + "\" batteryVoltage=\"" + f.getBatteryVoltage() + "\" throttle=\"" + f.getThrottle() + "\" current=\"" + f.getCurrent() + "\" capacityUsed=\"" + f.getCapacityUsed() + "\"/>\n"); |
163,8 → 160,8 |
public static void main(String[] args) throws IOException { |
OSDLogger test = new OSDLogger(); |
MKConnection _port = new MKTCPConnection(); //RXTXSerialPort(); |
_port.init("8080"); |
MKConnection _port = new RXTXSerialPort(); |
_port.init(null); |
FrameQueue q = new FrameQueue(_port); |
String encoding = "iso-8859-1"; |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/VariousInfoPane.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/OSDDataConsumer.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/SimpleOSDView.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/MapImageView.java |
---|
6,41 → 6,35 |
* house: geotagged geo:lat=47.324614 geo:lon=8.528202 |
*/ |
import java.awt.BasicStroke; |
import java.awt.Color; |
import java.awt.Font; |
import java.awt.Graphics; |
import java.awt.Graphics2D; |
import java.awt.Image; |
import java.awt.Toolkit; |
import java.util.Formatter; |
import java.util.Locale; |
import javax.swing.JFrame; |
import javax.swing.JPanel; |
import dongfang.mkt.datatype.GPSPosition; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
public class MapImageView extends JPanel implements OSDDataConsumer { |
public class MapImageView extends JPanel { |
private Image mapImage; |
private Image copterImage; |
private static final int MAPWIDTH = (int)(1132); |
private static final int MAPHEIGHT = (int)(977); |
private static final int MAPWIDTH = 600; |
private static final int MAPHEIGHT = 400; |
// It is assumed the map is north/south oriented! |
private GPSPosition upperLeft; |
private GPSPosition lowerRight; |
private GPSPosition copterPosition; |
private GPSPosition homePosition; |
private GPSPosition hightlightPosition; |
private int noseDirection; |
private int flightDirection; |
private double speed; |
private double height; |
private static final double degToRadianFactor = 360 / 2 / Math.PI; |
public MapImageView(GPSPosition upperLeft, GPSPosition lowerRight, String imageFileName) { |
setSize(MAPWIDTH, MAPHEIGHT); |
this.upperLeft = upperLeft; |
this.lowerRight = lowerRight; |
mapImage = Toolkit.getDefaultToolkit().getImage(imageFileName); |
} |
private double relativeX(GPSPosition pos) { |
double lon = pos.getLongitude(); |
47,9 → 41,7 |
// The sign should be OK here (positive) both east and west. |
double span = lowerRight.getLongitude() - upperLeft.getLongitude(); |
double offset = lon - upperLeft.getLongitude(); |
double result = offset / span; |
if (result<0) result = 0; else if(result>1) result=1; |
return result; |
return offset / span; |
} |
private double relativeY(GPSPosition pos) { |
57,106 → 49,40 |
// The sign should be OK here (positive) both east and west. |
double span = lowerRight.getLatitude() - upperLeft.getLatitude(); |
double offset = lat - upperLeft.getLatitude(); |
double result = offset / span; |
if (result<0) result = 0; else if(result>1) result=1; |
return result; |
return offset / span; |
} |
public void paintComponent(Graphics g) { |
int size = 32; |
int noseDotSize = 10; |
Graphics2D g2d = (Graphics2D)g; |
g2d.setStroke(new BasicStroke(5)); |
// Draw our Image object. |
g2d.drawImage(mapImage, 0, 0, MAPWIDTH, MAPHEIGHT, this); // at location |
if (homePosition != null) { |
g.setColor(Color.blue); |
int x = (int)(MAPWIDTH * relativeX(homePosition) + 0.5); |
int y = (int)(MAPHEIGHT * relativeY(homePosition) + 0.5); |
g.drawRect(x-size/2, y-size/2, size, size); |
} |
if (copterPosition != null) { |
int x = (int)(MAPWIDTH * relativeX(copterPosition) + 0.5); |
int y = (int)(MAPHEIGHT * relativeY(copterPosition) + 0.5); |
//g2d.clearRect(x-100, y-100, 200, 200); |
double speedfactor = 15; |
int speedArrowX = x + (int)(speedfactor * speed * Math.cos((90 - flightDirection) / degToRadianFactor)); |
int speedArrowY = y - (int)(speedfactor * speed * Math.sin((90 - flightDirection) / degToRadianFactor)); |
int noseDotX = x + (int)(size/2 * Math.cos((90 - noseDirection) / degToRadianFactor)); |
int noseDotY = y - (int)(size/2 * Math.sin((90 - noseDirection) / degToRadianFactor)); |
int noseDotEndX = x + (int)(size * Math.cos((90 - noseDirection) / degToRadianFactor)); |
int noseDotEndY = y - (int)(size * Math.sin((90 - noseDirection) / degToRadianFactor)); |
g2d.setColor(Color.yellow); |
g.drawLine(x, y, speedArrowX, speedArrowY); |
g2d.setColor(Color.red); |
g.drawArc(x-size/2, y-size/2, size, size, 0, 360); |
//g2d.setStroke(new BasicStroke(5)); |
g.drawLine(noseDotX, noseDotY, noseDotEndX, noseDotEndY); |
String heightFormatString = "%+10.1f"; |
StringBuilder sb = new StringBuilder(); |
Formatter formatter = new Formatter(sb, Locale.US); |
formatter.format(heightFormatString, height); |
g.setColor(Color.green); |
Font f = g2d.getFont(); |
f = f.deriveFont(18.0f); |
f = f.deriveFont(Font.BOLD); |
g2d.setFont(f); |
g.drawString(sb.toString(), x-120, y); |
} |
g.drawImage(mapImage, 0, 0, MAPWIDTH, MAPHEIGHT, this); // at location |
g.setColor(Color.red); |
int x = (int)(MAPWIDTH * relativeX(hightlightPosition) + 0.5); |
int y = (int)(MAPHEIGHT * relativeY(hightlightPosition) + 0.5); |
g.drawArc(x, y, 20, 20, 0, 360); |
} |
public void update(OSDDataResponseFrame data, long timestamp) { |
if (data==null) return; |
copterPosition = data.getCurrentPosition(); |
homePosition = data.getHomePosition(); |
speed = data.getGroundSpeed(); |
height = data.getHeightByPressure(); |
noseDirection = data.getCompassHeading(); |
flightDirection = data.getDirectionOfFlight(); |
repaint(); |
} |
public void init() { |
public static void main(String[] args) { |
GPSPosition upperLeft = new GPSPosition(); |
GPSPosition lowerRight = new GPSPosition(); |
GPSPosition highlight = new GPSPosition(); |
/* |
upperLeft.setLatitude(47.328355); |
upperLeft.setLongitude(8.518231); |
upperLeft.setLatitude(47.327450); |
upperLeft.setLongitude(8.521657); |
// * l/r: geotagged geo:lat=47.321749 geo:lon=8.534403 |
lowerRight.setLatitude(47.321532); |
lowerRight.setLongitude(8.530822); |
lowerRight.setLatitude(47.321749); |
lowerRight.setLongitude(8.534403); |
// geotagged geo:lat=47.324614 geo:lon=8.528202 |
highlight.setLatitude(47.324714); |
highlight.setLongitude(8.528102); |
*/ |
upperLeft.setLatitude(47.344577); |
upperLeft.setLongitude(8.529304); |
// * l/r: geotagged geo:lat=47.321749 geo:lon=8.534403 |
lowerRight.setLatitude(47.337576); |
lowerRight.setLongitude(8.542220); |
setSize(MAPWIDTH, MAPHEIGHT); |
this.upperLeft = upperLeft; |
this.lowerRight = lowerRight; |
mapImage = Toolkit.getDefaultToolkit().getImage("qth.jpg"); |
MapImageView v = new MapImageView(upperLeft, lowerRight, "flugplatz_small.png"); |
v.hightlightPosition = highlight; |
JFrame f = new JFrame(); |
f.getContentPane().add(v); |
f.setSize(v.getSize()); |
f.setVisible(true); |
} |
} |