From 494dcc6ba3961aee339bfa7befe2c4901a04a3ba Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 9 Nov 2021 17:23:35 +1100
Subject: [PATCH] SITL: make SITL::ADSB a SITL::SerialDevice

---
 libraries/SITL/SIM_ADSB.cpp | 30 ++++++++++++++----------------
 libraries/SITL/SIM_ADSB.h   | 26 +++++++++-----------------
 2 files changed, 23 insertions(+), 33 deletions(-)

diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp
index cf3611cf56..4d68e4a8a3 100644
--- a/libraries/SITL/SIM_ADSB.cpp
+++ b/libraries/SITL/SIM_ADSB.cpp
@@ -29,14 +29,17 @@
 
 namespace SITL {
 
-SIM *_sitl;
-
 /*
   update a simulated vehicle
  */
 void ADSB_Vehicle::update(float delta_t)
 {
     if (!initialised) {
+        const SIM *_sitl = AP::sitl();
+        if (_sitl == nullptr) {
+            return;
+        }
+
         initialised = true;
         ICAO_address = (uint32_t)(rand() % 10000);
         snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
@@ -80,7 +83,7 @@ void ADSB_Vehicle::update(float delta_t)
 /*
   update the ADSB peripheral state
 */
-void ADSB::update(void)
+void ADSB::update(const class Aircraft &aircraft)
 {
     if (_sitl == nullptr) {
         _sitl = AP::sitl();
@@ -109,32 +112,25 @@ void ADSB::update(void)
     }
     
     // see if we should do a report
-    send_report();
+    send_report(aircraft);
 }
 
 /*
   send a report to the vehicle control code over MAVLink
 */
-void ADSB::send_report(void)
+void ADSB::send_report(const class Aircraft &aircraft)
 {
     if (AP_HAL::millis() < 10000) {
         // simulated aircraft don't appear until 10s after startup. This avoids a windows
         // threading issue with non-blocking sockets and the initial wait on uartA
         return;
     }
-    if (!mavlink.connected && mav_socket.connect(target_address, target_port_base + 10 * instance)) {
-        ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port_base + 10 * instance);
-        mavlink.connected = true;
-    }
-    if (!mavlink.connected) {
-        return;
-    }
 
     // check for incoming MAVLink messages
     uint8_t buf[100];
     ssize_t ret;
 
-    while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) {
+    while ((ret=read_from_autopilot((char*)buf, sizeof(buf))) > 0) {
         for (uint8_t i=0; i<ret; i++) {
             mavlink_message_t msg;
             mavlink_status_t status;
@@ -185,7 +181,7 @@ void ADSB::send_report(void)
                                            &msg, &heartbeat);
         chan0_status->current_tx_seq = saved_seq;
 
-        mav_socket.send(&msg.magic, len);
+        write_to_autopilot((char*)&msg.magic, len);
 
         last_heartbeat_ms = now;
     }
@@ -194,6 +190,8 @@ void ADSB::send_report(void)
     /*
       send a ADSB_VEHICLE messages
      */
+    const Location &home = aircraft.get_home();
+
     uint32_t now_us = AP_HAL::micros();
     if (now_us - last_report_us >= reporting_period_ms*1000UL) {
         for (uint8_t i=0; i<num_vehicles; i++) {
@@ -246,7 +244,7 @@ void ADSB::send_report(void)
             uint8_t msgbuf[len];
             len = mavlink_msg_to_send_buffer(msgbuf, &msg);
             if (len > 0) {
-                mav_socket.send(msgbuf, len);
+                write_to_autopilot((char*)msgbuf, len);
             }
         }
     }
@@ -270,7 +268,7 @@ void ADSB::send_report(void)
         uint8_t msgbuf[len];
         len = mavlink_msg_to_send_buffer(msgbuf, &msg);
         if (len > 0) {
-            mav_socket.send(msgbuf, len);
+            write_to_autopilot((char*)msgbuf, len);
             ::printf("ADSBsim send tx health packet\n");
         }
     }
diff --git a/libraries/SITL/SIM_ADSB.h b/libraries/SITL/SIM_ADSB.h
index 7a46dd921e..c2e74946bb 100644
--- a/libraries/SITL/SIM_ADSB.h
+++ b/libraries/SITL/SIM_ADSB.h
@@ -50,43 +50,35 @@ private:
     uint64_t stationary_object_created_ms; // allows expiring of slow/stationary objects
 };
         
-class ADSB {
+    class ADSB : public SerialDevice {
 public:
-    ADSB(const struct sitl_fdm &_fdm, const Location& _home, const uint8_t _instance) : home(_home), instance(_instance) {};
-    void update(void);
+    ADSB() {};
+    void update(const class Aircraft &aircraft);
 
 private:
-    const char *target_address = "127.0.0.1";
-    const uint16_t target_port_base = 5762;
-
-    const uint8_t instance = 0;
-
-    const Location& home;
-    uint8_t num_vehicles = 0;
+    uint8_t num_vehicles;
     static const uint8_t num_vehicles_MAX = 200;
     ADSB_Vehicle vehicles[num_vehicles_MAX];
     
     // reporting period in ms
     const float reporting_period_ms = 1000;
-    uint32_t last_report_us = 0;
-    uint32_t last_update_us = 0;
-    uint32_t last_tx_report_ms = 0;
+    uint32_t last_report_us;
+    uint32_t last_update_us;
+    uint32_t last_tx_report_ms;
     
-    uint32_t last_heartbeat_ms = 0;
+    uint32_t last_heartbeat_ms;
     bool seen_heartbeat = false;
     uint8_t vehicle_system_id;
     uint8_t vehicle_component_id;
 
-    SocketAPM mav_socket { false };
     struct {
         // socket to telem2 on aircraft
-        bool connected;
         mavlink_message_t rxmsg;
         mavlink_status_t status;
         uint8_t seq;
     } mavlink {};
 
-    void send_report(void);
+    void send_report(const SITL::Aircraft&);
 };
 
 }  // namespace SITL