read message if it's waiting. stop discovery if connection is lost

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/ServiceDiscoverer.kt

@@ -7,6 +7,8 @@ import info.nightscout.androidaps.logging.LTag
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.callbacks.BleCommCallbacks
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.ConnectException
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.CharacteristicType
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.session.Connected
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.session.Connection
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.session.Connection.Companion.STOP_CONNECTING_CHECK_INTERVAL_MS
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.session.ConnectionWaitCondition
 import java.math.BigInteger
@@ -15,7 +17,8 @@ import java.util.*
 class ServiceDiscoverer(
     private val logger: AAPSLogger,
     private val gatt: BluetoothGatt,
-    private val bleCallbacks: BleCommCallbacks
+    private val bleCallbacks: BleCommCallbacks,
+    private val connection: Connection
 ) {
 
     /***
@@ -33,7 +36,7 @@ class ServiceDiscoverer(
         }
         connectionWaitCond.stopConnection?.let {
             while (!bleCallbacks.waitForServiceDiscovery(STOP_CONNECTING_CHECK_INTERVAL_MS)) {
-                if (it.count == 0L) {
+                if (it.count == 0L || connection.connectionState() !is Connected)  {
                     throw ConnectException("stopConnecting called")
                 }
             }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/io/BleIO.kt

@@ -11,6 +11,7 @@ import info.nightscout.androidaps.logging.LTag
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.callbacks.BleCommCallbacks
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.callbacks.WriteConfirmationError
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.callbacks.WriteConfirmationSuccess
+import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.command.BleCommandRTS
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.exceptions.*
 import java.util.concurrent.BlockingQueue
 import java.util.concurrent.TimeUnit
@@ -84,12 +85,17 @@ open class BleIO(
      * Called before sending a new message.
      * The incoming queues should be empty, so we log when they are not.
      */
-    fun flushIncomingQueue() {
+    open fun flushIncomingQueue(): Boolean {
+        var foundRTS = false
         do {
             val found = incomingPackets.poll()?.also {
-                aapsLogger.warn(LTag.PUMPBTCOMM, "BleIO: queue not empty, flushing: {${it.toHex()}")
+                aapsLogger.warn(LTag.PUMPBTCOMM, "BleIO: queue not empty, flushing: ${it.toHex()}")
+                if (it.isNotEmpty() && it[0] == BleCommandRTS.data[0]) {
+                    foundRTS = true
+                }
             }
         } while (found != null)
+        return foundRTS
     }
 
     /**

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/io/CmdBleIO.kt

@@ -2,7 +2,9 @@ package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io
 
 import android.bluetooth.BluetoothGatt
 import android.bluetooth.BluetoothGattCharacteristic
+import info.nightscout.androidaps.extensions.toHex
 import info.nightscout.androidaps.logging.AAPSLogger
+import info.nightscout.androidaps.logging.LTag
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.OmnipodDashBleManagerImpl
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.callbacks.BleCommCallbacks
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.command.BleCommand

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt

@@ -35,7 +35,12 @@ class MessageIO(
 
     @Suppress("ReturnCount")
     fun sendMessage(msg: MessagePacket): MessageSendResult {
-        cmdBleIO.flushIncomingQueue()
+        val foundRTS = cmdBleIO.flushIncomingQueue()
+        if (foundRTS) {
+            val msg = receiveMessage()
+            aapsLogger.warn(LTag.PUMPBTCOMM, "sendMessage received message=$msg")
+            throw IllegalStateException("Received message while trying to send")
+        }
         dataBleIO.flushIncomingQueue()
 
         val rtsSendResult = cmdBleIO.sendAndConfirmPacket(BleCommandRTS.data)
@@ -219,6 +224,6 @@ class MessageIO(
     companion object {
 
         private const val MAX_PACKET_READ_TRIES = 4
-        private const val MESSAGE_READ_TIMEOUT_MS = 2500.toLong()
+        private const val MESSAGE_READ_TIMEOUT_MS = 5000.toLong()
     }
 }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/session/Connection.kt

@@ -89,7 +89,7 @@ class Connection(
         }
         podState.bluetoothConnectionState = OmnipodDashPodStateManager.BluetoothConnectionState.CONNECTED
 
-        val discoverer = ServiceDiscoverer(aapsLogger, gatt, bleCommCallbacks)
+        val discoverer = ServiceDiscoverer(aapsLogger, gatt, bleCommCallbacks, this)
         val discovered = discoverer.discoverServices(connectionWaitCond)
         val cmdBleIO = CmdBleIO(
             aapsLogger,