diff --git a/src/test/java/ru/r2cloud/MeasureRotatorPrecision.java b/src/test/java/ru/r2cloud/MeasureRotatorPrecision.java new file mode 100644 index 00000000..4c2c9e14 --- /dev/null +++ b/src/test/java/ru/r2cloud/MeasureRotatorPrecision.java @@ -0,0 +1,208 @@ +package ru.r2cloud; + +import java.io.BufferedReader; +import java.io.BufferedWriter; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.FileWriter; +import java.io.IOException; +import java.nio.file.FileSystems; +import java.text.ParseException; +import java.text.SimpleDateFormat; +import java.util.Locale; +import java.util.TimeZone; +import java.util.UUID; +import java.util.concurrent.CountDownLatch; +import java.util.concurrent.Future; +import java.util.regex.Pattern; + +import org.hipparchus.util.FastMath; +import org.orekit.bodies.GeodeticPoint; +import org.orekit.frames.TopocentricFrame; +import org.orekit.propagation.analytical.tle.TLEPropagator; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import ru.r2cloud.lora.loraat.JSerial; +import ru.r2cloud.model.ObservationRequest; +import ru.r2cloud.model.RotatorConfiguration; +import ru.r2cloud.model.Tle; +import ru.r2cloud.predict.PredictOreKit; +import ru.r2cloud.rotator.allview.AllviewClient; +import ru.r2cloud.rotator.allview.AllviewRotatorService; +import ru.r2cloud.rotctrld.Position; +import ru.r2cloud.rotctrld.RotCtrlException; +import ru.r2cloud.rotctrld.RotctrldClient; +import ru.r2cloud.satellite.RotatorService; +import ru.r2cloud.util.Configuration; +import ru.r2cloud.util.ThreadPoolFactoryImpl; + +public class MeasureRotatorPrecision { + + private static final Logger LOG = LoggerFactory.getLogger(MeasureRotatorPrecision.class); + + private static final int ROTCTRLD_PORT = 8000; + private static final String ALLVIEW_DEVICE_FILE = "/dev/cu.usbserial-A10M67FQ"; + + public static void main(String[] args) throws Exception { + Configuration config = new Configuration(MeasureRotatorPrecision.class.getClassLoader().getResourceAsStream("config-dev.properties"), System.getProperty("user.home") + File.separator + ".r2cloud", "config-common-test.properties", FileSystems.getDefault()); + config.setProperty("locaiton.lat", "51.721"); + config.setProperty("locaiton.lon", "5.030"); + config.setProperty("scheduler.orekit.path", "./src/test/resources/data/orekit-data"); + PredictOreKit predict = new PredictOreKit(config); + + Tle tle = new Tle(new String[] { "funcube-1", "1 39444U 13066AE 20157.75071106 .00000221 00000-0 33451-4 0 9997", "2 39444 97.5589 158.6491 0056696 309.6463 49.9756 14.82127945351637" }); + ObservationRequest req = new ObservationRequest(); + req.setTle(tle); + req.setStartTimeMillis(getTime("2020-06-06 05:50:29")); + req.setEndTimeMillis(getTime("2020-06-06 06:01:42")); + Double lat = config.getDouble("locaiton.lat"); + Double lon = config.getDouble("locaiton.lon"); + req.setGroundStation(new GeodeticPoint(FastMath.toRadians(lat), FastMath.toRadians(lon), 0.0)); + + OffsetClock clock = new OffsetClock(req.getStartTimeMillis()); + RotatorConfiguration rotatorConfig = createValidConfig(); + + // choose which measure to take by uncommeting to the function +// measureRotatorService(predict, req, clock, rotatorConfig); +// measureAllviewService(predict, req, clock, rotatorConfig); + } + + private static void measureAllviewService(PredictOreKit predict, ObservationRequest req, OffsetClock clock, RotatorConfiguration rotatorConfig) throws InterruptedException, IOException, FileNotFoundException { + CountDownLatch latch = new CountDownLatch(1); + final File result = new File("allview.csv"); + AllviewClient client = new AllviewClient(ALLVIEW_DEVICE_FILE, 5000, new JSerial()); + try { + client.start(); + } catch (Exception e1) { + LOG.error("unable to start allview client", e1); + return; + } + Thread monitorThread = new Thread(new Runnable() { + @Override + public void run() { + try (BufferedWriter w = new BufferedWriter(new FileWriter(result))) { + while (!Thread.currentThread().isInterrupted()) { + Position current = client.getPosition(); + long currentMillis = clock.millis(); + w.append(String.valueOf(current.getAzimuth())).append(",").append(String.valueOf(current.getElevation())).append(",").append(String.valueOf(currentMillis)).append("\n"); + if (currentMillis > req.getEndTimeMillis()) { + break; + } + try { + Thread.sleep(100); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + } + LOG.info("measurement complete. results at: {}", result.getAbsolutePath()); + } catch (IOException e) { + LOG.error("cannot create file for metrics", e); + } finally { + latch.countDown(); + } + } + }, "monitor-thread"); + monitorThread.start(); + + AllviewRotatorService service = new AllviewRotatorService(client, rotatorConfig, predict, new ThreadPoolFactoryImpl(10000), clock); + service.start(); + Future observationFuture = service.schedule(req, req.getStartTimeMillis(), null); + latch.await(); + observationFuture.cancel(true); + service.stop(); + + enrichWithExpectedPosition(predict, req, result); + } + + private static void measureRotatorService(PredictOreKit predict, ObservationRequest req, OffsetClock clock, RotatorConfiguration rotatorConfig) throws InterruptedException, IOException, FileNotFoundException { + CountDownLatch latch = new CountDownLatch(1); + final File result = new File("coarse.csv"); + Thread monitorThread = new Thread(new Runnable() { + @Override + public void run() { + RotctrldClient client = new RotctrldClient(rotatorConfig.getHostname(), rotatorConfig.getPort(), rotatorConfig.getTimeout()); + try (BufferedWriter w = new BufferedWriter(new FileWriter(result))) { + client.start(); + while (!Thread.currentThread().isInterrupted()) { + Position current; + try { + current = client.getPosition(); + } catch (RotCtrlException e1) { + LOG.error("can't get position", e1); + break; + } + long currentMillis = clock.millis(); + w.append(String.valueOf(current.getAzimuth())).append(",").append(String.valueOf(current.getElevation())).append(",").append(String.valueOf(currentMillis)).append("\n"); + if (currentMillis > req.getEndTimeMillis()) { + break; + } + try { + Thread.sleep(100); + } catch (InterruptedException e) { + Thread.currentThread().interrupt(); + break; + } + } + LOG.info("measurement complete. results at: {}", result.getAbsolutePath()); + } catch (IOException e) { + LOG.error("cannot create file for metrics", e); + } finally { + latch.countDown(); + } + } + }, "monitor-thread"); + monitorThread.start(); + + RotatorService service = new RotatorService(rotatorConfig, predict, new ThreadPoolFactoryImpl(10000), clock); + service.start(); + Future observationFuture = service.schedule(req, req.getStartTimeMillis(), null); + latch.await(); + observationFuture.cancel(true); + service.stop(); + + enrichWithExpectedPosition(predict, req, result); + } + + private static void enrichWithExpectedPosition(PredictOreKit predict, ObservationRequest req, final File result) throws IOException, FileNotFoundException { + if (!result.exists()) { + return; + } + try (BufferedReader r = new BufferedReader(new FileReader(result)); BufferedWriter w = new BufferedWriter(new FileWriter(result.getName() + "_enriched.csv"))) { + String curLine = null; + Pattern p = Pattern.compile(","); + TopocentricFrame groundStation = predict.getPosition(req.getGroundStation()); + TLEPropagator tlePropagator = TLEPropagator.selectExtrapolator(new org.orekit.propagation.analytical.tle.TLE(req.getTle().getRaw()[1], req.getTle().getRaw()[2])); + while ((curLine = r.readLine()) != null) { + String[] parts = p.split(curLine); + long time = Long.valueOf(parts[2]); + Position satPosition = predict.getSatellitePosition(time, groundStation, tlePropagator); + w.append(parts[0]).append(',').append(parts[1]).append(',').append(parts[2]).append(',').append(String.valueOf(satPosition.getAzimuth())).append(',').append(String.valueOf(satPosition.getElevation())).append('\n'); + } + } + } + + private static RotatorConfiguration createValidConfig() { + RotatorConfiguration config = new RotatorConfiguration(); + config.setId(UUID.randomUUID().toString()); + config.setHostname("127.0.0.1"); + config.setPort(ROTCTRLD_PORT); + config.setCycleMillis(1000); + config.setTimeout(10000); + config.setTolerance(5); + return config; + } + + private static long getTime(String str) { + SimpleDateFormat sdf = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss", Locale.UK); + sdf.setTimeZone(TimeZone.getTimeZone("GMT")); + try { + return sdf.parse(str).getTime(); + } catch (ParseException e) { + throw new RuntimeException(e); + } + } + +} diff --git a/src/test/java/ru/r2cloud/OffsetClock.java b/src/test/java/ru/r2cloud/OffsetClock.java new file mode 100644 index 00000000..d437c972 --- /dev/null +++ b/src/test/java/ru/r2cloud/OffsetClock.java @@ -0,0 +1,17 @@ +package ru.r2cloud; + +import ru.r2cloud.util.Clock; + +public class OffsetClock implements Clock { + + private final long offset; + + public OffsetClock(long initialTime) { + this.offset = System.currentTimeMillis() - initialTime; + } + + @Override + public long millis() { + return System.currentTimeMillis() - offset; + } +} diff --git a/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java b/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java index 3ed0aa0b..f01cfd04 100644 --- a/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java +++ b/src/test/java/ru/r2cloud/rotator/allview/AllviewClient.java @@ -48,6 +48,13 @@ public void slew(Position nextPosition, Position previousPosition) throws IOExce } catch (IOException e) { currentAzimuth = previousPosition.getAzimuth(); } + // slightly overrun. do not go back. just skip the cycle + if (nextPosition.getAzimuth() > previousPosition.getAzimuth() && nextPosition.getAzimuth() < currentAzimuth) { + currentAzimuth = nextPosition.getAzimuth(); + } + if (nextPosition.getAzimuth() < previousPosition.getAzimuth() && nextPosition.getAzimuth() > currentAzimuth) { + currentAzimuth = nextPosition.getAzimuth(); + } } else { currentAzimuth = previousPosition.getAzimuth(); } diff --git a/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java b/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java index 1243c769..7b5cf22e 100644 --- a/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java +++ b/src/test/java/ru/r2cloud/rotator/allview/AllviewMotor.java @@ -41,7 +41,7 @@ private static int convertInteger(String inputStr) { return ((input & 0x0000FF) << 16) | (input & 0x00FF00) | ((input & 0xFF0000) >> 16); } - public static double convertAngle(String inputStr) { + private double convertAngle(String inputStr) { int converted = convertInteger(inputStr); boolean positive = true; if (converted > 0x800000) { @@ -50,7 +50,7 @@ public static double convertAngle(String inputStr) { converted = 0x800000 - converted; positive = false; } - double ticks_per_angle = (double) 865050 / 360.0; + double ticks_per_angle = (double) cpr / 360.0; double result = converted / ticks_per_angle; if (!positive) { return 360.0 - result; @@ -91,7 +91,7 @@ public void slew(double degreePerSec) throws IOException { boolean changeDirectionToCcw = (degreePerSec < 0 && currentDegree > 0); boolean changeDirectionNeeded = (changeDirectionToCw || changeDirectionToCcw); double degreeAbs = Math.abs(degreePerSec); - int number = (int) ((getTimerInterruptFreq() * 360.0f) / degreeAbs / 865050.0f); + int number = (int) ((getTimerInterruptFreq() * 360.0f) / degreeAbs / cpr); if (currentSpeed != 0 && currentSpeed == number && !changeDirectionNeeded) { return; } @@ -138,7 +138,7 @@ public boolean isStopRequired(double degree) throws IOException { return true; } // only when required speed is actually changed - int number = (int) ((getTimerInterruptFreq() * 360.0f) / Math.abs(degree) / 865050.0f); + int number = (int) ((getTimerInterruptFreq() * 360.0f) / Math.abs(degree) / cpr); if (currentSpeed != number) { return true; } diff --git a/src/test/java/ru/r2cloud/rotator/allview/AllviewRotatorService.java b/src/test/java/ru/r2cloud/rotator/allview/AllviewRotatorService.java new file mode 100644 index 00000000..dd4ef8fa --- /dev/null +++ b/src/test/java/ru/r2cloud/rotator/allview/AllviewRotatorService.java @@ -0,0 +1,174 @@ +package ru.r2cloud.rotator.allview; + +import java.io.IOException; +import java.util.concurrent.Future; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; + +import org.orekit.frames.TopocentricFrame; +import org.orekit.propagation.analytical.tle.TLEPropagator; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import ru.r2cloud.Lifecycle; +import ru.r2cloud.model.DeviceConnectionStatus; +import ru.r2cloud.model.ObservationRequest; +import ru.r2cloud.model.RotatorConfiguration; +import ru.r2cloud.model.RotatorStatus; +import ru.r2cloud.predict.PredictOreKit; +import ru.r2cloud.rotctrld.Position; +import ru.r2cloud.util.Clock; +import ru.r2cloud.util.NamingThreadFactory; +import ru.r2cloud.util.ThreadPoolFactory; +import ru.r2cloud.util.Util; + +public class AllviewRotatorService implements Lifecycle { + + private static final Logger LOG = LoggerFactory.getLogger(AllviewRotatorService.class); + + private ScheduledExecutorService executor = null; + + private final RotatorStatus status = new RotatorStatus(); + private final RotatorConfiguration config; + private final PredictOreKit predict; + private final ThreadPoolFactory threadpoolFactory; + private final Clock clock; + private final AllviewClient client; + + public AllviewRotatorService(AllviewClient client, RotatorConfiguration config, PredictOreKit predict, ThreadPoolFactory threadpoolFactory, Clock clock) { + this.predict = predict; + this.threadpoolFactory = threadpoolFactory; + this.clock = clock; + this.config = config; + this.client = client; + } + + @Override + public synchronized void start() { + LOG.info("[{}] starting rotator on: {}:{}", config.getId(), config.getHostname(), config.getPort()); + status.setHostport(config.getHostname() + ":" + config.getPort()); + executor = threadpoolFactory.newScheduledThreadPool(1, new NamingThreadFactory("rotator")); + } + + @Override + public synchronized void stop() { + if (executor != null) { + Util.shutdown(executor, threadpoolFactory.getThreadPoolShutdownMillis()); + } + client.stop(); + LOG.info("[{}] stopped", config.getId()); + } + + public Future schedule(ObservationRequest req, long current, Future startFuture) { + if (executor == null) { + return null; + } + TLEPropagator tlePropagator = TLEPropagator.selectExtrapolator(new org.orekit.propagation.analytical.tle.TLE(req.getTle().getRaw()[1], req.getTle().getRaw()[2])); + TopocentricFrame groundStation = predict.getPosition(req.getGroundStation()); + Future result = executor.scheduleAtFixedRate(new Runnable() { + + private Position previousPosition; + private boolean log = true; + + @Override + public void run() { + long current = clock.millis(); + if (current > req.getEndTimeMillis()) { + LOG.info("[{}] observation time passed. cancelling rotation", req.getId()); + client.stop(); + throw new RuntimeException("observation time passed"); + } + if (startFuture != null && startFuture.isDone()) { + LOG.info("[{}] observation stopped. cancelling rotation", req.getId()); + client.stop(); + throw new RuntimeException("observation stopped"); + } + + if (!ensureClientConnected(req.getId(), log)) { + log = false; + // try reconnect on the next iteration + return; + } + + Position nextPosition; + if (previousPosition == null) { + + try { + client.stopMotors(); + + previousPosition = predict.getSatellitePosition(current, groundStation, tlePropagator); + + client.waitMotorStop(); + + client.setPosition(previousPosition); + + client.waitMotorStop(); + + } catch (IOException e) { + LOG.error("move to initial position", e); + client.stop(); + // force reconnect on the next iteration + status.setStatus(DeviceConnectionStatus.FAILED); + status.setFailureMessage(e.getMessage()); + previousPosition = null; + return; + } + } + + nextPosition = predict.getSatellitePosition(current + config.getCycleMillis(), groundStation, tlePropagator); + if (nextPosition.getElevation() < 0.0) { + LOG.info("[{}] negative elevation requested. most likely stale or invalid tle. cancelling rotation", req.getId()); + throw new RuntimeException("negative elevation"); + } + + try { + client.slew(nextPosition, previousPosition); + previousPosition = nextPosition; + } catch (IOException e) { + LOG.error("can't slew", e); + client.stop(); + // force reconnect on the next iteration + status.setStatus(DeviceConnectionStatus.FAILED); + status.setFailureMessage(e.getMessage()); + previousPosition = null; + return; + } + + } + + }, req.getStartTimeMillis() - current, config.getCycleMillis(), TimeUnit.MILLISECONDS); + return result; + } + + private synchronized boolean ensureClientConnected(String id, boolean shouldLogError) { + if (status.getStatus().equals(DeviceConnectionStatus.CONNECTED)) { + return true; + } + try { + client.start(); + status.setStatus(DeviceConnectionStatus.CONNECTED); + long start = clock.millis(); + String model = client.getMotorBoardVersion(); + long latency = clock.millis() - start; + status.setModel(model); + LOG.info("[{}] initialized for model: {}. communication latency: {} ms", config.getId(), status.getModel(), latency); + return true; + } catch (Exception e) { + status.setStatus(DeviceConnectionStatus.FAILED); + status.setFailureMessage(e.getMessage()); + if (shouldLogError) { + if (id != null) { + Util.logIOException(LOG, "[" + id + "] unable to connect to rotctrld", e); + } else { + Util.logIOException(LOG, "unable to connect to rotctrld", e); + } + } + return false; + } + } + + public RotatorStatus getStatus() { + return status; + } + +} \ No newline at end of file