Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1568 → Rev 1567

/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);
}
}