RC2SerialInput added

src/me/drton/jmavsim/RC2SerialInput.java

@@ -0,0 +1,122 @@
+package me.drton.jmavsim;
+
+import jssc.SerialPort;
+import jssc.SerialPortException;
+import me.drton.jmavlib.mavlink.MAVLinkMessage;
+import me.drton.jmavlib.mavlink.MAVLinkSchema;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.channels.ByteChannel;
+
+/**
+ * RC2SerialInput driver, generates MAVLink MANUAL_CONTROL messages from RC2Serial converter.
+ * Can be used to control SIL-simulated system with normal RC transmitter.
+ * Arduino-based RC2Serial bridge required, see tools/RC2Serial/RC2Serial.ino.
+ */
+public class RC2SerialInput extends MAVLinkNode {
+    static private final int PACKET_SIZE = 35;
+    private int sysId;
+    private int componentId;
+    private SerialPort serialPort;
+    private ByteChannel channel = null;
+    private ByteBuffer buffer = ByteBuffer.allocate(1024);
+    private double rcMin = 1000;
+    private double rcTrim = 1500;
+    private double rcMax = 2000;
+    private boolean debug = false;
+
+    public RC2SerialInput(MAVLinkSchema schema, int sysId, int componentId) {
+        super(schema);
+        this.sysId = sysId;
+        this.componentId = componentId;
+    }
+
+    public void setDebug(boolean debug) {
+        this.debug = debug;
+    }
+
+    public void setRCCalibration(double rcMin, double rcTrim, double rcMax) {
+        this.rcMin = rcMin;
+        this.rcTrim = rcTrim;
+        this.rcMax = rcMax;
+    }
+
+    public void open(String portName, int baudRate, int dataBits, int stopBits, int parity) throws IOException {
+        serialPort = new SerialPort(portName);
+        try {
+            serialPort.openPort();
+            serialPort.setParams(baudRate, dataBits, stopBits, parity);
+        } catch (SerialPortException e) {
+            throw new IOException(e);
+        }
+        channel = new SerialPortChannel(serialPort);
+    }
+
+    public void close() throws IOException {
+        try {
+            serialPort.closePort();
+        } catch (SerialPortException e) {
+            throw new IOException(e);
+        }
+        serialPort = null;
+    }
+
+    public boolean isOpened() {
+        return serialPort != null && serialPort.isOpened();
+    }
+
+    @Override
+    public void handleMessage(MAVLinkMessage msg) {
+    }
+
+    @Override
+    public void update(long t) {
+        if (isOpened()) {
+            try {
+                int r = channel.read(buffer);
+                if (r <= 0) {
+                    return;
+                }
+                buffer.flip();
+                while (buffer.remaining() >= PACKET_SIZE) {
+                    if (buffer.get() == (byte) 'R' && buffer.get() == (byte) 'C') {
+                        double[] values = new double[16];
+                        for (int i = 0; i < 16; i++) {
+                            values[i] = (buffer.getShort() & 0xFFFF) / 10.0;
+                        }
+                        if (debug) {
+                            System.out.printf("RC channels:");
+                            for (int i = 0; i < 16; i++) {
+                                System.out.printf("%s ", values[i]);
+                            }
+                            System.out.println();
+                        }
+                        MAVLinkMessage msg = new MAVLinkMessage(schema, "MANUAL_CONTROL", sysId, componentId);
+                        msg.set("target", 0);
+                        msg.set("x", scaleRC(values[1]));
+                        msg.set("y", scaleRC(values[0]));
+                        msg.set("z", scaleRC(values[3]));
+                        msg.set("r", scaleRC(values[2]));
+                        if (debug) {
+                            System.out.println(msg);
+                        }
+                        sendMessage(msg);
+                    }
+                }
+                buffer.compact();
+            } catch (IOException e) {
+                e.printStackTrace();
+                return;
+            }
+        }
+    }
+
+    private int scaleRC(double value) {
+        if (value > rcTrim) {
+            return Math.min(1000, (int) ((value - rcTrim) * 1000.0 / (rcMax - rcTrim)));
+        } else {
+            return Math.max(-1000, (int) ((value - rcTrim) * 1000.0 / (rcTrim - rcMin)));
+        }
+    }
+}

