commonMain.com.divpundir.mavlink.connection.BufferedMavConnection.kt Maven / Gradle / Ivy

Show more of this group Show more artifacts with this name
Show all versions of connection-core Show documentation
A modern MAVLink library for the JVM written in Kotlin.
The newest version!
package com.divpundir.mavlink.connection

import com.divpundir.mavlink.api.MavDialect
import com.divpundir.mavlink.api.MavFrame
import com.divpundir.mavlink.api.MavMessage
import kotlinx.atomicfu.atomic
import kotlinx.atomicfu.locks.reentrantLock
import kotlinx.atomicfu.locks.withLock
import okio.BufferedSink
import okio.BufferedSource
import okio.Closeable
import okio.IOException

/**
 * A [MavConnection] implementation that reads [MavFrame]s form a [BufferedSource] and writes them to a [BufferedSink].
 * The [dialect] is used to deserialize the raw frame to a [MavFrame]. The [resource] is the origin of the source and
 * the sink. For example, a TCP Socket or a serial port.
 */
public class BufferedMavConnection(
    private val source: BufferedSource,
    private val sink: BufferedSink,
    private val resource: Closeable,
    private val dialect: MavDialect,
) : MavConnection {

    private val readLock = reentrantLock()
    private val writeLock = reentrantLock()

    private val sequence = atomic(0)

    @Throws(IOException::class)
    override fun connect() { }

    @Throws(IOException::class)
    override fun close() {
        resource.close()
    }

    @Throws(IOException::class)
    override fun next(): MavFrame> = readLock.withLock {
        nextFrame()
    }

    private fun nextFrame(): MavFrame> {
        while (true) {
            val rawFrame = nextRawFrame()
            val companion = getMessageCompanionOrNull(rawFrame) ?: continue

            val payload = try {
                companion.deserialize(rawFrame.payload)
            } catch (e: IOException) {
                continue
            }

            return when (rawFrame.stx) {
                MavRawFrame.Stx.V1 -> MavFrameV1Impl(rawFrame, payload)
                MavRawFrame.Stx.V2 -> MavFrameV2Impl(rawFrame, payload)
                else -> throw IllegalStateException("Invalid MAVLink frame marker")
            }
        }
    }

    private fun nextRawFrame(): MavRawFrame {
        while (true) {
            val peeked = source.peek()

            when (peeked.readByte().toUByte()) {
                MavRawFrame.Stx.V1 -> {
                    val payloadSize = peeked.readByte().toUByte().toInt()

                    val remainingSize = MavRawFrame.Sizes.SEQ +
                        MavRawFrame.Sizes.SYS_ID + MavRawFrame.Sizes.COMP_ID + MavRawFrame.Sizes.MSG_ID_V1 +
                        payloadSize + MavRawFrame.Sizes.CHECKSUM

                    if (!peeked.request(remainingSize.toLong())) {
                        source.skip(1)
                        continue
                    }

                    val totalSize = MavRawFrame.Sizes.STX + MavRawFrame.Sizes.LEN + remainingSize.toLong()

                    return MavRawFrame.fromV1Bytes(source.readByteArray(totalSize))
                }

                MavRawFrame.Stx.V2 -> {
                    val payloadSize = peeked.readByte().toUByte().toInt()
                    val incompatibleFlags = peeked.readByte().toUByte()

                    val signatureTotalSize = if (incompatibleFlags == MavRawFrame.Flags.INCOMPAT_SIGNED) {
                        MavRawFrame.Sizes.SIGNATURE_LINK_ID + MavRawFrame.Sizes.SIGNATURE_TIMESTAMP + MavRawFrame.Sizes.SIGNATURE
                    } else {
                        0
                    }

                    val remainingSize = MavRawFrame.Sizes.COMPAT_FLAGS +
                        MavRawFrame.Sizes.SEQ +
                        MavRawFrame.Sizes.SYS_ID + MavRawFrame.Sizes.COMP_ID + MavRawFrame.Sizes.MSG_ID_V2 +
                        payloadSize + MavRawFrame.Sizes.CHECKSUM +
                        signatureTotalSize

                    if (!peeked.request(remainingSize.toLong())) {
                        source.skip(1)
                        continue
                    }

                    val totalSize = MavRawFrame.Sizes.STX + MavRawFrame.Sizes.LEN +
                        MavRawFrame.Sizes.INCOMPAT_FLAGS + remainingSize.toLong()

                    return MavRawFrame.fromV2Bytes(source.readByteArray(totalSize))
                }

                else -> {
                    source.skip(1)
                    continue
                }
            }
        }
    }

    private fun getMessageCompanionOrNull(rawFrame: MavRawFrame): MavMessage.MavCompanion>? {
        val metadata = dialect.resolveCompanionOrNull(rawFrame.messageId) ?: return null
        if (!rawFrame.validateCrc(metadata.crcExtra)) return null
        return metadata
    }

    @Throws(IOException::class)
    override fun > sendV1(
        systemId: UByte,
        componentId: UByte,
        payload: T
    ) {
        send(
            MavRawFrame.createV1(
                seq = sequence.getAndIncrement().toUByte(),
                systemId = systemId,
                componentId = componentId,
                messageId = payload.instanceCompanion.id,
                payload = payload.serializeV1(),
                crcExtra = payload.instanceCompanion.crcExtra,
            )
        )
    }

    @Throws(IOException::class)
    override fun > sendUnsignedV2(
        systemId: UByte,
        componentId: UByte,
        payload: T
    ) {
        send(
            MavRawFrame.createUnsignedV2(
                seq = sequence.getAndIncrement().toUByte(),
                systemId = systemId,
                componentId = componentId,
                messageId = payload.instanceCompanion.id,
                payload = payload.serializeV2(),
                crcExtra = payload.instanceCompanion.crcExtra,
            )
        )
    }

    @Throws(IOException::class)
    override fun > sendSignedV2(
        systemId: UByte,
        componentId: UByte,
        payload: T,
        linkId: UByte,
        timestamp: UInt,
        secretKey: ByteArray
    ) {
        send(
            MavRawFrame.createSignedV2(
                seq = sequence.getAndIncrement().toUByte(),
                systemId = systemId,
                componentId = componentId,
                messageId = payload.instanceCompanion.id,
                payload = payload.serializeV2(),
                crcExtra = payload.instanceCompanion.crcExtra,
                signatureLinkId = linkId,
                signatureTimestamp = timestamp,
                secretKey = secretKey
            )
        )
    }

    @Throws(IOException::class)
    private fun send(rawFrame: MavRawFrame) {
        writeLock.withLock {
            sink.write(rawFrame.rawBytes)
            sink.flush()
        }
    }
}