diff --git a/src/main/java/org/jointheleague/jcodrone/protocol/information/Address.java b/src/main/java/org/jointheleague/jcodrone/Address.java similarity index 51% rename from src/main/java/org/jointheleague/jcodrone/protocol/information/Address.java rename to src/main/java/org/jointheleague/jcodrone/Address.java index 245d995..32adf2a 100644 --- a/src/main/java/org/jointheleague/jcodrone/protocol/information/Address.java +++ b/src/main/java/org/jointheleague/jcodrone/Address.java @@ -1,9 +1,13 @@ -package org.jointheleague.jcodrone.protocol.information; +package org.jointheleague.jcodrone; import org.jointheleague.jcodrone.protocol.InvalidDataSizeException; import org.jointheleague.jcodrone.protocol.Serializable; +import org.jointheleague.jcodrone.protocol.link.LinkDiscoveredDevice; +import java.io.ByteArrayOutputStream; import java.nio.ByteBuffer; +import java.util.stream.Collectors; +import java.util.stream.Stream; public class Address implements Serializable { private final byte[] address; @@ -17,6 +21,26 @@ public static byte getSize() { return 6; } + public static Address parse(byte[] data) throws InvalidDataSizeException { + if (data.length != getSize()) { + throw new InvalidDataSizeException(getSize(), data.length); + } + + ByteBuffer buffer = ByteBuffer.wrap(data); + byte[] address = new byte[getSize()]; + buffer.get(address, 0, getSize()); + return new Address(address); + } + + public static Address parse(String address) { + byte[] bytes = Stream.of(address.split(":")).mapToInt(i -> Integer.parseInt(i, 16)). + collect(ByteArrayOutputStream::new, (baos, i) -> baos.write((byte) i), + (baos1, baos2) -> baos1.write(baos2.toByteArray(), 0, baos2.size())) + .toByteArray(); + + return new Address(bytes); + } + public byte getInstanceSize() { return getSize(); } @@ -28,15 +52,12 @@ public byte[] toArray() { return buffer.array(); } - public static Address parse(byte[] data) throws InvalidDataSizeException { - if (data.length != getSize()) { - throw new InvalidDataSizeException(getSize(), data.length); - } - - ByteBuffer buffer = ByteBuffer.wrap(data); - byte[] address = new byte[6]; - buffer.get(address, 0, getSize()); - return new Address(address); + public byte[] getAddress() { + return address; } + @Override + public String toString() { + return LinkDiscoveredDevice.intStream(address).mapToObj(i -> String.format("%02X", 0x0FF & i)).collect(Collectors.joining(":")); + } } diff --git a/src/main/java/org/jointheleague/jcodrone/CoDrone.java b/src/main/java/org/jointheleague/jcodrone/CoDrone.java index 92529a2..d5b8b37 100644 --- a/src/main/java/org/jointheleague/jcodrone/CoDrone.java +++ b/src/main/java/org/jointheleague/jcodrone/CoDrone.java @@ -157,44 +157,57 @@ public void close() { * * @throws CoDroneNotFoundException Thrown if no drone is found. Check logs to determine what serial port was used. */ - public void connect() throws CoDroneNotFoundException, MessageNotSentException { - connect(null, null, false); + public void connect() throws CoDroneNotFoundException, MessageNotSentException, InterruptedException { + connectPort(null); + link(null); } /** - * This connect method allows the connection to a specific drone over bluetooth via the controller connected - * to the specified serial por. Additionally the controller can be reset before the connection is attempted. + * The default connection method connects to the nearest drone connected to the last available serial port + * enumerated by the operating system. * - * @param portName System name of the serial port connected to the controller. - * @param deviceName The name of the drone to connect to. - * @param resetSystem Resets the controller before attempting to connect to the drone. - * @throws CoDroneNotFoundException Thrown if the specified port or drone can not be found. + * @throws CoDroneNotFoundException Thrown if no drone is found. Check logs to determine what serial port was used. */ - @SuppressWarnings("WeakerAccess") - public void connect(String portName, String deviceName, boolean resetSystem) throws CoDroneNotFoundException, MessageNotSentException { + public void connect(String deviceName) { if (deviceName != null && !deviceName.isEmpty() && deviceName.length() != 12) { throw new IllegalArgumentException( String.format("Invalid device name length %s.", deviceName.length())); } + connectPort(null); + } + + /** + * The default connection method connects to the nearest drone connected to the last available serial port + * enumerated by the operating system. + * + * @throws CoDroneNotFoundException Thrown if no drone is found. Check logs to determine what serial port was used. + */ + public void connect(Address address) { + connectPort(null); + } + + /** + * This connect method allows the connection to a specific drone over bluetooth via the controller connected + * to the specified serial port. Additionally the controller can be reset before the connection is attempted. + * + * @param portName System name of the serial port connected to the controller. + * @throws CoDroneNotFoundException Thrown if the specified port or drone can not be found. + */ + private void connectPort(String portName) { if (comPort == null || !comPort.isOpen()) { if (portName == null || portName.isEmpty()) { open(); } else { open(portName); - //Thread.sleep(100); } } else if (!portName.equalsIgnoreCase(comPort.getSystemPortName())) { close(); open(portName); - //Thread.sleep(100); - } - - // system reset - if (resetSystem) { - link.resetSystem(); } + } + private void link(String deviceName) throws MessageNotSentException, CoDroneNotFoundException, InterruptedException { link.connect(deviceName); } @@ -221,6 +234,12 @@ void transfer(Header header, Serializable data) { comPort.writeBytes(message.array(), message.capacity()); log.info("Sent: {}", DatatypeConverter.printHexBinary(message.array())); + // TODO: Remove this wait + try { + Thread.sleep(60); + } catch (InterruptedException e) { + log.warn("Sleep interrupted after sending command."); + } } @@ -233,7 +252,7 @@ void transfer(Header header, Serializable data) { * * @param type The command from the CommandType enumeration. */ - public void sendCommand(CommandType type) throws MessageNotSentException { + public void sendCommand(CommandType type) { sendCommand(type, (byte) 0); } @@ -431,6 +450,7 @@ public void underAttack() throws MessageNotSentException { public void flyDirect(DirectControl control) { Flight.flyDirect(this, control); } + /** * Sets a single light mode. * @@ -625,4 +645,8 @@ public TrimFlight getTrimFlight() { public boolean isFlightMode() { return (internals.getState() != null && internals.getState().isFlightMode()); } + + public void requestLinkState() throws MessageNotSentException { + link.requestState(); + } } diff --git a/src/main/java/org/jointheleague/jcodrone/Link.java b/src/main/java/org/jointheleague/jcodrone/Link.java index a49550c..97d6311 100644 --- a/src/main/java/org/jointheleague/jcodrone/Link.java +++ b/src/main/java/org/jointheleague/jcodrone/Link.java @@ -2,61 +2,120 @@ import org.apache.logging.log4j.LogManager; import org.apache.logging.log4j.Logger; -import org.jointheleague.jcodrone.protocol.information.Address; +import org.jointheleague.jcodrone.protocol.DataType; import org.jointheleague.jcodrone.protocol.link.LinkDiscoveredDevice; +import org.jointheleague.jcodrone.system.ModeLink; import org.jointheleague.jcodrone.system.ModeLinkBroadcast; import java.util.ArrayList; +import java.util.Arrays; import java.util.Comparator; import java.util.Optional; +import java.util.concurrent.TimeUnit; +import java.util.concurrent.locks.Condition; +import java.util.concurrent.locks.ReentrantLock; public class Link { + public static final ModeLinkBroadcast BROADCAST_MODE = ModeLinkBroadcast.ACTIVE; + private static Logger log = LogManager.getLogger(Link.class); private final CoDrone codrone; private Address address; private ArrayList devices; - private boolean discoveringDevices; + private final ReentrantLock deviceDiscoveryLock = new ReentrantLock(); + private Condition discovery = deviceDiscoveryLock.newCondition(); private boolean connected; + private ModeLink mode = ModeLink.NONE; + private ModeLinkBroadcast broadcastMode = ModeLinkBroadcast.NONE; + private boolean scanning = false; public Link(CoDrone codrone) { this.codrone = codrone; } - public void connect(String deviceName) throws CoDroneNotFoundException, MessageNotSentException { + public void connect(String deviceName) throws CoDroneNotFoundException, MessageNotSentException, InterruptedException { + connect(deviceName, null); + } + + + public void connect(Address address) throws CoDroneNotFoundException, MessageNotSentException, InterruptedException { + connect(null, address); + } + + + public void connect(String deviceName, Address address) throws CoDroneNotFoundException, MessageNotSentException, InterruptedException { // ModeLinkBroadcast.Passive mode change - setBroadcastMode(ModeLinkBroadcast.PASSIVE); - try { - Thread.sleep(100); - } catch (InterruptedException e) { - e.printStackTrace(); + requestState(); + for (int retries = 0; retries < 3 && getLinkBroadcastMode() != BROADCAST_MODE; retries++) { + setBroadcastMode(BROADCAST_MODE); + TimeUnit.MILLISECONDS.sleep(100); + requestState(); + } + + if (getLinkBroadcastMode() != BROADCAST_MODE) { + log.error("Could not switch link to selected broadcast mode. Is red LED flashing?"); + throw new CoDroneNotFoundException(); + } + + if (mode == ModeLink.CONNECTED) { + // TODO If we are currently corrected to the correct drone. +// if ((deviceName != null && !deviceName.isEmpty())) { +// } else if (address != null) { +// } else { + disconnect(); +// } } // start searching device devices = new ArrayList<>(20); - discoveringDevices = true; + deviceDiscoveryLock.lock(); + log.debug("Starting discovery."); + clearDevices(); startDiscovery(); - - // wait for 5sec - for (int i = 0; i < 50; i++) { - try { - Thread.sleep(100); - } catch (InterruptedException e) { + if (discovery.await(5, TimeUnit.SECONDS)) { + if (!scanning) { + log.error("Scanning started message was not received."); + throw new CoDroneNotFoundException(); } - if (!discoveringDevices) break; + } else { + log.error("Timeout waiting for scanning to start."); + throw new CoDroneNotFoundException(); } - try { - Thread.sleep(2000); - } catch (InterruptedException e) { + + if (discovery.await(5, TimeUnit.SECONDS)) { + if (scanning) { + log.error("Scanning did not complete as expected."); + throw new CoDroneNotFoundException(); + } + } else { + log.error("Timeout waiting for scanning to end."); + throw new CoDroneNotFoundException(); } + TimeUnit.SECONDS.sleep(1); + if (devices.size() == 0) { throw new CoDroneNotFoundException(); } LinkDiscoveredDevice targetDevice; - if (deviceName == null || deviceName.isEmpty()) { + if (deviceName != null && !deviceName.isEmpty()) { + Optional matchingDevice = devices.stream().filter(p -> p.getName().substring(0, 12).equals(deviceName.substring(0, 12))).findFirst(); + if (matchingDevice.isPresent()) { + targetDevice = matchingDevice.get(); + } else { + throw new CoDroneNotFoundException(deviceName); + } + } else if (address != null) { + Optional matchingDevice = devices.stream().filter(p -> Arrays.equals(p.getAddress(), address.getAddress())).findFirst(); + if (matchingDevice.isPresent()) { + targetDevice = matchingDevice.get(); + } else { + throw new CoDroneNotFoundException(deviceName); + } + } else { Optional closestDevice = devices.stream().max(new Comparator() { @Override public int compare(LinkDiscoveredDevice o1, LinkDiscoveredDevice o2) { @@ -68,17 +127,13 @@ public int compare(LinkDiscoveredDevice o1, LinkDiscoveredDevice o2) { } else { throw new CoDroneNotFoundException(); } - } else{ - Optional matchingDevice = devices.stream().filter(p -> p.getName().substring(0,12).equals(deviceName.substring(0,12))).findFirst(); - if (matchingDevice.isPresent()) { - targetDevice = matchingDevice.get(); - } else { - throw new CoDroneNotFoundException(deviceName); - } } + log.debug("Connecting to {}[{}]", targetDevice.getNameString(), targetDevice.getFormattedAddress()); + setBroadcastMode(ModeLinkBroadcast.PASSIVE); connected = false; connectToIndex(targetDevice.getIndex()); + // TODO Wait for connected for (int i = 0; i < 50; i++) { try { Thread.sleep(100); @@ -86,10 +141,12 @@ public int compare(LinkDiscoveredDevice o1, LinkDiscoveredDevice o2) { } if (connected) break; } - try { - Thread.sleep(1200); - } catch (InterruptedException e) { - } + + // TODO Wait for ready to command + } + + public void disconnect() throws MessageNotSentException { + codrone.sendCommand(CommandType.LINK_DISCONNECT); } public void resetSystem() throws MessageNotSentException { @@ -102,15 +159,11 @@ public void resetSystem() throws MessageNotSentException { } } - public void connectToIndex(byte index) throws MessageNotSentException { + public void connectToIndex(byte index) { codrone.sendCommand(CommandType.LINK_CONNECT, index); } - public void disconnect() throws MessageNotSentException { - codrone.sendCommand(CommandType.LINK_DISCONNECT); - } - - public void setBroadcastMode(ModeLinkBroadcast mode) throws MessageNotSentException { + public void setBroadcastMode(ModeLinkBroadcast mode) { codrone.sendCommand(CommandType.LINK_MODE_BROADCAST, mode.value()); } @@ -134,10 +187,6 @@ public void clearDevices() { devices = new ArrayList<>(20); } - public void setDiscoveringDevices(boolean discoveringDevices) { - this.discoveringDevices = discoveringDevices; - } - public void setConnected(boolean connected) { this.connected = connected; } @@ -149,4 +198,37 @@ public void addDevice(LinkDiscoveredDevice device) { public Address getAddress() { return address; } + + public void requestState() { + codrone.sendCommand(CommandType.REQUEST, DataType.LINK_STATE.value()); + } + + + public ModeLinkBroadcast getLinkBroadcastMode() { + return broadcastMode; + } + + public void setStartedDiscovery() { + log.debug("Notify scan started."); + deviceDiscoveryLock.lock(); + scanning = true; + discovery.signal(); + deviceDiscoveryLock.unlock(); + } + + public void setStoppedDiscovery() { + log.debug("Notify scan stopped."); + deviceDiscoveryLock.lock(); + scanning = false; + discovery.signal(); + deviceDiscoveryLock.unlock(); + } + + public void setCurrentMode(ModeLink mode) { + this.mode = mode; + } + + public void setCurrentBroadcastMode(ModeLinkBroadcast broadcastMode) { + this.broadcastMode = broadcastMode; + } } \ No newline at end of file diff --git a/src/main/java/org/jointheleague/jcodrone/examples/TestPorts.java b/src/main/java/org/jointheleague/jcodrone/examples/TestPorts.java index ffa76f0..6722816 100644 --- a/src/main/java/org/jointheleague/jcodrone/examples/TestPorts.java +++ b/src/main/java/org/jointheleague/jcodrone/examples/TestPorts.java @@ -1,11 +1,12 @@ package org.jointheleague.jcodrone.examples; import org.jointheleague.jcodrone.CoDrone; -import org.jointheleague.jcodrone.DirectControl; import org.jointheleague.jcodrone.LightModeBuilder; import org.jointheleague.jcodrone.protocol.light.LightMode; import org.jointheleague.jcodrone.protocol.light.LightModeDrone; +import java.awt.*; + public class TestPorts { public static void main(String argx[]) { try (CoDrone drone = new CoDrone()){ @@ -36,16 +37,33 @@ public static void main(String argx[]) { drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) -55, -15)); Thread.sleep(2000); } - drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) 0, (byte) 0)); - Thread.sleep(200); - LightMode mode = new LightModeBuilder().setColor("BLUE").setInterval(5).setMode(LightModeDrone.EYE_HOLD).build(); - drone.lightMode(mode); - Thread.sleep(1000); - mode = new LightModeBuilder().setColor("green").setInterval(200).setMode(LightModeDrone.EYE_DIMMING).build(); - drone.lightMode(mode); - Thread.sleep(3000); - drone.land(); - Thread.sleep(1000); +// drone.takeoff(); +// Thread.sleep(2000); +// drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) 0, 30)); +// Thread.sleep(2000); +// +// for (int i = 0; i < 3; i++) { +// LightMode mode = new LightModeBuilder().setColor("BLUE").setInterval(5).setMode(LightModeDrone.EYE_HOLD).build(); +// drone.lightMode(mode); +// Thread.sleep(100); +// drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) 55, 30)); +// Thread.sleep(2000); +// mode = new LightModeBuilder().setColor("RED").setInterval(5).setMode(LightModeDrone.EYE_HOLD).build(); +// drone.lightMode(mode); +// Thread.sleep(100); +// drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) -55, -15)); +// Thread.sleep(2000); +// } +// drone.flyDirect(new DirectControl((byte) 0, (byte) 0, (byte) 0, (byte) 0)); +// Thread.sleep(200); +// LightMode mode = new LightModeBuilder().setColor("BLUE").setInterval(5).setMode(LightModeDrone.EYE_HOLD).build(); +// drone.lightMode(mode); +// Thread.sleep(1000); +// mode = new LightModeBuilder().setColor("green").setInterval(200).setMode(LightModeDrone.EYE_DIMMING).build(); +// drone.lightMode(mode); +// Thread.sleep(3000); +// drone.land(); +// Thread.sleep(10000); } catch (Exception e) { e.printStackTrace(); } diff --git a/src/main/java/org/jointheleague/jcodrone/protocol/DataType.java b/src/main/java/org/jointheleague/jcodrone/protocol/DataType.java index 74b0f34..a8bd5c9 100644 --- a/src/main/java/org/jointheleague/jcodrone/protocol/DataType.java +++ b/src/main/java/org/jointheleague/jcodrone/protocol/DataType.java @@ -1,11 +1,15 @@ package org.jointheleague.jcodrone.protocol; +import org.jointheleague.jcodrone.Address; import org.jointheleague.jcodrone.protocol.common.*; import org.jointheleague.jcodrone.protocol.information.*; import org.jointheleague.jcodrone.protocol.light.*; import org.jointheleague.jcodrone.protocol.link.*; import org.jointheleague.jcodrone.protocol.message.Message; -import org.jointheleague.jcodrone.protocol.update.*; +import org.jointheleague.jcodrone.protocol.update.Update; +import org.jointheleague.jcodrone.protocol.update.UpdateInformation; +import org.jointheleague.jcodrone.protocol.update.UpdateLocationCorrect; +import org.jointheleague.jcodrone.protocol.update.UpdateLookupTarget; import java.util.HashMap; import java.util.Map; diff --git a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkDiscoveredDevice.java b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkDiscoveredDevice.java index 13048a4..c297308 100644 --- a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkDiscoveredDevice.java +++ b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkDiscoveredDevice.java @@ -86,15 +86,19 @@ public static IntStream intStream(byte[] array) { @Override public void handle(CoDrone coDrone, Link link, Sensors sensors, Internals internals) { - log.debug("Device {} found with address {}", this.getNameString(), this.getFormattedAddress()); + log.debug("Device {} [{}] RSSI: {}", this.getNameString(), this.getFormattedAddress(), this.rssi); link.addDevice(this); } public String getNameString() { - return new String(name); + return new String(name).trim(); } public String getFormattedAddress() { return intStream(address).mapToObj(x -> Integer.toHexString(0x0FF & x)).collect(Collectors.joining(":")); } + + public byte[] getAddress() { + return address; + } } diff --git a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkEvent.java b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkEvent.java index 2eaa964..cbb0b3c 100644 --- a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkEvent.java +++ b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkEvent.java @@ -54,10 +54,10 @@ public void handle(CoDrone coDrone, Link link, Sensors sensors, Internals intern switch (event) { case SCANNING: link.clearDevices(); - link.setDiscoveringDevices(true); + link.setStartedDiscovery(); break; case SCAN_STOP: - link.setDiscoveringDevices(false); + link.setStoppedDiscovery(); break; case CONNECTED: link.setConnected(true); diff --git a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkState.java b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkState.java index 987435b..9a4793b 100644 --- a/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkState.java +++ b/src/main/java/org/jointheleague/jcodrone/protocol/link/LinkState.java @@ -1,5 +1,11 @@ package org.jointheleague.jcodrone.protocol.link; +import org.apache.logging.log4j.LogManager; +import org.apache.logging.log4j.Logger; +import org.jointheleague.jcodrone.CoDrone; +import org.jointheleague.jcodrone.Internals; +import org.jointheleague.jcodrone.Link; +import org.jointheleague.jcodrone.Sensors; import org.jointheleague.jcodrone.protocol.InvalidDataSizeException; import org.jointheleague.jcodrone.protocol.Serializable; import org.jointheleague.jcodrone.system.ModeLink; @@ -8,6 +14,7 @@ import java.nio.ByteBuffer; public class LinkState implements Serializable { + private static Logger log = LogManager.getLogger(LinkState.class); private final ModeLink mode; private final ModeLinkBroadcast broadcast; @@ -43,4 +50,15 @@ public static LinkState parse(byte[] data) throws InvalidDataSizeException { ModeLinkBroadcast broadcast = ModeLinkBroadcast.fromByte(buffer.get()); return new LinkState(mode, broadcast); } + + @Override + public void handle(CoDrone coDrone, Link link, Sensors sensors, Internals internals) { + link.setCurrentMode(this.mode); + link.setCurrentBroadcastMode(this.broadcast); + log.debug("Link Mode: {} Broadcast Mode: {} ", this.mode.name(), this.broadcast.name()); + } + + public ModeLinkBroadcast getBroadcastMode() { + return broadcast; + } } diff --git a/src/test/java/org/jointheleague/jcodrone/protocol/information/AddressTest.java b/src/test/java/org/jointheleague/jcodrone/protocol/information/AddressTest.java new file mode 100644 index 0000000..ab08652 --- /dev/null +++ b/src/test/java/org/jointheleague/jcodrone/protocol/information/AddressTest.java @@ -0,0 +1,59 @@ +package org.jointheleague.jcodrone.protocol.information; + +import org.jointheleague.jcodrone.Address; +import org.junit.Rule; +import org.junit.Test; +import org.junit.rules.ExpectedException; + +import static org.junit.Assert.assertArrayEquals; +import static org.junit.Assert.assertEquals; + +public class AddressTest { + + @Rule + public ExpectedException thrown = ExpectedException.none(); + + @Test + public void parseDecimal() { + Address address = Address.parse("01:02:03:04:05:06"); + byte[] bytes = address.toArray(); + + assertArrayEquals(new byte[]{1, 2, 3, 4, 5, 6}, bytes); + } + + @Test + public void parseUpperHex() { + Address address = Address.parse("0A:0B:0C:0D:0E:0F"); + byte[] bytes = address.toArray(); + + assertArrayEquals(new byte[]{10, 11, 12, 13, 14, 15}, bytes); + } + + @Test + public void parseLowerHex() { + Address address = Address.parse("0a:0b:0c:0d:0e:0f"); + byte[] bytes = address.toArray(); + + assertArrayEquals(new byte[]{10, 11, 12, 13, 14, 15}, bytes); + } + + @Test + public void parseFailure() throws NumberFormatException { + thrown.expect(NumberFormatException.class); + thrown.expectMessage("For input string: \"No\""); + Address address = Address.parse("No:t :va:li:d!:!!"); + byte[] bytes = address.toArray(); + } + + @Test + public void getStringDecimal() { + Address address = new Address(new byte[]{1, 2, 3, 4, 5, 6}); + assertEquals("01:02:03:04:05:06", address.toString()); + } + + @Test + public void getStringHex() { + Address address = new Address(new byte[]{(byte) 0xDE, (byte) 0xAD, (byte) 0xBE, (byte) 0xEF, (byte) 0xCA, (byte) 0xFE}); + assertEquals("DE:AD:BE:EF:CA:FE", address.toString()); + } +} \ No newline at end of file