src/me/drton/jmavsim/SerialPortChannel.java

@@ -0,0 +1,60 @@
+package me.drton.jmavsim;
+
+import jssc.SerialPort;
+import jssc.SerialPortException;
+
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.nio.channels.ByteChannel;
+
+public class SerialPortChannel implements ByteChannel {
+    private final SerialPort serialPort;
+
+    public SerialPortChannel(SerialPort serialPort) {
+        this.serialPort = serialPort;
+    }
+
+    @Override
+    public int read(ByteBuffer buffer) throws IOException {
+        try {
+            int available = serialPort.getInputBufferBytesCount();
+            if (available <= 0) {
+                return 0;
+            }
+            byte[] b = serialPort.readBytes(Math.min(available, buffer.remaining()));
+            if (b != null) {
+                buffer.put(b);
+                return b.length;
+            } else {
+                return 0;
+            }
+        } catch (SerialPortException e) {
+            throw new IOException(e);
+        }
+    }
+
+    @Override
+    public int write(ByteBuffer buffer) throws IOException {
+        try {
+            byte[] b = new byte[buffer.remaining()];
+            buffer.get(b);
+            return serialPort.writeBytes(b) ? b.length : 0;
+        } catch (SerialPortException e) {
+            throw new IOException(e);
+        }
+    }
+
+    @Override
+    public boolean isOpen() {
+        return serialPort.isOpened();
+    }
+
+    @Override
+    public void close() throws IOException {
+        try {
+            serialPort.closePort();
+        } catch (SerialPortException e) {
+            throw new IOException(e);
+        }
+    }
+}

tools/RC2Serial/RC2Serial.ino

@@ -0,0 +1,60 @@
+/**
+ * RC2Serial: Simple PPM -> serial converter on Arduino.
+ *
+ * Wiring:
+ * Pin 3: PPM input
+ * Output send via USB
+ *
+ * Serial output on USB: 115200, 8 data bits, 1 stop bit, no parity
+ * Frame contains 2 byte header ("RC") and 16 channels values multiplied by 10 (e.g. 15000 for 1500 us), in uint16_t (big endian):
+ * [uint8_t header0] [uint8_t header0] [uint16_t channel0] [uint16_t channel1] ... [uint16_t channel15]
+ */
+
+#define PPM_Pin 3  // this must be 2 or 3
+#define MULTIPLIER (F_CPU / 1000000 / 8)  // TIMER1 prescaler: 8
+#define PPM_CHANNELS_MAX 16
+
+#pragma push(pack, 1)
+struct {
+    uint8_t header0;
+    uint8_t header1;
+    uint16_t values[PPM_CHANNELS_MAX];
+} data;
+#pragma pop(pack)
+
+void setup() {
+    Serial.begin(115200);
+
+    pinMode(PPM_Pin, INPUT);
+    attachInterrupt(PPM_Pin - 2, read_ppm, FALLING);
+
+    TCCR1A = 0;  // reset TIMER1
+    TCCR1B = 0;
+    TCCR1B |= (1 << CS11);  // set TIMER1 prescaler: 8
+}
+
+void loop() {
+    delay(1000);
+}
+
+void read_ppm() {
+    static byte channel = 0;
+
+    unsigned long counter = TCNT1;
+    TCNT1 = 0;
+
+    if (counter > 2500 * MULTIPLIER) {
+        data.header0 = 'R';
+        data.header1 = 'C';
+        for (int i = channel; i < PPM_CHANNELS_MAX; i++) {
+            data.values[i] = 0;
+        }
+        channel = 0;
+        Serial.write((byte*)&data, sizeof(data));
+    } else {
+        if (channel < PPM_CHANNELS_MAX) {
+            uint16_t v = counter * 10 / MULTIPLIER;
+            data.values[channel++] = (v >> 8 | v << 8); // Swap bytes to big endian
+        }
+    }
+}