diff --git a/plugins-dev/sunfish/src/java/pt/lsts/neptus/plugins/sunfish/iridium/feedback/IridiumStatusTableModel.java b/plugins-dev/sunfish/src/java/pt/lsts/neptus/plugins/sunfish/iridium/feedback/IridiumStatusTableModel.java index 0b5778c537..c846a44dad 100644 --- a/plugins-dev/sunfish/src/java/pt/lsts/neptus/plugins/sunfish/iridium/feedback/IridiumStatusTableModel.java +++ b/plugins-dev/sunfish/src/java/pt/lsts/neptus/plugins/sunfish/iridium/feedback/IridiumStatusTableModel.java @@ -39,6 +39,7 @@ import java.util.List; import java.util.Map; import java.util.Map.Entry; +import java.util.Objects; import java.util.TimeZone; import javax.swing.table.AbstractTableModel; @@ -55,6 +56,7 @@ import pt.lsts.neptus.comm.iridium.ImcIridiumMessage; import pt.lsts.neptus.comm.iridium.IridiumCommand; import pt.lsts.neptus.comm.iridium.IridiumMessage; +import pt.lsts.neptus.comm.manager.imc.ImcId16; import pt.lsts.neptus.comm.manager.imc.ImcMsgManager; import pt.lsts.neptus.i18n.I18n; import pt.lsts.neptus.messages.TypedMessageFilter; @@ -91,8 +93,18 @@ public void onMessage(MessageInfo info, IMCMessage msg) { if (msg.getMgid() == IridiumMsgRx.ID_STATIC) { IridiumMessage m; try { + Date now = new Date(); m = IridiumMessage.deserialize(msg.getRawData("data")); //msgs.addElement(m); // m.source == ImcMsgManager.getManager().getLocalId().intValue() + if (!new Date(m.timestampMillis).before(now) && !Objects.equals(msg.getDate(), new Date(0))) { + m.timestampMillis = msg.getDate().getTime(); + } + if (m.source == ImcId16.NULL_ID.intValue()) { + m.source = msg.getSrc(); + } + if (m.destination == ImcId16.NULL_ID.intValue()) { + m.destination = msg.getDst(); + } msgs.add(m); fireTableRowsInserted(msgs.size()-1, msgs.size()-1); } @@ -112,7 +124,17 @@ public void onMessage(MessageInfo info, IMCMessage msg) { else if (msg.getMgid() == IridiumMsgTx.ID_STATIC) { IridiumMessage m; try { + Date now = new Date(); m = IridiumMessage.deserialize(msg.getRawData("data")); + if (!new Date(m.timestampMillis).before(now) && !Objects.equals(msg.getDate(), new Date(0))) { + m.timestampMillis = msg.getDate().getTime(); + } + if (m.source == ImcId16.NULL_ID.intValue()) { + m.source = msg.getSrc(); + } + if (m.destination == ImcId16.NULL_ID.intValue()) { + m.destination = msg.getDst(); + } synchronized (msgs) { msgs.add(m); if (msg.getSrc() == ImcMsgManager.getManager().getLocalId().intValue()) { // Only keeps local diff --git a/src/java/pt/lsts/neptus/comm/iridium/DuneIridiumMessenger.java b/src/java/pt/lsts/neptus/comm/iridium/DuneIridiumMessenger.java index 47268f2e1a..90c2539017 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/DuneIridiumMessenger.java +++ b/src/java/pt/lsts/neptus/comm/iridium/DuneIridiumMessenger.java @@ -38,7 +38,8 @@ import java.util.Collections; import java.util.Date; import java.util.HashSet; -import java.util.Vector; +import java.util.List; +import java.util.Set; import pt.lsts.imc.IMCDefinition; import pt.lsts.imc.IMCInputStream; @@ -67,9 +68,7 @@ public class DuneIridiumMessenger implements IridiumMessenger, MessageListener messagesReceived = new Vector<>(); - - protected HashSet listeners = new HashSet<>(); + protected Set listeners = new HashSet<>(); @Override public void addListener(IridiumMessageListener listener) { @@ -91,7 +90,6 @@ public void onMessage(MessageInfo info, IMCMessage msg) { if (msg.getMgid() == IridiumMsgRx.ID_STATIC) { try { IridiumMessage m = IridiumMessage.deserialize(msg.getRawData("data")); - messagesReceived.add(m); NeptusLog.pub().info("Received a " + m.getClass().getSimpleName() + " from " + msg.getSourceName()); for (IridiumMessageListener listener : listeners) listener.messageReceived(m); @@ -119,21 +117,15 @@ else if (msg.getMgid() == IridiumTxStatus.ID_STATIC) { } } + private static void sendToProvider(IridiumMsgTx tx, ImcSystem system) throws Exception { + if (!ImcMsgManager.getManager().sendMessageToSystem(tx, system.getName())) + throw new Exception("Error while sending message to " + system.getName() + " via IMC."); + } + @Override public void sendMessage(IridiumMessage msg) throws Exception { + ImcSystem system = getProviderToUse(); - ArrayList providers = new ArrayList(); - providers.addAll(getIridiumServiceProviders()); - - if (providers.isEmpty()) { - throw new Exception("No Iridium service providers are available"); - } - - providers.sort(Collections.reverseOrder()); - ImcSystem system = ImcSystemsHolder.lookupSystemByName(providers.iterator().next()); - - System.out.println("Subscribed to Iridium Device Updates through "+system.getName()); - // Activate and deactivate subscriptions should use the id of the used gateway //if (msg instanceof ActivateSubscription || msg instanceof DeactivateSubscription) { // ImcSystem system = ImcSystemsHolder.lookupSystemByName(messengerName); @@ -145,10 +137,38 @@ public void sendMessage(IridiumMessage msg) throws Exception { tx.setReqId((++req_id % 65535)); tx.setTtl(3600); tx.setData(msg.serialize()); - if (!ImcMsgManager.getManager().sendMessageToSystem(tx, system.getName())) - throw new Exception("Error while sending message to " + system.getName() + " via IMC."); + sendToProvider(tx, system); } - + + @Override + public void sendMessageRaw(String destinationName, String destinationAddr, byte[] data) throws Exception { + ImcSystem system = getProviderToUse(); + + // Activate and deactivate subscriptions should use the id of the used gateway + //if (msg instanceof ActivateSubscription || msg instanceof DeactivateSubscription) { + // ImcSystem system = ImcSystemsHolder.lookupSystemByName(messengerName); + // if (system != null) + // msg.setSource(system.getId().intValue()); + + IridiumMsgTx tx = new IridiumMsgTx(); + tx.setReqId((++req_id % 65535)); + tx.setTtl(3600); + tx.setData(data); + sendToProvider(tx, system); + } + + private ImcSystem getProviderToUse() throws Exception { + List providers = new ArrayList<>(getIridiumServiceProviders()); + if (providers.isEmpty()) { + throw new Exception("No Iridium service providers are available"); + } + + providers.sort(Collections.reverseOrder()); + ImcSystem system = ImcSystemsHolder.lookupSystemByName(providers.iterator().next()); + NeptusLog.pub().info("Subscribed to Iridium Device Updates through {}", system.getName()); + return system; + } + public Collection getIridiumServiceProviders() { ArrayList names = new ArrayList<>(); ImcSystem[] providers = ImcSystemsHolder.lookupSystemByService("iridium", SystemTypeEnum.ALL, true); @@ -162,7 +182,7 @@ public Collection getIridiumServiceProviders() { @Override public Collection pollMessages(Date timeSince) throws Exception { - return new Vector<>(); + return Collections.emptyList(); } @Override @@ -178,12 +198,10 @@ public boolean isAvailable() { @Override public void cleanup() { listeners.clear(); - messagesReceived.clear(); } @Override public String toString() { return getName(); } - } diff --git a/src/java/pt/lsts/neptus/comm/iridium/HubIridiumMessenger.java b/src/java/pt/lsts/neptus/comm/iridium/HubIridiumMessenger.java index 59151ce24c..005bccc177 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/HubIridiumMessenger.java +++ b/src/java/pt/lsts/neptus/comm/iridium/HubIridiumMessenger.java @@ -32,10 +32,25 @@ */ package pt.lsts.neptus.comm.iridium; +import com.google.gson.Gson; +import org.apache.commons.codec.binary.Hex; +import org.apache.commons.io.IOUtils; +import pt.lsts.neptus.NeptusLog; +import pt.lsts.neptus.comm.iridium.Position.PosType; +import pt.lsts.neptus.comm.manager.imc.ImcId16; +import pt.lsts.neptus.comm.manager.imc.ImcSystem; +import pt.lsts.neptus.comm.manager.imc.ImcSystemsHolder; +import pt.lsts.neptus.types.coord.LocationType; +import pt.lsts.neptus.types.vehicle.VehicleType; +import pt.lsts.neptus.types.vehicle.VehiclesHolder; +import pt.lsts.neptus.util.ByteUtil; +import pt.lsts.neptus.util.conf.GeneralPreferences; + import java.io.BufferedWriter; import java.io.ByteArrayOutputStream; import java.io.File; import java.io.FileWriter; +import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; import java.io.OutputStream; @@ -44,29 +59,19 @@ import java.net.URLConnection; import java.text.DateFormat; import java.text.SimpleDateFormat; +import java.util.ArrayList; import java.util.Calendar; import java.util.Collection; import java.util.Date; import java.util.GregorianCalendar; import java.util.HashSet; +import java.util.List; +import java.util.Locale; +import java.util.Set; import java.util.TimeZone; -import java.util.Vector; import java.util.regex.Matcher; import java.util.regex.Pattern; -import org.apache.commons.codec.binary.Hex; -import org.apache.commons.io.IOUtils; - -import com.google.gson.Gson; - -import pt.lsts.neptus.NeptusLog; -import pt.lsts.neptus.comm.iridium.Position.PosType; -import pt.lsts.neptus.comm.manager.imc.ImcSystem; -import pt.lsts.neptus.comm.manager.imc.ImcSystemsHolder; -import pt.lsts.neptus.types.coord.LocationType; -import pt.lsts.neptus.util.ByteUtil; -import pt.lsts.neptus.util.conf.GeneralPreferences; - /** * @author zp * @@ -81,8 +86,9 @@ public class HubIridiumMessenger implements IridiumMessenger { protected String systemsUrl = serverUrl+"systems"; protected String activeSystemsUrl = systemsUrl+"/active"; protected String messagesUrl = serverUrl+"iridium"; + protected String messagesRawUrl = serverUrl+"iridium/raw"; protected int timeoutMillis = 10000; - protected HashSet listeners = new HashSet<>(); + protected Set listeners = new HashSet<>(); private static final Pattern p = Pattern.compile("\\((.)\\) \\((.*)\\) (.*) / (.*), (.*) / .*"); private static final TimeZone tz = TimeZone.getTimeZone("UTC"); private static final DateFormat dateFormat = new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss"); @@ -127,47 +133,82 @@ public void addListener(IridiumMessageListener listener) { public void removeListener(IridiumMessageListener listener) { listeners.remove(listener); } - - @Override - public void sendMessage(IridiumMessage msg) throws Exception { - - byte[] data = msg.serialize(); - data = new String(Hex.encodeHex(data)).getBytes(); - - URL u = new URL(messagesUrl); + + private HttpURLConnection getHttpURLConnection(String url) throws IOException { + URL u = new URL(url); HttpURLConnection conn = (HttpURLConnection) u.openConnection(); conn.setDoOutput(true); conn.setRequestMethod( "POST" ); conn.setRequestProperty( "Content-Type", "application/hub" ); - conn.setRequestProperty( "Content-Length", String.valueOf(data.length * 2) ); conn.setConnectTimeout(timeoutMillis); if (authKey != null && !authKey.isEmpty()) { conn.setRequestProperty ("Authorization", authKey); } - - try (OutputStream os = conn.getOutputStream()) { - os.write(data); - } + return conn; + } + + private void checkResponseFromServer(String url, String msgLabel, String msgType, String sentHexData, HttpURLConnection conn) throws Exception { + NeptusLog.pub().info("{} : {} {}", url, conn.getResponseCode(), conn.getResponseMessage()); - NeptusLog.pub().info("{} : {} {}", messagesUrl, conn.getResponseCode(), conn.getResponseMessage()); - try (InputStream is = conn.getInputStream(); ByteArrayOutputStream incoming = new ByteArrayOutputStream()) { IOUtils.copy(is, incoming); - NeptusLog.pub().info("Sent {} through HTTP: {} {}", msg.getClass().getSimpleName(), - conn.getResponseCode(), conn.getResponseMessage()); + NeptusLog.pub().info("Sent {} through HTTP: {} {}", msgLabel, + conn.getResponseCode(), conn.getResponseMessage()); - logHubInteraction(msg.getClass().getSimpleName()+" ("+msg.getMessageType()+")", messagesUrl, - conn.getRequestMethod(), ""+conn.getResponseCode(), ByteUtil.encodeToHex(msg.serialize()), - incoming.toString()); + logHubInteraction(msgLabel + " (" + msgType + ")", url, + conn.getRequestMethod(), String.valueOf(conn.getResponseCode()), + sentHexData, incoming.toString()); } catch (Exception e) { NeptusLog.pub().error(e); } - + if (conn.getResponseCode() != 200) { - throw new Exception("Server returned "+conn.getResponseCode()+": "+conn.getResponseMessage()); + throw new Exception("Server returned "+ conn.getResponseCode()+": "+ conn.getResponseMessage()); + } + } + + @Override + public void sendMessage(IridiumMessage msg) throws Exception { + byte[] data = msg.serialize(); + data = new String(Hex.encodeHex(data)).getBytes(); + + HttpURLConnection conn = getHttpURLConnection(messagesUrl); + conn.setRequestProperty( "Content-Length", String.valueOf(data.length * 2) ); + + try (OutputStream os = conn.getOutputStream()) { + os.write(data); + } + + checkResponseFromServer(messagesUrl, msg.getClass().getSimpleName(), String.valueOf(msg.getMessageType()), + ByteUtil.encodeToHex(msg.serialize()), conn); + } + + /** + * Send this raw message across + * @param destinationName The name of the destination + * (e.g. the name of the vehicle that should receive the message) + * @param imeiAddr The address of the destination, in this case the imei of the Iridium + * device that should receive the message, leave empty if not known + * @param data The raw data to be sent + */ + @Override + public void sendMessageRaw(String destinationName, String imeiAddr, byte[] data) throws Exception { + HttpURLConnection conn = getHttpURLConnection(messagesRawUrl); + + conn.setRequestProperty( "source", GeneralPreferences.imcCcuName.toLowerCase(Locale.ROOT)); + conn.setRequestProperty( "destination", destinationName); + conn.setRequestProperty( "imei", imeiAddr); + + conn.setRequestProperty( "Content-Length", String.valueOf(data.length * 2) ); + + try (OutputStream os = conn.getOutputStream()) { + os.write(new String(Hex.encodeHex(data)).getBytes()); } + + checkResponseFromServer(messagesRawUrl, "raw data", "-1", + ByteUtil.encodeToHex(data), conn); } public synchronized void logHubInteraction(String message, String url, String method, String statusCode, String requestData, String responseData) throws Exception { @@ -220,7 +261,7 @@ public Collection pollMessages(Date timeSince) throws Exception HubMessage[] msgs = gson.fromJson(baos.toString(), HubMessage[].class); - Vector ret = new Vector<>(); + List ret = new ArrayList<>(); for (HubMessage m : msgs) { try { @@ -300,10 +341,12 @@ public static String dateToString(Date d) { } public static Date stringToDate(String d) { + if (d == null || d.isEmpty()) + return null; + try { return dateFormat.parse(d); - } - catch (Exception e) { + } catch (Exception e) { e.printStackTrace(); } return null; @@ -314,15 +357,60 @@ public static class HubMessage { String msg; String updated_at; boolean plaintext; - + + // New fields for RockBlock messages + String imei; + int source = ImcId16.NULL_ID.intValue(); // imc id + int destination = ImcId16.NULL_ID.intValue();; // imc id + String created_at; + public IridiumMessage message() throws Exception { byte[] data = Hex.decodeHex(msg.toCharArray()); - return IridiumMessage.deserialize(data); + Date now = new Date(); + IridiumMessage irMsg = IridiumMessage.deserialize(data); + if (source != ImcId16.NULL_ID.intValue()) + irMsg.source = source; + if (destination != ImcId16.NULL_ID.intValue()) + irMsg.destination = destination; + if (irMsg.source == ImcId16.NULL_ID.intValue()) { + // Let us try to fill the source from imei + irMsg.source = findSystemIdByImei(imei); + } + if (!new Date(irMsg.timestampMillis).before(now) && + created_at != null && stringToDate(created_at) != null) { + irMsg.timestampMillis = stringToDate(created_at).getTime(); + } + return irMsg; } - + + public static int findSystemIdByImei(String imei) { + VehicleType vt = VehiclesHolder.getVehicleWithImei(imei); + if (vt == null) { + return ImcId16.NULL_ID.intValue(); + } + + ImcSystem imcSys = ImcSystemsHolder.getSystemWithName(vt.getId()); + if (imcSys == null) { + return vt.getImcId().intValue(); + } + return ImcId16.NULL_ID.intValue(); + } + + public byte[] messageRaw() { + try { + return Hex.decodeHex(msg.toCharArray()); + } catch (Exception e) { + return null; + } + } + public Date updatedAt() { return stringToDate(updated_at); } + + public Date createdAt() { + return stringToDate(created_at); + } } public static class HubSystemMsg { @@ -364,5 +452,9 @@ public static void main(String[] args) throws Exception { Collection msgs = messenger.pollMessages(d); System.out.println(msgs.size()); msgs.forEach(m -> System.out.println(m.asImc())); + + byte[] msg = new byte[]{0x24, 0x01, 0x00, 0x2a, (byte) 0xa5}; + IridiumManager.getManager().sendRaw("caravel", + "300125060492800", msg); } } diff --git a/src/java/pt/lsts/neptus/comm/iridium/IridiumManager.java b/src/java/pt/lsts/neptus/comm/iridium/IridiumManager.java index 6072fbc8ae..01942e9a9d 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/IridiumManager.java +++ b/src/java/pt/lsts/neptus/comm/iridium/IridiumManager.java @@ -33,11 +33,15 @@ package pt.lsts.neptus.comm.iridium; import java.awt.Component; +import java.io.ByteArrayOutputStream; import java.time.Duration; import java.util.ArrayList; import java.util.Arrays; +import java.util.Calendar; import java.util.Collection; import java.util.Date; +import java.util.GregorianCalendar; +import java.util.TimeZone; import java.util.concurrent.Executors; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; @@ -46,16 +50,23 @@ import org.apache.commons.codec.binary.Hex; +import pt.lsts.imc.AssetReport; +import pt.lsts.imc.FuelLevel; import pt.lsts.imc.IMCDefinition; import pt.lsts.imc.IMCMessage; +import pt.lsts.imc.IMCOutputStream; import pt.lsts.imc.IMCUtil; import pt.lsts.imc.IridiumMsgTx; +import pt.lsts.imc.LogBookEntry; import pt.lsts.imc.MessagePart; +import pt.lsts.imc.Voltage; import pt.lsts.imc.net.IMCFragmentHandler; import pt.lsts.neptus.NeptusLog; +import pt.lsts.neptus.comm.manager.imc.EntitiesResolver; import pt.lsts.neptus.comm.manager.imc.ImcMsgManager; import pt.lsts.neptus.util.ByteUtil; import pt.lsts.neptus.util.ImageUtils; +import pt.lsts.neptus.util.MathMiscUtils; import pt.lsts.neptus.util.conf.GeneralPreferences; /** @@ -106,8 +117,9 @@ public IridiumMessenger getCurrentMessenger() { } } - private Runnable pollMessages = new Runnable() { + private final Runnable pollMessages = new Runnable() { Date lastTime = new Date(System.currentTimeMillis() - Duration.ofHours(1).toMillis()); + //Date lastTime = new GregorianCalendar(2024, Calendar.NOVEMBER, 6).getTime(); // new Date(System.currentTimeMillis() - Duration.ofHours(1).toMillis()); @Override public void run() { @@ -127,8 +139,7 @@ public void run() { lastCall = now; NeptusLog.pub().info("Start polling messages from Iridium network."); Collection msgs = getCurrentMessenger().pollMessages(lastTime); - NeptusLog.pub().info("Polled {} messages from Iridium network.", - msgs.size()); + NeptusLog.pub().info("Polled {} messages from Iridium network.", msgs.size()); for (IridiumMessage m : msgs) { try { processMessage(m); @@ -149,7 +160,16 @@ public void run() { } } }; - + + public static Date parseTimeString(String timeOfDay) { + GregorianCalendar date = new GregorianCalendar(TimeZone.getTimeZone("UTC")); + String[] timeParts = timeOfDay.split(":"); + date.set(Calendar.HOUR_OF_DAY, Integer.parseInt(timeParts[0])); + date.set(Calendar.MINUTE, Integer.parseInt(timeParts[1])); + date.set(Calendar.SECOND, Integer.parseInt(timeParts[2])); + return date.getTime(); + } + public boolean isAvailable() { return getCurrentMessenger().isAvailable(); } @@ -161,7 +181,22 @@ public synchronized boolean isActive() { public void processMessage(IridiumMessage msg) { try { IridiumMsgTx transmission = new IridiumMsgTx(); - transmission.setData(msg.serialize()); + + if (msg.getMessageType() < 0) { + // This allows to send the original message bytes + try (ByteArrayOutputStream baos = new ByteArrayOutputStream()) { + IMCOutputStream ios = new IMCOutputStream(baos); + ios.setBigEndian(false); + int size = msg.serializeFields(ios); + transmission.setData(Arrays.copyOf(baos.toByteArray(), size)); + } catch (Exception e) { + NeptusLog.pub().warn(e.getMessage()); + transmission.setData(msg.serialize()); + } + } else { + transmission.setData(msg.serialize()); + } + transmission.setSrc(msg.getSource()); transmission.setDst(msg.getDestination()); transmission.setTimestamp(msg.timestampMillis/1000.0); @@ -175,8 +210,171 @@ public void processMessage(IridiumMessage msg) { for (IMCMessage m : msgs) { NeptusLog.pub().info("Posting resulting "+m.getAbbrev()+" message to bus."); - ImcMsgManager.getManager().postInternalMessage("iridium", m); + ImcMsgManager.getManager().postInternalMessage("iridium", m); + } + + if (msg instanceof PlainTextReportMessage) { + processAndCreateAssetReportFrom((PlainTextReportMessage) msg); + processAndCreateFuelAndBattVoltageFrom((PlainTextReportMessage) msg); + processAndCreateOpModeFrom((PlainTextReportMessage) msg); + } + } + + private static GregorianCalendar parseReportTime(PlainTextReportMessage reportMsg) { + // Get day month and year from date + Date recDate = new Date(reportMsg.timestampMillis); + Calendar calendar = Calendar.getInstance(); + calendar.setTime(recDate); + int day = calendar.get(Calendar.DAY_OF_MONTH); + int month = calendar.get(Calendar.MONTH) + 1; // Months are 0-based in Calendar + int year = calendar.get(Calendar.YEAR); + GregorianCalendar reportTime = new GregorianCalendar(TimeZone.getTimeZone("UTC")); + String[] timeParts = reportMsg.timeOfDay.split(":"); + reportTime.set(Calendar.YEAR, year); + reportTime.set(Calendar.MONTH, month); + reportTime.set(Calendar.DAY_OF_MONTH, day); + reportTime.set(Calendar.HOUR_OF_DAY, Integer.parseInt(timeParts[0])); + reportTime.set(Calendar.MINUTE, Integer.parseInt(timeParts[1])); + reportTime.set(Calendar.SECOND, Integer.parseInt(timeParts[2])); + return reportTime; + } + + private static void processAndCreateAssetReportFrom(PlainTextReportMessage reportMsg) { + NeptusLog.pub().info("Posting resulting plain text report message to bus."); + AssetReport report = new AssetReport(); + report.setSrc(reportMsg.getSource()); + report.setDst(reportMsg.getDestination()); + report.setTimestamp(reportMsg.timestampMillis / 1000.0); + report.setMedium(AssetReport.MEDIUM.SATELLITE); + + report.setReportTime(report.getTimestamp()); + + try { + GregorianCalendar reportTime = parseReportTime(reportMsg); + report.setReportTime(reportTime.getTimeInMillis() / 1000.0); + } catch (Exception e) { + NeptusLog.pub().warn(e.getMessage()); } + + report.setName(reportMsg.vehicle); + report.setLat(Math.toRadians(reportMsg.latDeg)); + report.setLon(Math.toRadians(reportMsg.lonDeg)); + report.setDepth(-1); + report.setAlt(-1); + + ImcMsgManager.getManager().postInternalMessage("iridium", report); + } + + private void processAndCreateFuelAndBattVoltageFrom(PlainTextReportMessage reportMsg) { + FuelLevel fuel = new FuelLevel(); + fuel.setSrc(reportMsg.getSource()); + fuel.setDst(reportMsg.getDestination()); + fuel.setTimestamp(reportMsg.timestampMillis / 1000.0); + + Voltage batteryVoltage = new Voltage(); + batteryVoltage.setSrc(reportMsg.getSource()); + batteryVoltage.setDst(reportMsg.getDestination()); + batteryVoltage.setTimestamp(reportMsg.timestampMillis / 1000.0); + + LogBookEntry logBookEntry = new LogBookEntry(); + logBookEntry.setSrc(reportMsg.getSource()); + logBookEntry.setDst(reportMsg.getDestination()); + logBookEntry.setTimestamp(reportMsg.timestampMillis / 1000.0); + + try { + GregorianCalendar reportTime = parseReportTime(reportMsg); + fuel.setTimestamp(reportTime.getTimeInMillis() / 1000.0); + batteryVoltage.setTimestamp(reportTime.getTimeInMillis() / 1000.0); + logBookEntry.setTimestamp(reportTime.getTimeInMillis() / 1000.0); + } catch (Exception e) { + NeptusLog.pub().warn(e.getMessage()); + } + + fuel.setValue(reportMsg.fuelPercentage); + fuel.setConfidence(reportMsg.batteryConfidencePercentage); + + int entBatt = EntitiesResolver.resolveId(reportMsg.vehicle, "Batteries"); + if (entBatt > 0) + batteryVoltage.setSrcEnt(entBatt); + batteryVoltage.setValue(reportMsg.batteryVoltage); + + logBookEntry.setHtime(logBookEntry.getTimestamp()); + logBookEntry.setContext("Text report from Iridium"); + logBookEntry.setText((reportMsg.batteryVoltage > 0 ? "Batteries voltage is " + + MathMiscUtils.round(reportMsg.batteryVoltage, 1) + " V " : "") + + (reportMsg.fuelPercentage > 0 ? "Fuel level is " + Math.round(reportMsg.fuelPercentage) + + "% (confidence " + Math.round(reportMsg.batteryConfidencePercentage) + "%)" : "")); + + boolean sendLogBookEntry = false; + if (reportMsg.fuelPercentage > 0) { + NeptusLog.pub().info("Posting resulting fuel report message to bus."); + ImcMsgManager.getManager().postInternalMessage("iridium", fuel); + sendLogBookEntry = true; + } + if (reportMsg.batteryVoltage > 0) { + NeptusLog.pub().info("Posting resulting battery voltage report message to bus."); + ImcMsgManager.getManager().postInternalMessage("iridium", batteryVoltage); + sendLogBookEntry = true; + } + if (sendLogBookEntry) { + NeptusLog.pub().info("Posting resulting log book entry message to bus."); + ImcMsgManager.getManager().postInternalMessage("iridium", logBookEntry); + } + } + + private void processAndCreateOpModeFrom(PlainTextReportMessage reportMsg) { + if (reportMsg.statusIndicator.isEmpty()) + return; + + NeptusLog.pub().info("Posting resulting op. mode report message to bus."); + LogBookEntry logBookEntry = new LogBookEntry(); + logBookEntry.setSrc(reportMsg.getSource()); + logBookEntry.setDst(reportMsg.getDestination()); + logBookEntry.setTimestamp(reportMsg.timestampMillis / 1000.0); + + try { + GregorianCalendar reportTime = parseReportTime(reportMsg); + logBookEntry.setTimestamp(reportTime.getTimeInMillis() / 1000.0); + } catch (Exception e) { + NeptusLog.pub().warn(e.getMessage()); + } + + logBookEntry.setHtime(logBookEntry.getTimestamp()); + logBookEntry.setContext("Text report from Iridium"); + logBookEntry.setType(LogBookEntry.TYPE.INFO); + + switch (reportMsg.statusIndicator) { + case "S": + logBookEntry.setType(LogBookEntry.TYPE.INFO); + logBookEntry.setText("Vehicle is in service mode"); + break; + case "B": + logBookEntry.setType(LogBookEntry.TYPE.WARNING); + logBookEntry.setText("Vehicle is in boot mode"); + break; + case "C": + logBookEntry.setType(LogBookEntry.TYPE.INFO); + logBookEntry.setText("Vehicle is in calibration mode"); + break; + case "E": + logBookEntry.setType(LogBookEntry.TYPE.ERROR); + logBookEntry.setText("Vehicle is in error mode"); + break; + case "X": + logBookEntry.setType(LogBookEntry.TYPE.WARNING); + logBookEntry.setText("Vehicle is in external mode"); + break; + case "M": + logBookEntry.setType(LogBookEntry.TYPE.INFO); + logBookEntry.setText("Vehicle is in maneuver mode"); + break; + default: + logBookEntry.setType(LogBookEntry.TYPE.INFO); + logBookEntry.setText("Vehicle is in " + reportMsg.statusIndicator + " mode"); + break; + } + + ImcMsgManager.getManager().postInternalMessage("iridium", logBookEntry); } public void selectMessenger(Component parent) { @@ -266,18 +464,32 @@ public static void testMessageSerialization() { } } - - /** * This method will send the given message using the currently selected messenger * * @param msg - * @return */ public void send(IridiumMessage msg) throws Exception { - NeptusLog.pub().info("Sending iridum message via "+getCurrentMessenger().getName()+": "+ByteUtil.encodeToHex(msg.serialize())); + NeptusLog.pub().info("Sending iridium message via "+getCurrentMessenger().getName()+": "+ByteUtil.encodeToHex(msg.serialize())); getCurrentMessenger().sendMessage(msg); } + + /** + * This method will send the given raw message using the currently selected messenger + * + * @param destinationName The name of the destination + * (e.g. the name of the vehicle that should receive the message) + * @param destinationAddr The address of the destination, this depends on the messenger + * (e.g. the IMC address of the vehicle that should receive the message, + * or the imei of the Iridium device that should receive the message) + * This can be empty or null, the messenger will try its best to find the + * missing information. + * @param data The data to be sent + */ + public void sendRaw(String destinationName, String destinationAddr, byte[] data) throws Exception { + NeptusLog.pub().info("Sending iridium raw message via "+getCurrentMessenger().getName()+": "+ByteUtil.encodeToHex(data)); + getCurrentMessenger().sendMessageRaw(destinationName, destinationAddr, data); + } public static void main(String[] args) throws Exception { String hexText = "ffff0000db07002247545653000000000000000041002147545653000000000000000042000147545653000000000000000031000047545653000000000000000032000047545653000000000000000030"; diff --git a/src/java/pt/lsts/neptus/comm/iridium/IridiumMessage.java b/src/java/pt/lsts/neptus/comm/iridium/IridiumMessage.java index ee695ae361..77f3ab80d5 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/IridiumMessage.java +++ b/src/java/pt/lsts/neptus/comm/iridium/IridiumMessage.java @@ -42,6 +42,7 @@ import pt.lsts.imc.IMCInputStream; import pt.lsts.imc.IMCMessage; import pt.lsts.imc.IMCOutputStream; +import pt.lsts.neptus.comm.manager.imc.ImcId16; /** * @author zp @@ -49,7 +50,9 @@ */ public abstract class IridiumMessage implements Comparable { - public int source, destination, message_type; + public int source = ImcId16.NULL_ID.intValue(); + public int destination = ImcId16.NULL_ID.intValue(); + public int message_type = -1; public long timestampMillis = System.currentTimeMillis(); public abstract int serializeFields(IMCOutputStream out) throws Exception; public abstract int deserializeFields(IMCInputStream in) throws Exception; @@ -87,9 +90,10 @@ public static IridiumMessage deserialize(byte[] data) throws Exception { IMCInputStream iis = new IMCInputStream(new ByteArrayInputStream(data), IMCDefinition.getInstance()); iis.setBigEndian(false); iis.mark(10); - int source = iis.readUnsignedShort(); - int dest = iis.readUnsignedShort(); - int mgid = iis.readUnsignedShort(); + int avlBytes = iis.available(); + int source = avlBytes >= 2 ? iis.readUnsignedShort() : ImcId16.NULL_ID.intValue(); + int dest = avlBytes >= 4 ? iis.readUnsignedShort() : ImcId16.NULL_ID.intValue(); + int mgid = avlBytes >= 6 ? iis.readUnsignedShort() : -1; IridiumMessage m = null; if (iridiumTypes.containsKey(mgid)) { m = iridiumTypes.get(mgid).getDeclaredConstructor().newInstance(); diff --git a/src/java/pt/lsts/neptus/comm/iridium/IridiumMessenger.java b/src/java/pt/lsts/neptus/comm/iridium/IridiumMessenger.java index 31f993abef..239aac8618 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/IridiumMessenger.java +++ b/src/java/pt/lsts/neptus/comm/iridium/IridiumMessenger.java @@ -45,9 +45,25 @@ public interface IridiumMessenger { * Send this message across */ public void sendMessage(IridiumMessage msg) throws Exception; - + + /** + * Send this message across + * + * @param destinationName The name of the destination + * (e.g. the name of the vehicle that should receive the message) + * @param destinationAddr The address of the destination, this depends on the messenger + * (e.g. the IMC address of the vehicle that should receive the message, + * or the imei of the Iridium device that should receive the message) + * This can be empty or null, the messenger will try its best to find the + * missing information. + * @param data The data to be sent + * @return + */ + public void sendMessageRaw(String destinationName, String destinationAddr, byte[] data) throws Exception; + /** * Retrieve any messages that were received since given time + * It is expected that the messenger sends a IridiumManager to send an IridiumMsgTx message to the internal bus */ public Collection pollMessages(Date timeSince) throws Exception; @@ -70,7 +86,7 @@ public interface IridiumMessenger { * Remove existing message listener */ public void removeListener(IridiumMessageListener listener); - + /** * Method that is called when messengers are removed */ diff --git a/src/java/pt/lsts/neptus/comm/iridium/PlainTextMessage.java b/src/java/pt/lsts/neptus/comm/iridium/PlainTextMessage.java index f58edbce2d..728f945c60 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/PlainTextMessage.java +++ b/src/java/pt/lsts/neptus/comm/iridium/PlainTextMessage.java @@ -38,8 +38,11 @@ import pt.lsts.imc.TextMessage; import pt.lsts.neptus.NeptusLog; +import java.io.UnsupportedEncodingException; +import java.nio.charset.StandardCharsets; +import java.util.ArrayList; import java.util.Collection; -import java.util.Vector; +import java.util.List; /** * @author pdias @@ -48,6 +51,7 @@ public class PlainTextMessage extends IridiumMessage { String text; + byte[] rawData; public PlainTextMessage() { super(-1); @@ -55,20 +59,21 @@ public PlainTextMessage() { @Override public int serializeFields(IMCOutputStream out) throws Exception { - out.write(text.getBytes("UTF-8")); + out.write(rawData); out.close(); - return text.getBytes("UTF-8").length; + return rawData.length; } @Override public int deserializeFields(IMCInputStream in) throws Exception { int bav = in.available(); - bav = bav < 0 ? 0 : bav; + bav = Math.max(bav, 0); byte[] data = new byte[bav]; in.readFully(data); - text = new String(data, "UTF-8"); + rawData = data; + text = new String(data, StandardCharsets.UTF_8); text = text.trim(); - return text.getBytes("UTF-8").length; + return text.getBytes(StandardCharsets.UTF_8).length; } public final String getText() { @@ -77,12 +82,23 @@ public final String getText() { public final void setText(String text) { this.text = text; + this.rawData = text.getBytes(StandardCharsets.UTF_8); + } + + public final byte[] getRawData() { + return rawData; + } + + public final void setRawData(byte[] rawData) throws UnsupportedEncodingException { + this.rawData = rawData; + text = new String(rawData, StandardCharsets.UTF_8); } @Override public Collection asImc() { - Vector msgs = new Vector<>(); - msgs.add(new TextMessage("iridium", text)); + List msgs = new ArrayList<>(); + TextMessage imcTxtMsg = new TextMessage("iridium", text); + msgs.add(imcTxtMsg); return msgs; } @@ -93,6 +109,9 @@ public String toString() { } static IridiumMessage createTextMessageFrom(IMCInputStream in) throws Exception { + if (in.markSupported()) { + in.mark(Integer.MAX_VALUE); + } try { PlainTextReportMessage plainTextReport = new PlainTextReportMessage(); plainTextReport.deserializeFields(in); @@ -101,6 +120,7 @@ static IridiumMessage createTextMessageFrom(IMCInputStream in) throws Exception NeptusLog.pub().warn("Not able to parse iridium msg as PlainTextReportMessage, trying another or simple text"); } + in.reset(); PlainTextMessage plainTextMessage = new PlainTextMessage(); plainTextMessage.deserializeFields(in); return plainTextMessage; diff --git a/src/java/pt/lsts/neptus/comm/iridium/PlainTextReportMessage.java b/src/java/pt/lsts/neptus/comm/iridium/PlainTextReportMessage.java index 45af5b9a96..1c04449f89 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/PlainTextReportMessage.java +++ b/src/java/pt/lsts/neptus/comm/iridium/PlainTextReportMessage.java @@ -41,8 +41,10 @@ import javax.xml.bind.annotation.adapters.HexBinaryAdapter; import java.io.ByteArrayInputStream; +import java.nio.charset.StandardCharsets; +import java.util.ArrayList; import java.util.Collection; -import java.util.Vector; +import java.util.List; import java.util.regex.Matcher; import java.util.regex.Pattern; @@ -52,7 +54,8 @@ */ public class PlainTextReportMessage extends IridiumMessage { - private static Pattern p = Pattern.compile("\\((.)\\) \\((.*)\\) (.*) / (.*), (.*) / .*"); + private static final Pattern p0 = Pattern.compile("\\((.)\\) \\((.*)\\) (.*) / (.*), (.*) / f:(\\d*) v:(\\d*) c:(\\d*) / s: ?(.)(.*?)"); + private static final Pattern p = Pattern.compile("\\((.)\\) \\((.*)\\) (.*) / (.*), (.*) / .*"); String report; @@ -62,45 +65,64 @@ public class PlainTextReportMessage extends IridiumMessage { double latDeg; double lonDeg; + double fuelPercentage = -1; + double batteryVoltage = -1; + double batteryConfidencePercentage = -1; + + String statusIndicator = ""; + public PlainTextReportMessage() { super(-1); } @Override public int serializeFields(IMCOutputStream out) throws Exception { - out.write(report.getBytes("ISO-8859-1")); + out.write(report.getBytes(StandardCharsets.ISO_8859_1)); out.close(); - return report.getBytes("ISO-8859-1").length; + return report.getBytes(StandardCharsets.ISO_8859_1).length; } @Override public int deserializeFields(IMCInputStream in) throws Exception { int bav = in.available(); - bav = bav < 0 ? 0 : bav; + bav = Math.max(bav, 0); byte[] data = new byte[bav]; int len = in.read(data); - report = new String(data, "ISO-8859-1"); + report = new String(data, StandardCharsets.ISO_8859_1); parse(); return len; } @Override public Collection asImc() { - Vector msgs = new Vector<>(); + List msgs = new ArrayList<>(); msgs.add(new TextMessage("iridium", report)); return msgs; } @Override public String toString() { - return "Report: " + report + "\n"; + return "Report: " + report + "\n" + + "Vehicle: " + vehicle + "\n" + + "Time of day: " + timeOfDay + "\n" + + "Lat: " + latDeg + "\n" + + "Lon: " + lonDeg + "\n" + + "Fuel: " + fuelPercentage + "\n" + + "Battery: " + batteryVoltage + "\n" + + "Battery confidence: " + batteryConfidencePercentage + "\n" + + "Status: " + statusIndicator; + } - private void parse() { - Matcher matcher = p.matcher(report); + private void parse() throws Exception { + Matcher matcher = p0.matcher(report); if (!matcher.matches()) { - return; + matcher = p.matcher(report); + if (!matcher.matches()) { + throw new Exception("Invalid report format: " + report); + } } + vehicle = matcher.group(2); timeOfDay = matcher.group(3); String latMins = matcher.group(4); @@ -109,10 +131,18 @@ private void parse() { if (source == -1) { return; } - String latParts[] = latMins.split(" "); - String lonParts[] = lonMins.split(" "); + String[] latParts = latMins.split(" "); + String[] lonParts = lonMins.split(" "); latDeg = getCoords(latParts); lonDeg = getCoords(lonParts); + + if (matcher.groupCount() <= 6) + return; + + fuelPercentage = Double.parseDouble(matcher.group(6)); + batteryVoltage = Double.parseDouble(matcher.group(7)) / 10.0; + batteryConfidencePercentage = Double.parseDouble(matcher.group(8)); + statusIndicator = matcher.group(9); } private double getCoords(String[] coordParts) { @@ -134,6 +164,7 @@ public static void main(String[] args) { try { txtIridium.deserializeFields(iis); NeptusLog.pub().info("Received a plain text from " + txtIridium.report); + System.out.println("Received a plain text from " + txtIridium); } catch (Exception e) { NeptusLog.pub().error(e); diff --git a/src/java/pt/lsts/neptus/comm/iridium/RockBlockIridiumMessenger.java b/src/java/pt/lsts/neptus/comm/iridium/RockBlockIridiumMessenger.java index 20e2c7be8c..eb80d24308 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/RockBlockIridiumMessenger.java +++ b/src/java/pt/lsts/neptus/comm/iridium/RockBlockIridiumMessenger.java @@ -37,7 +37,6 @@ import java.io.InputStream; import java.io.InputStreamReader; import java.net.URL; -import java.nio.charset.Charset; import java.nio.charset.StandardCharsets; import java.util.ArrayList; import java.util.Collection; @@ -45,7 +44,7 @@ import java.util.HashSet; import java.util.List; import java.util.Properties; -import java.util.Vector; +import java.util.Set; import java.util.concurrent.ExecutionException; import java.util.concurrent.Future; import java.util.concurrent.TimeUnit; @@ -90,7 +89,7 @@ * This class uses the RockBlock HTTP API (directly) to send messages to Iridium destinations and a gmail inbox to poll * for incoming messages * - * @see http://rockblock.rock7mobile.com/downloads/RockBLOCK-Web-Services-User-Guide.pdf + * @see https://www.groundcontrol.com/wp-content/uploads/2022/02/RockBLOCK-Web-Services-User-Guide.pdf * @author zp */ @IridiumProvider(id="rock7", name="RockBlock Messenger", description="Sends Iridium messages directly via RockBlock web service and receives new messages by polling a gmail address") @@ -100,7 +99,7 @@ public class RockBlockIridiumMessenger implements IridiumMessenger { protected boolean available = true; protected static String serverUrl = "https://secure.rock7mobile.com/rockblock/MT"; - protected HashSet listeners = new HashSet<>(); + protected Set listeners = new HashSet<>(); private static long lastSuccess = -1; @NeptusProperty @@ -190,27 +189,21 @@ private void setGmailUsername(String username) { this.gmailUsername = username; } - @Override - public void sendMessage(IridiumMessage msg) throws Exception { - VehicleType vt = VehiclesHolder.getVehicleWithImc(new ImcId16(msg.getDestination())); - if (vt == null) { - throw new Exception("Cannot send message to an unknown destination"); - } - IridiumArgs args = (IridiumArgs) vt.getProtocolsArgs().get("iridium"); + private boolean askCredentials() throws IOException { if (askRockBlockPassword || rockBlockPassword == null || rockBlockUsername == null) { Pair credentials = GuiUtils.askCredentials(ConfigFetch.getSuperParentFrame(), "Enter RockBlock Credentials", getRockBlockUsername(), getRockBlockPassword()); if (credentials == null) - return; + return true; setRockBlockUsername(credentials.first()); setRockBlockPassword(credentials.second()); PluginUtils.saveProperties(CONF_ROCKBLOCK_PROPS, this); askRockBlockPassword = false; } - - String result = sendToRockBlockHttp(args.getImei(), getRockBlockUsername(), getRockBlockPassword(), - msg.serialize()); + return false; + } + private void checkResponseFromServer(String result) throws Exception { if (!result.split(",")[0].equals("OK")) { String[] errorCode = result.split(","); if (errorCode[0].equalsIgnoreCase("FAILED") && errorCode[1].equalsIgnoreCase("10")) { @@ -221,6 +214,40 @@ public void sendMessage(IridiumMessage msg) throws Exception { } } + @Override + public void sendMessage(IridiumMessage msg) throws Exception { + VehicleType vt = VehiclesHolder.getVehicleWithImc(new ImcId16(msg.getDestination())); + if (vt == null) { + throw new Exception("Cannot send message to an unknown destination"); + } + IridiumArgs args = (IridiumArgs) vt.getProtocolsArgs().get("iridium"); + + if (askCredentials()) + return; + + String result = sendToRockBlockHttp(args.getImei(), getRockBlockUsername(), getRockBlockPassword(), + msg.serialize()); + checkResponseFromServer(result); + } + + @Override + public void sendMessageRaw(String destinationName, String imeiAddr, byte[] data) throws Exception { + if (imeiAddr == null || imeiAddr.isEmpty()) { + VehicleType vt = VehiclesHolder.getVehicleById(destinationName); + if (vt == null) { + throw new Exception("Cannot send message to an unknown destination"); + } + IridiumArgs args = (IridiumArgs) vt.getProtocolsArgs().get("iridium"); + imeiAddr = args.getImei(); + } + + if (askCredentials()) + return; + + String result = sendToRockBlockHttp(imeiAddr, getRockBlockUsername(), getRockBlockPassword(), data); + checkResponseFromServer(result); + } + public String sendToRockBlockHttp(String destImei, String username, String password, byte[] data) throws IOException { @@ -258,7 +285,7 @@ public String sendToRockBlockHttp(String destImei, String username, String passw } } - private Pattern pattern = Pattern.compile("APPLICATION/OCTET-STREAM; name=(\\d+)-(\\d+)\\.bin"); + private final Pattern pattern = Pattern.compile("APPLICATION/OCTET-STREAM; name=(\\d+)-(\\d+)\\.bin"); @Override public Collection pollMessages(Date timeSince) throws Exception { @@ -299,7 +326,7 @@ else if (m.getContent() instanceof MimeMultipart) { if (matcher.matches()) { InputStream stream = (InputStream) p.getContent(); byte[] data = IOUtils.toByteArray(stream); - IridiumMessage msg = process(data, matcher.group(1)); + IridiumMessage msg = process(data, matcher.group(1), matcher.group(2), m.getSentDate()); if (msg != null) messages.add(msg); } @@ -310,28 +337,40 @@ else if (m.getContent() instanceof MimeMultipart) { catch (AuthenticationFailedException ex) { askGmailPassword = true; ex.printStackTrace(); - return new Vector<>(); + return new ArrayList<>(); } catch (NoSuchProviderException ex) { ex.printStackTrace(); - return new Vector<>(); + return new ArrayList<>(); } catch (MessagingException ex) { ex.printStackTrace(); - return new Vector<>(); + return new ArrayList<>(); } catch (Exception e) { e.printStackTrace(); - return new Vector<>(); + return new ArrayList<>(); } messages.sort((m1, m2) -> Long.compare(m1.timestampMillis, m2.timestampMillis)); return messages; } - private IridiumMessage process(byte[] data, String from) { + private IridiumMessage process(byte[] data, String fromImei, String seqNumber, Date sentDate) { try { - return IridiumMessage.deserialize(data); + Date now = new Date(); + IridiumMessage irMsg = IridiumMessage.deserialize(data); + if (irMsg.source == ImcId16.NULL_ID.intValue()) { + // Let us try to fill the source from imei + irMsg.source = HubIridiumMessenger.HubMessage.findSystemIdByImei(fromImei); + } + + // If not set, set the timestamp + if (!new Date(irMsg.timestampMillis).before(now) && sentDate != null) { + irMsg.timestampMillis = sentDate.getTime(); + } + + return irMsg; } catch (Exception e){ e.printStackTrace(); diff --git a/src/java/pt/lsts/neptus/comm/iridium/SimulatedMessenger.java b/src/java/pt/lsts/neptus/comm/iridium/SimulatedMessenger.java index 9d9abc7b46..70f0addb94 100644 --- a/src/java/pt/lsts/neptus/comm/iridium/SimulatedMessenger.java +++ b/src/java/pt/lsts/neptus/comm/iridium/SimulatedMessenger.java @@ -38,9 +38,10 @@ import java.net.HttpURLConnection; import java.net.URL; import java.util.Collection; +import java.util.Collections; import java.util.Date; import java.util.HashSet; -import java.util.Vector; +import java.util.Set; import com.google.common.eventbus.Subscribe; @@ -50,7 +51,12 @@ import pt.lsts.imc.IridiumMsgRx; import pt.lsts.imc.IridiumMsgTx; import pt.lsts.neptus.NeptusLog; +import pt.lsts.neptus.comm.manager.imc.ImcId16; import pt.lsts.neptus.comm.manager.imc.ImcMsgManager; +import pt.lsts.neptus.comm.manager.imc.ImcSystem; +import pt.lsts.neptus.comm.manager.imc.ImcSystemsHolder; +import pt.lsts.neptus.types.vehicle.VehicleType; +import pt.lsts.neptus.types.vehicle.VehiclesHolder; import pt.lsts.neptus.util.conf.GeneralPreferences; /** @@ -61,9 +67,7 @@ + "directly in the bus of the destination via IMC. Used only for debug / simulation purposes") public class SimulatedMessenger implements IridiumMessenger { - protected Vector messagesReceived = new Vector<>(); - - protected HashSet listeners = new HashSet<>(); + protected Set listeners = new HashSet<>(); protected String serverUrl = GeneralPreferences.ripplesUrl + "/api/v1/"; private final String authKey = GeneralPreferences.ripplesApiKey; @@ -135,9 +139,37 @@ public void sendMessage(IridiumMessage msg) throws Exception { ImcMsgManager.getManager().sendMessage(rx); } + /** + * @param destinationName The name of the destination + * (e.g. the name of the vehicle that should receive the message) + * @param destinationAddr It is ignored + * @param data The data to be sent + * @return + * @throws Exception + */ + @Override + public void sendMessageRaw(String destinationName, String destinationAddr, byte[] data) throws Exception { + int imcId = ImcId16.NULL_ID.intValue(); + VehicleType veh = VehiclesHolder.getVehicleById(destinationName); + if (veh != null) { + imcId = veh.getImcId().intValue(); + } else { + ImcSystem imcSys = ImcSystemsHolder.getSystemWithName(destinationName); + if (imcSys != null) + imcId = imcSys.getId().intValue(); + } + + IridiumMsgRx rx = new IridiumMsgRx(); + rx.setOrigin("Iridium simulated messenger"); + rx.setDst(imcId); + rx.setSrc(ImcMsgManager.getManager().getLocalId().intValue()); + rx.setData(data); + ImcMsgManager.getManager().sendMessage(rx); + } + @Override public Collection pollMessages(Date timeSince) throws Exception { - return new Vector<>(); + return Collections.emptyList(); } @Override @@ -158,6 +190,5 @@ public String toString() { @Override public void cleanup() { listeners.clear(); - messagesReceived.clear(); } } diff --git a/src/java/pt/lsts/neptus/types/vehicle/VehiclesHolder.java b/src/java/pt/lsts/neptus/types/vehicle/VehiclesHolder.java index 0d82b9b141..c85aec98a2 100644 --- a/src/java/pt/lsts/neptus/types/vehicle/VehiclesHolder.java +++ b/src/java/pt/lsts/neptus/types/vehicle/VehiclesHolder.java @@ -38,6 +38,7 @@ import java.awt.event.WindowEvent; import java.io.File; import java.lang.reflect.Constructor; +import java.util.Collection; import java.util.Hashtable; import java.util.LinkedHashMap; import java.util.LinkedList; @@ -54,6 +55,7 @@ import pt.lsts.neptus.console.ConsoleLayout; import pt.lsts.neptus.i18n.I18n; import pt.lsts.neptus.types.comm.CommMean; +import pt.lsts.neptus.types.comm.protocol.IridiumArgs; import pt.lsts.neptus.types.coord.CoordinateSystem; import pt.lsts.neptus.types.coord.CoordinateSystemsHolder; import pt.lsts.neptus.types.mission.MissionType; @@ -175,6 +177,18 @@ public static VehicleType getVehicleWithImc(ImcId16 imcId) { return null; } + public static VehicleType getVehicleWithImei(String imei) { + Collection vtList = VehiclesHolder.getVehiclesList().values(); + if (vtList.isEmpty()) { + return null; + } + + return vtList.stream().filter(vt -> vt.getProtocolsArgs().containsKey(CommMean.IRIDIUM)).filter(vt -> { + IridiumArgs args = (IridiumArgs) vt.getProtocolsArgs().get(CommMean.IRIDIUM); + return args.getImei().equals(imei); + }).findFirst().orElse(null); + } + /** * */