Fixes to work with FBOSD (support sysId != 1)

andropilot/src/main/scala/com/geeksville/andropilot/service/AndropilotService.scala

@@ -235,9 +235,6 @@ class AndropilotService extends Service with TTSClient with AndroidLogger
 
     actor.useRequestById = !useOldArducopter
 
-    // This lets the vehicle model receive messages from its vehicle...
-    // FIXME - somehow direct by port id instead
-    MavlinkEventBus.subscribe(actor, actor.targetSystem)
     vehicle = Some(actor)
 
     speaker = Some(MockAkka.actorOf(new Speaker(this, actor), "speaker"))

common/src/main/scala/com/geeksville/flight/VehicleClient.scala

@@ -23,8 +23,10 @@ import com.geeksville.akka.InstrumentedActor
 
 /**
  * An endpoint client that talks to a vehicle (adds message retries etc...)
+ *
+ * @param targetOverride if specified then we will only talk with the specified sysId
  */
-class VehicleClient(override val targetSystem: Int = 1) extends HeartbeatMonitor with VehicleSimulator with HeartbeatSender with MavlinkConstants {
+class VehicleClient(targetOverride: Option[Int] = None) extends HeartbeatMonitor with VehicleSimulator with HeartbeatSender with MavlinkConstants {
 
   case class RetryExpired(ctx: RetryContext)
 
@@ -34,8 +36,29 @@ class VehicleClient(override val targetSystem: Int = 1) extends HeartbeatMonitor
 
   private val retries = HashSet[RetryContext]()
 
+  // Default to listening to all traffic until we know the id of our vehicle
+  // This lets the vehicle model receive messages from its vehicle...
+  private var subscriber = MavlinkEventBus.subscribe(this, targetOverride.getOrElse(-1))
+
+  /**
+   * If an override has been set, use that otherwise try to talk to whatever vehicle we've received heartbeats from
+   */
+  override def targetSystem = targetOverride.getOrElse {
+    heartbeatSysId.getOrElse(1)
+  }
+
   override def systemId = 253 // We always claim to be a ground controller (FIXME, find a better way to pick a number)
 
+  override protected def onHeartbeatFound() {
+    if (!targetOverride.isDefined) {
+      // We didn't previously have any particular sysId filter installed.  Now that we know our vehicle
+      // we can be more selective.  Resubscribe with the new system id
+      MavlinkEventBus.removeSubscription(subscriber)
+      subscriber = MavlinkEventBus.subscribe(this, targetSystem)
+    }
+    super.onHeartbeatFound()
+  }
+
   override def onReceive = mReceive.orElse(super.onReceive)
 
   private def mReceive: InstrumentedActor.Receiver = {

common/src/main/scala/com/geeksville/flight/VehicleModel.scala

@@ -76,9 +76,12 @@ case class StatusText(str: String, severity: Int = MsgStatusChanged.SEVERITY_MED
 }
 
 /**
- * Listens to a particular vehicle, capturing interesting state like heartbeat, cur lat, lng, alt, mode, status and next waypoint
+ * Listens to a particular vehicle, capturing interesting state like heartbeat,
+ * cur lat, lng, alt, mode, status and next waypoint
+ *
+ * @param targetOverride if specified then we will only talk with the specified sysId
  */
-class VehicleModel(targetSystem: Int = 1) extends VehicleClient(targetSystem) with WaypointModel with FenceModel {
+class VehicleModel(targetOverride: Option[Int] = None) extends VehicleClient(targetOverride) with WaypointModel with FenceModel {
 
   // We can receive _many_ position updates.  Limit to one update per second (to keep from flooding the gui thread)
   private val locationThrottle = new Throttled(1000)

common/src/main/scala/com/geeksville/flight/VehicleSimulator.scala

@@ -58,7 +58,7 @@ mavlink_version uint8_t_mavlink_version MAVLink version, not writable by user, g
   /**
    * The system we are trying to control
    */
-  def targetSystem: Int = 1
+  protected def targetSystem: Int = 1
 
   /**
    * In special cases (spectator mode, or a RX only radio, we want to suppress _any_ packet sends to the vehicle)

common/src/main/scala/com/geeksville/mavlink/HeartbeatMonitor.scala

@@ -45,6 +45,8 @@ class HeartbeatMonitor extends InstrumentedActor {
 
   def hasHeartbeat = mySysId.isDefined
 
+  def heartbeatSysId = mySysId
+
   def onReceive = {
     case msg: msg_heartbeat =>
       // We don't care about the heartbeats from a GCS