From dd27458de8c90fa075f033c251e9a44a7974c0f6 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 15 Apr 2021 17:57:47 -0700
Subject: [PATCH 01/37] fix px4 connection for wsl 2.

---
 AirLib/include/common/AirSimSettings.hpp      |  14 +-
 .../include/sensors/gps/GpsSimpleParams.hpp   |   2 +-
 .../car/firmwares/ardurover/ArduRoverApi.hpp  |   4 +-
 .../firmwares/arducopter/ArduCopterApi.hpp    |   4 +-
 .../firmwares/mavlink/ArduCopterSoloApi.hpp   |   4 +-
 .../mavlink/MavLinkMultirotorApi.hpp          | 316 ++++++++++++++----
 DroneServer/main.cpp                          |   4 +-
 HelloSpawnedDrones/HelloSpawnedDrones.vcxproj |   4 +-
 MavLinkCom/MavLinkCom.vcxproj                 |   1 +
 MavLinkCom/MavLinkCom.vcxproj.filters         |   1 +
 MavLinkCom/common_utils/Utils.hpp             |  27 +-
 MavLinkCom/include/AsyncResult.hpp            |   9 +-
 MavLinkCom/include/MavLinkConnection.hpp      |   7 +-
 MavLinkCom/include/MavLinkDebugLog.hpp        |  37 ++
 MavLinkCom/include/MavLinkNode.hpp            |   2 +-
 MavLinkCom/include/MavLinkVehicle.hpp         |   2 +-
 MavLinkCom/src/MavLinkConnection.cpp          |  15 +-
 MavLinkCom/src/MavLinkVehicle.cpp             |   4 +-
 MavLinkCom/src/impl/MavLinkConnectionImpl.cpp |  24 +-
 MavLinkCom/src/impl/MavLinkConnectionImpl.hpp |   5 +-
 MavLinkCom/src/impl/MavLinkVehicleImpl.cpp    |  34 +-
 MavLinkCom/src/impl/MavLinkVehicleImpl.hpp    |   2 +-
 PythonClient/multirotor/stability_test.py     |  83 +++++
 build.cmd                                     |  69 ++--
 docs/px4_logging.md                           |  19 +-
 docs/px4_sitl.md                              |   8 +
 docs/px4_sitl_wsl2.md                         | 106 ++++++
 mkdocs.yml                                    |   1 +
 28 files changed, 652 insertions(+), 156 deletions(-)
 create mode 100644 MavLinkCom/include/MavLinkDebugLog.hpp
 create mode 100644 PythonClient/multirotor/stability_test.py
 create mode 100644 docs/px4_sitl_wsl2.md

diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index 8300f7a777..fd3b663d1b 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -280,10 +280,10 @@ struct AirSimSettings {
         int tcp_port = 4560;
 
         // The PX4 SITL app requires receiving drone commands over a different mavlink channel called
-        // the "ground control station" channel.
-        // So set this to empty string to disable this separate command channel.
+        // the "ground control station" (or GCS) channel.
         std::string control_ip_address = "127.0.0.1";
-        int control_port = 14580;
+        int control_port_local = 14540;
+        int control_port_remote = 14580;
 
         // The log viewer can be on a different machine, so you can configure it's ip address and port here.
         int logviewer_ip_port = 14388;
@@ -311,6 +311,7 @@ struct AirSimSettings {
         std::string model = "Generic";
 
         std::map<std::string, float> params;
+        std::string logs;
     };
 
     struct MavLinkVehicleSetting : public VehicleSetting {
@@ -714,7 +715,9 @@ struct AirSimSettings {
         connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port);
 
         connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address);
-        connection_info.control_port = settings_json.getInt("ControlPort", connection_info.control_port);
+        connection_info.control_port_local = settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy
+        connection_info.control_port_local = settings_json.getInt("ControlPortLocal", connection_info.control_port_local);
+        connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote);
 
         std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address);
         if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0)
@@ -725,7 +728,7 @@ struct AirSimSettings {
         if (settings_json.hasKey("SitlPort"))
         {
             // backwards compat
-            connection_info.control_port = settings_json.getInt("SitlPort", connection_info.control_port);
+            connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local);
         }
 
         connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip);
@@ -738,6 +741,7 @@ struct AirSimSettings {
         connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port);
         connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate);
         connection_info.model = settings_json.getString("Model", connection_info.model);
+        connection_info.logs = settings_json.getString("Logs", connection_info.logs);
 
         Settings params;
         if (settings_json.getChild("Parameters", params)) {
diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
index 52d8cf88c8..43e38277a5 100644
--- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp
+++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
@@ -12,7 +12,7 @@ namespace msr { namespace airlib {
 struct GpsSimpleParams {
     real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
     real_T eph_initial = 100.0f, epv_initial = 100.0f;   //initially fully diluted positions
-    real_T eph_final = 0.3f, epv_final = 0.4f;
+    real_T eph_final = 0.1f, epv_final = 0.1f;   // PX4 won't converge GPS sensor fusion until we get to 10% accuracty.
     real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;
 
     real_T update_latency = 0.2f;    //sec
diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
index 37ce927c36..b3f8966497 100644
--- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
+++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
@@ -142,10 +142,10 @@ class ArduRoverApi : public CarApiBase {
         }
 
         Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
-        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
 
         udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
-        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
+        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local);
     }
 
 private:
diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
index 400ac408ed..2f7d1992f6 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
@@ -318,10 +318,10 @@ class ArduCopterApi : public MultirotorApiBase {
         }
 
         Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
-        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
 
         udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
-        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
+        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local);
     }
 
 private:
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
index 6190c86ef0..bc1db991f7 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
@@ -113,9 +113,9 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
             }
 
             Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
-            Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
+            Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
 
-            udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port);
+            udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port_local);
             mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg) {
                 this->rotorPowerMessageHandler(connection, msg);
             };
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 4b9c625f93..dd9912cddd 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -20,6 +20,7 @@
 #include <tuple>
 
 #include "common/Common.hpp"
+#include "common/common_utils/FileSystem.hpp"
 #include "common/common_utils/SmoothingFilter.hpp"
 #include "common/common_utils/Timer.hpp"
 #include "common/CommonStructs.hpp"
@@ -81,11 +82,30 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     //reset PX4 stack
     virtual void resetImplementation() override
     {
+        // note this is called AFTER "initialize" when we've connected to the drone
+        // so this method cannot reset any of that connection state.
         MultirotorApiBase::resetImplementation();
-
-        resetState();
         was_reset_ = true;
-        setNormalMode();
+    }
+
+    unsigned long long getSimTime() {
+        // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
+        //if (lock_step_enabled_) {
+            if (sim_time_us_ == 0) {
+                sim_time_us_ = clock()->nowNanos() / 1000;
+            }
+            return sim_time_us_;
+        // }
+        //else {
+        //    return clock()->nowNanos() / 1000;
+        //}
+    }
+
+    void advanceTime() {
+        if (lock_step_enabled_) {
+            // sim_time_us_ += ticks_per_update_;
+        }
+        sim_time_us_ = clock()->nowNanos() / 1000;
     }
 
     //update sensors in PX4 stack
@@ -97,10 +117,16 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
                 return;
 
-            if (send_params_) {
-                send_params_ = false;
-                sendParams();
-            }
+            //auto now = clock()->nowNanos() / 1000;
+
+            //if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now)
+            //{
+            //    // then update() is being called too often, so we just skip this one.
+            //    // TODO: I think this needs to be aware of AirSim ClockSpeed...
+            //    // return;
+            //}
+
+            advanceTime();
 
             //send sensor updates
             const auto& imu_output = getImuData("");
@@ -111,7 +137,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 imu_output.angular_velocity,
                 mag_output.magnetic_field_body,
                 baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
-
+            
+            sendSystemTime();
 
             const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
             if (count_distance_sensors != 0) {
@@ -146,7 +173,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                     sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
                         gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
                 }
-            }
+            }            
+
         }
         catch (std::exception& e) {
             addStatusMessage("Exception sending messages to vehicle");
@@ -273,6 +301,48 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         return rc;
     }
 
+    void onArmed() {
+        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+            auto con = mav_vehicle_->getConnection();
+            if (con != nullptr) {
+                if (log_ != nullptr) {
+                    log_->close();
+                }
+
+                std::time_t t = std::time(0);   // get time now
+                std::tm* now = std::localtime(&t);
+                auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
+                auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder);
+                auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec);
+                auto fullpath = common_utils::FileSystem::combine(path, filename);
+                addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str()));
+                log_ = std::make_shared<mavlinkcom::MavLinkFileLog>();
+                log_->openForWriting(fullpath, false);
+                con->startLoggingSendMessage(log_);
+                con->startLoggingReceiveMessage(log_);
+                if (con != connection_) {
+                    // log the SITL channel also
+                    connection_->startLoggingSendMessage(log_);
+                    connection_->startLoggingReceiveMessage(log_);
+                }
+            }
+        }
+    }
+
+    void onDisarmed() {
+        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+            auto con = mav_vehicle_->getConnection();
+            if (con != nullptr) {
+                con->stopLoggingSendMessage();
+                addStatusMessage("Closing log file.");
+            }
+            connection_->stopLoggingSendMessage();
+            if (log_ != nullptr) {
+                log_->close();
+            }
+        }
+    }
+
     void waitForHomeLocation(float timeout_sec)
     {
         if (!current_state_.home.is_set) {
@@ -712,7 +782,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         if (qgc_proxy_ != nullptr) {
             qgc_proxy_->close();
             qgc_proxy_ = nullptr;
-        }
+        }       
+        
+        resetState();
     }
 
     void connect_thread()
@@ -724,8 +796,12 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             connectToLogViewer();
             connectToQGC();
         }
-        connecting_ = false;
-        connected_ = true;
+
+        if (connecting_) {
+            // only set connected if we haven't already been told to disconnect.
+            connecting_ = false;
+            connected_ = true;
+        }
     }
 
     virtual void close()
@@ -743,9 +819,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
 
     void openAllConnections()
     {
+        Utils::log("Opening mavlink connection");
         close(); //just in case if connections were open
         resetState(); //reset all variables we might have changed during last session
-
         connect();
     }
 
@@ -1034,11 +1110,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         close();
 
         connecting_ = true;
-        got_first_heartbeat_ = false;
-        is_hil_mode_set_ = false;
-        is_armed_ = false;
-        has_home_ = false;
-        is_controls_0_1_ = true;
+        is_controls_0_1_ = true;        
+        std::string remoteIpAddr;
         Utils::setValue(rotor_controls_, 0.0f);
 
         if (connection_info.use_tcp) {
@@ -1050,7 +1123,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             addStatusMessage(msg);
             try {
                 connection_ = std::make_shared<mavlinkcom::MavLinkConnection>();
-                connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port);
+                remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port);
             }
             catch (std::exception& e) {
                 addStatusMessage("Accepting TCP socket failed, is another instance running?");
@@ -1088,28 +1161,52 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
 
         if (connection_info_.control_ip_address != "") {
-            if (connection_info_.control_port == 0) {
-                throw std::invalid_argument("ControlPort setting has an invalid value.");
+            if (connection_info_.control_port_local == 0) {
+                throw std::invalid_argument("LocalControlPort setting has an invalid value.");
+            }
+            if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") {
+                remoteIpAddr = connection_info_.control_ip_address;
+            }
+            if (remoteIpAddr == "local" || remoteIpAddr == "localhost") {
+                remoteIpAddr = "127.0.0.1";
             }
 
             // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection.
             // The HIL mavlink connection can only handle HIL_SENSOR messages.  This separate channel is needed for
             // everything else.
-            addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP...",
-                connection_info_.control_port, connection_info_.local_host_ip.c_str(), connection_info_.control_ip_address.c_str()));
+            addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP %s ...",
+                connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), remoteIpAddr.c_str()));
 
             // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ?
             for (int retries = 60; retries >= 0 && connecting_; retries--) {
                 try {
-                    auto gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
-                        connection_info_.local_host_ip, connection_info_.control_ip_address, connection_info_.control_port);
+                    std::shared_ptr<mavlinkcom::MavLinkConnection> gcsConnection;
+                    if (remoteIpAddr == "127.0.0.1") {                        
+                        gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs",
+                            connection_info_.local_host_ip,
+                            connection_info_.control_port_local);
+                    }
+                    else {
+                        gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
+                            connection_info_.local_host_ip,
+                            remoteIpAddr,
+                            connection_info_.control_port_remote);
+                    }
                     mav_vehicle_->connect(gcsConnection);
+                    // need to try and send something to make sure the connection is good.
+                    mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
+                    break;
                 }
                 catch (std::exception&) {
                     std::this_thread::sleep_for(std::chrono::seconds(1));
                 }
             }
 
+            if (mav_vehicle_ == nullptr) {
+                // play was stopped!
+                return;
+            }
+
             if (mav_vehicle_->getConnection() != nullptr) {
                 addStatusMessage(std::string("Ground control connected over UDP."));
             }
@@ -1117,31 +1214,73 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 addStatusMessage(std::string("Timeout trying to connect ground control over UDP."));
                 return;
             }
+        }        
+        try {
+            connectVehicle();
+        }
+        catch (std::exception& e) {
+            addStatusMessage("Error connecting vehicle:");
+            addStatusMessage(e.what());
         }
+    }
 
-        connectVehicle();
+    void processControlMessages(const mavlinkcom::MavLinkMessage& msg)
+    {
+        // Utils::log(Utils::stringf("Control msg %d", msg.msgid));
+        // PX4 usually sends the following on the control channel.
+        // If nothing is arriving here it means our control channel UDP connection isn't working.
+        // MAVLINK_MSG_ID_HIGHRES_IMU
+        // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET
+        // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW
+        // MAVLINK_MSG_ID_GPS_RAW_INT
+        // MAVLINK_MSG_ID_TIMESYNC
+        // MAVLINK_MSG_ID_ALTITUDE
+        // MAVLINK_MSG_ID_VFR_HUD
+        // MAVLINK_MSG_ID_ESTIMATOR_STATUS
+        // MAVLINK_MSG_ID_EXTENDED_SYS_STATE
+        // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN
+        // MAVLINK_MSG_ID_PING
+        // MAVLINK_MSG_ID_SYS_STATUS
+        // MAVLINK_MSG_ID_SYSTEM_TIME
+        // MAVLINK_MSG_ID_LINK_NODE_STATUS
+        // MAVLINK_MSG_ID_AUTOPILOT_VERSION
+        // MAVLINK_MSG_ID_COMMAND_ACK
+        // MAVLINK_MSG_ID_STATUSTEXT
+        // MAVLINK_MSG_ID_PARAM_VALUE
+        processMavMessages(msg);
     }
 
     void connectVehicle()
     {
         // listen to this UDP mavlink connection also
         auto mavcon = mav_vehicle_->getConnection();
-        if (mavcon != connection_) {
+        if (mavcon != nullptr && mavcon != connection_) {
             mavcon->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
-                unused(connection);
-                processMavMessages(msg);
-                });
+                unused(connection);                
+                processControlMessages(msg);
+            });
         }
         else {
             mav_vehicle_->connect(connection_);
         }
 
         connected_ = true;
-        // now we can start our heartbeats.
-        mav_vehicle_->startHeartbeat();
 
         // Also request home position messages
         mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
+
+        // now we can start our heartbeats.
+        mav_vehicle_->startHeartbeat();
+
+        // wait for px4 to connect so we can safely send any configured parameters
+        while (!send_params_ && connected_) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        if (connected_) {
+            sendParams();
+            send_params_ = false;
+        }
     }
 
     void createMavSerialConnection(const std::string& port_name, int baud_rate)
@@ -1184,15 +1323,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 hil_node_->connect(connection_);
                 addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str()));
 
-                mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
-                mav_vehicle_->connect(connection_); // in this case we can use the same connection.
-                mav_vehicle_->startHeartbeat();
                 // start listening to the HITL connection.
                 connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
                     unused(connection);
                     processMavMessages(msg);
                     });
 
+                mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
+
+                connectVehicle();
                 return;
             }
             catch (std::exception& e) {
@@ -1228,24 +1367,41 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     void sendParams()
     {
         // send any mavlink parameters from settings.json through to the connected vehicle.
-        if (connection_info_.params.size() > 0) {
-            for (auto iter : connection_info_.params) {
-                auto key = iter.first;
-                auto value = iter.second;
-                mavlinkcom::MavLinkParameter p;
-                p.name = key;
-                p.value = value;
-                bool result = false;
-                mav_vehicle_->setParameter(p).wait(1000, &result);
-                if (!result) {
-                    Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str()));
+        if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) {
+            try {
+                for (auto iter : connection_info_.params) {                
+                    auto key = iter.first;
+                    auto value = iter.second;
+                    mavlinkcom::MavLinkParameter p;
+                    p.name = key;
+                    p.value = value;
+                    bool result = false;
+                    mav_vehicle_->setParameter(p).wait(1000, &result);
+                    if (!result) {
+                        Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str()));
+                    }               
                 }
             }
+            catch (std::exception& ex) {
+                addStatusMessage("Exception sending parameters to vehicle");
+                addStatusMessage(ex.what());
+            }
         }
     }
 
     void setArmed(bool armed)
     {
+        if (is_armed_) {
+            if (!armed) {
+                onDisarmed();
+            }
+        }
+        else {
+            if (armed) {
+                onArmed();
+            }
+        }
+
         is_armed_ = armed;
         if (!armed) {
             //reset motor controls
@@ -1296,8 +1452,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                     // and it scales multi rotor servo output to 0 to 1.
                     is_controls_0_1_ = false;
                 }
-
-                send_params_ = true;
             }
             else if (is_simulation_mode_ && !is_hil_mode_set_) {
                 setHILMode();
@@ -1376,7 +1530,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 addStatusMessage("Got GPS Home Location");
                 has_home_ = true;
             }
-
+            send_params_ = true;
         }
         else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) {
             // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff.
@@ -1388,20 +1542,34 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) {
             // check landed state.
             getLandedState();
+            send_params_ = true;
         }
         else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) {
             mavlinkcom::MavLinkHomePosition home;
             home.decode(msg);
+            // this is a good time to send the params
+            send_params_ = true;
+        }
+        else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) {
+            // this is a good time to send the params
+            send_params_ = true;
+        }
+        else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) {
+            // this is a good time to send the params
+            send_params_ = true;
+        }
+        else {
+            // creates too much log output, only do this when debugging this issue specifically.
+            // Utils::log(Utils::stringf("Ignornig msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
         }
-        //else ignore message
     }
 
     void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt)
     {
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
-
-        auto now = clock()->nowNanos() / 1000;
+        
+        auto now = getSimTime();
         if (lock_step_enabled_) {
             if (last_hil_sensor_time_ + 100000 < now) {
                 // if 100 ms passes then something is terribly wrong, reset lockstep mode
@@ -1457,14 +1625,28 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         last_sensor_message_ = hil_sensor;
     }
 
+    void sendSystemTime() {
+        // SYSTEM TIME from host
+        auto tu = getSimTime();
+        uint32_t ms = (uint32_t)(tu / 1000);
+        if (ms != last_sys_time_) {
+            last_sys_time_ = ms;
+            mavlinkcom::MavLinkSystemTime msg_system_time;
+            msg_system_time.time_unix_usec = tu;
+            msg_system_time.time_boot_ms = last_sys_time_;
+            if (hil_node_ != nullptr) {
+                hil_node_->sendMessage(msg_system_time);
+            }
+        }
+    }
+
     void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation)
     {
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
 
         mavlinkcom::MavLinkDistanceSensor distance_sensor;
-
-        distance_sensor.time_boot_ms = static_cast<uint32_t>(clock()->nowNanos() / 1000000);
+        distance_sensor.time_boot_ms = static_cast<uint32_t>(getSimTime() / 1000);
         distance_sensor.min_distance = static_cast<uint16_t>(min_distance);
         distance_sensor.max_distance = static_cast<uint16_t>(max_distance);
         distance_sensor.current_distance = static_cast<uint16_t>(current_distance);
@@ -1491,6 +1673,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog,
         float eph, float epv, int fix_type, unsigned int satellites_visible)
     {
+        unused(satellites_visible);
+
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
 
@@ -1529,10 +1713,17 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         is_controls_0_1_ = true;
         hil_state_freq_ = -1;
         actuators_message_supported_ = false;
-        last_gps_time_ = 0;
         state_version_ = 0;
         current_state_ = mavlinkcom::VehicleState();
         target_height_ = 0;
+        got_first_heartbeat_ = false;
+        is_armed_ = false;
+        has_home_ = false;
+        sim_time_us_ = 0;
+        last_sys_time_ = 0;
+        last_gps_time_ = 0;
+        last_hil_sensor_time_ = 0;
+        hil_sensor_clock_ = 0;
         is_api_control_enabled_ = false;
         thrust_controller_ = PidController();
         Utils::setValue(rotor_controls_, 0.0f);
@@ -1607,15 +1798,12 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_;
 
     //variables required for VehicleApiBase implementation
-    bool got_first_heartbeat_, is_hil_mode_set_, is_armed_;
+    bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false;
     bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1?
     bool send_params_ = false;
     std::queue<std::string> status_messages_;
     int hil_state_freq_;
     bool actuators_message_supported_ = false;
-    uint64_t last_gps_time_ = 0;
-    uint64_t last_hil_sensor_time_ = 0;
-    uint64_t hil_sensor_clock_ = 0;
     bool was_reset_ = false;
     bool has_home_ = false;
     bool is_ready_ = false;
@@ -1631,6 +1819,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     double ground_variance_ = 1;
     const double GroundTolerance = 0.1;
 
+    // variables for throttling HIL_SENSOR and SYSTEM_TIME messages
+    const int DEFAULT_SIM_RATE = 250; // Hz.
+    const unsigned long ticks_per_update_ = 1000000 / DEFAULT_SIM_RATE; // microseconds
+    uint32_t last_sys_time_ = 0;
+    unsigned long long sim_time_us_ = 0;
+    uint64_t last_gps_time_ = 0;
+    uint64_t last_hil_sensor_time_ = 0;
+    uint64_t hil_sensor_clock_ = 0;
+
     //additional variables required for MultirotorApiBase implementation
     //this is optional for methods that might not use vehicle commands
     std::shared_ptr<mavlinkcom::MavLinkVehicle> mav_vehicle_;
@@ -1639,6 +1836,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     PidController thrust_controller_;
     common_utils::Timer hil_message_timer_;
     common_utils::Timer gcs_message_timer_;
+    std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
 
     //every time we return status update, we need to check if we have new data
     //this is why below two variables are marked as mutable
diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp
index b64000cc43..1806ec60c4 100644
--- a/DroneServer/main.cpp
+++ b/DroneServer/main.cpp
@@ -58,7 +58,9 @@ int main(int argc, const char* argv[])
         connection_info.qgc_ip_port = child.getInt("QgcPort", connection_info.qgc_ip_port);
 
         connection_info.control_ip_address = child.getString("ControlIp", connection_info.control_ip_address);
-        connection_info.control_port = child.getInt("ControlPort", connection_info.control_port);
+        connection_info.control_port_local = child.getInt("ControlPort", connection_info.control_port_local);
+        connection_info.control_port_local = child.getInt("ControlPortLocal", connection_info.control_port_local);
+        connection_info.control_port_remote = child.getInt("ControlPortRemote", connection_info.control_port_remote);
 
         connection_info.local_host_ip = child.getString("LocalHostIp", connection_info.local_host_ip);
 
diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj
index d150fadca8..7b61e1a5dc 100644
--- a/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj
+++ b/HelloSpawnedDrones/HelloSpawnedDrones.vcxproj
@@ -117,7 +117,7 @@
   </ItemDefinitionGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
     <ClCompile>
-      <PrecompiledHeader>Use</PrecompiledHeader>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
       <WarningLevel>Level3</WarningLevel>
       <Optimization>Disabled</Optimization>
       <SDLCheck>true</SDLCheck>
@@ -131,7 +131,7 @@
   </ItemDefinitionGroup>
   <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
     <ClCompile>
-      <PrecompiledHeader>Use</PrecompiledHeader>
+      <PrecompiledHeader>NotUsing</PrecompiledHeader>
       <WarningLevel>Level3</WarningLevel>
       <Optimization>MaxSpeed</Optimization>
       <FunctionLevelLinking>true</FunctionLevelLinking>
diff --git a/MavLinkCom/MavLinkCom.vcxproj b/MavLinkCom/MavLinkCom.vcxproj
index 319db2dadf..71c5a5fad6 100644
--- a/MavLinkCom/MavLinkCom.vcxproj
+++ b/MavLinkCom/MavLinkCom.vcxproj
@@ -380,6 +380,7 @@
     <ClInclude Include="common_utils\json.hpp" />
     <ClInclude Include="common_utils\StrictMode.hpp" />
     <ClInclude Include="common_utils\ThreadUtils.hpp" />
+    <ClInclude Include="include\MavLinkDebugLog.hpp" />
     <ClInclude Include="include\UdpSocket.hpp" />
     <ClInclude Include="include\AdHocConnection.hpp" />
     <ClInclude Include="include\AsyncResult.hpp" />
diff --git a/MavLinkCom/MavLinkCom.vcxproj.filters b/MavLinkCom/MavLinkCom.vcxproj.filters
index 0cc6ffdbac..fcaa6c57de 100644
--- a/MavLinkCom/MavLinkCom.vcxproj.filters
+++ b/MavLinkCom/MavLinkCom.vcxproj.filters
@@ -218,6 +218,7 @@
     <ClInclude Include="src\impl\AdHocConnectionImpl.hpp" />
     <ClInclude Include="include\UdpSocket.hpp" />
     <ClInclude Include="src\impl\UdpSocketImpl.hpp" />
+    <ClInclude Include="include\MavLinkDebugLog.hpp" />
   </ItemGroup>
   <ItemGroup>
     <Filter Include="Mavlink">
diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp
index 66f27b6d3d..7c0bd91c85 100644
--- a/MavLinkCom/common_utils/Utils.hpp
+++ b/MavLinkCom/common_utils/Utils.hpp
@@ -22,6 +22,7 @@
 #include <limits>
 #include <queue>
 #include "type_utils.hpp"
+#include "MavLinkDebugLog.hpp"
 
 #ifndef _WIN32
 #include <limits.h> // needed for CHAR_BIT used below
@@ -98,17 +99,6 @@ class Utils {
 
 
 public:
-    class Logger {
-    public:
-        virtual void log(int level, const std::string& message)
-        {
-            if (level >= 0)
-                std::cout << message;
-            else
-                std::cerr << message;
-        }
-    };
-    
     static void enableImmediateConsoleFlush() {
         //disable buffering
         setbuf(stdout, NULL);
@@ -136,25 +126,12 @@ class Utils {
         return static_cast<float>(radians * 180.0f / M_PIf);
     }
 
-    static Logger* getSetLogger(Logger* logger = nullptr)
-    {
-        static Logger logger_default_;
-        static Logger* logger_;
-
-        if (logger != nullptr)
-            logger_ = logger;
-        else if (logger_ == nullptr)
-            logger_ = &logger_default_;
-
-        return logger_;
-    }
-
     static constexpr int kLogLevelInfo = 0;
     static constexpr int kLogLevelWarn = -1;
     static constexpr int kLogLevelError = -2;
     static void log(std::string message, int level = kLogLevelInfo)
     {
-        getSetLogger()->log(level, message);
+        mavlinkcom::MavLinkDebugLog::getSetLogger()->log(level, message);
     }
 
     template <typename T>
diff --git a/MavLinkCom/include/AsyncResult.hpp b/MavLinkCom/include/AsyncResult.hpp
index 0bb6df72e6..64fb1ba310 100644
--- a/MavLinkCom/include/AsyncResult.hpp
+++ b/MavLinkCom/include/AsyncResult.hpp
@@ -30,7 +30,8 @@ namespace mavlinkcom {
 
         ~AsyncResultState() {
             complete();
-        }
+        }        
+        
         int getState() {
             return state_;
         }
@@ -83,6 +84,12 @@ namespace mavlinkcom {
             state_ = nullptr;
         }
 
+        static AsyncResult<T> Completed(T state) {
+            AsyncResult<T> r(nullptr);
+            r.setResult(state);
+            return r;
+        }
+
         void then(ResultHandler handler)
         {
             // keep state alive while we wait for async result.
diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp
index 6dc7cd3e07..c77693b770 100644
--- a/MavLinkCom/include/MavLinkConnection.hpp
+++ b/MavLinkCom/include/MavLinkConnection.hpp
@@ -89,7 +89,8 @@ namespace mavlinkcom {
         // You may need to open this port in your firewall.
         // The  localAddr can also a specific local ip address if you need to specify which
         // NIC to use, for example, wifi versus hard wired ethernet adapter.  For localhost pass 127.0.0.1.
-        void acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort);
+        // It returns the address of the remote machine that connected.
+        std::string acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort);
 
         // instance methods
         std::string getName();
@@ -106,6 +107,10 @@ namespace mavlinkcom {
         void startLoggingSendMessage(std::shared_ptr<MavLinkLog> log);
         void stopLoggingSendMessage();
 
+        // log every message that is "received" by a subscriber.
+        void startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log);
+        void stopLoggingReceiveMessage();
+
         uint8_t getNextSequence();
 
         // Advanced method that create a bridge between two connections.  For example, if you use connectRemoteUdp to connect to 
diff --git a/MavLinkCom/include/MavLinkDebugLog.hpp b/MavLinkCom/include/MavLinkDebugLog.hpp
new file mode 100644
index 0000000000..4dada3d665
--- /dev/null
+++ b/MavLinkCom/include/MavLinkDebugLog.hpp
@@ -0,0 +1,37 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef mavlink_Logger_hpp
+#define mavlink_Logger_hpp
+
+#include <iostream>
+#include <string>
+
+namespace mavlinkcom
+{
+    class MavLinkDebugLog {
+    public:
+        virtual void log(int level, const std::string& message)
+        {
+            if (level >= 0)
+                std::cout << message;
+            else
+                std::cerr << message;
+        }
+
+        static MavLinkDebugLog* getSetLogger(MavLinkDebugLog* logger = nullptr)
+        {
+            static MavLinkDebugLog logger_default_;
+            static MavLinkDebugLog* logger_;
+
+            if (logger != nullptr)
+                logger_ = logger;
+            else if (logger_ == nullptr)
+                logger_ = &logger_default_;
+
+            return logger_;
+        }
+    };
+}
+
+#endif
\ No newline at end of file
diff --git a/MavLinkCom/include/MavLinkNode.hpp b/MavLinkCom/include/MavLinkNode.hpp
index 54ee613d0c..be754090eb 100644
--- a/MavLinkCom/include/MavLinkNode.hpp
+++ b/MavLinkCom/include/MavLinkNode.hpp
@@ -92,7 +92,7 @@ namespace mavlinkcom {
         // passing a message along).
         void sendMessage(MavLinkMessage& msg);
 
-        // send a command to the remote node
+        // send a command to the remote node        
         void sendCommand(MavLinkCommand& cmd);
 
         // send a command to the remote node and get async result that tells you whether it succeeded or not.
diff --git a/MavLinkCom/include/MavLinkVehicle.hpp b/MavLinkCom/include/MavLinkVehicle.hpp
index 81902f83e4..9607041bbf 100644
--- a/MavLinkCom/include/MavLinkVehicle.hpp
+++ b/MavLinkCom/include/MavLinkVehicle.hpp
@@ -37,7 +37,7 @@ namespace mavlinkcom {
         AsyncResult<bool> setMissionMode();
         AsyncResult<MavLinkHomePosition> waitForHomePosition();
         AsyncResult<bool> allowFlightControlOverUsb();
-        AsyncResult<bool>  setMode(int modeFlags, int customMode = 0, int customSubMode = 0);
+        AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
 
         // wait for drone to reach specified local z, ensure it is not just blowing past the z location,
         // wait for it to settle down with dz delta, and dvz delta velocity.
diff --git a/MavLinkCom/src/MavLinkConnection.cpp b/MavLinkCom/src/MavLinkConnection.cpp
index 0d1b93ee98..27070f494a 100644
--- a/MavLinkCom/src/MavLinkConnection.cpp
+++ b/MavLinkCom/src/MavLinkConnection.cpp
@@ -32,9 +32,9 @@ std::shared_ptr<MavLinkConnection>  MavLinkConnection::connectTcp(const std::str
     return MavLinkConnectionImpl::connectTcp(nodeName, localAddr, remoteIpAddr, remotePort);
 }
 
-void  MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort)
+std::string MavLinkConnection::acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort)
 {
-    pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort);
+    return pImpl->acceptTcp(shared_from_this(), nodeName, localAddr, listeningPort);
 }
 
 void MavLinkConnection::startListening(const std::string& nodeName, std::shared_ptr<Port> connectedPort)
@@ -53,6 +53,17 @@ void MavLinkConnection::stopLoggingSendMessage()
     pImpl->stopLoggingSendMessage();
 }
 
+// log every message that is "received"
+void MavLinkConnection::startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log)
+{
+    pImpl->startLoggingReceiveMessage(log);
+}
+
+void MavLinkConnection::stopLoggingReceiveMessage()
+{
+    pImpl->stopLoggingReceiveMessage();
+}
+
 void MavLinkConnection::close()
 {
     pImpl->close();
diff --git a/MavLinkCom/src/MavLinkVehicle.cpp b/MavLinkCom/src/MavLinkVehicle.cpp
index 3d4a183407..8ab9fd3040 100644
--- a/MavLinkCom/src/MavLinkVehicle.cpp
+++ b/MavLinkCom/src/MavLinkVehicle.cpp
@@ -50,10 +50,10 @@ AsyncResult<bool>  MavLinkVehicle::returnToHome()
 	return ptr->returnToHome();
 }
 
-AsyncResult<bool>  MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode)
+AsyncResult<bool>  MavLinkVehicle::setMode(int modeFlags, int customMode, int customSubMode, bool waitForAck)
 {
     auto ptr = static_cast<MavLinkVehicleImpl*>(pImpl.get());
-    return ptr->setMode(modeFlags, customMode, customSubMode);
+    return ptr->setMode(modeFlags, customMode, customSubMode, waitForAck);
 }
 
 bool MavLinkVehicle::isLocalControlSupported()
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
index 2e4655b7bc..1202aa1c40 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
@@ -86,7 +86,7 @@ std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectTcp(const std:
     return createConnection(nodeName, socket);
 }
 
-void MavLinkConnectionImpl::acceptTcp(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, const std::string& localAddr, int listeningPort)
+std::string MavLinkConnectionImpl::acceptTcp(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, const std::string& localAddr, int listeningPort)
 {
     std::string local = localAddr;
     close();
@@ -95,10 +95,13 @@ void MavLinkConnectionImpl::acceptTcp(std::shared_ptr<MavLinkConnection> parent,
     port = socket; // this is so that a call to close() can cancel this blocking accept call.
     socket->accept(localAddr, listeningPort);
 
+    std::string remote = socket->remoteAddress();
+
     socket->setNonBlocking();
     socket->setNoDelay();
 
     parent->startListening(nodeName, socket);
+    return remote;
 }
 
 std::shared_ptr<MavLinkConnection>  MavLinkConnectionImpl::connectSerial(const std::string& nodeName, const std::string& portName, int baudRate, const std::string& initString)
@@ -144,6 +147,17 @@ void MavLinkConnectionImpl::stopLoggingSendMessage()
     sendLog_ = nullptr;
 }
 
+// log every message that is "sent" using sendMessage.
+void MavLinkConnectionImpl::startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log)
+{
+    receiveLog_ = log;
+}
+
+void MavLinkConnectionImpl::stopLoggingReceiveMessage()
+{
+    receiveLog_ = nullptr;
+}
+
 void MavLinkConnectionImpl::close()
 {
     closed = true;
@@ -159,7 +173,8 @@ void MavLinkConnectionImpl::close()
         msg_available_.post();
         publish_thread_.join();
     }
-    sendLog_ = nullptr;
+    sendLog_ = nullptr;    
+    receiveLog_ = nullptr;
 }
 
 bool MavLinkConnectionImpl::isOpen()
@@ -502,7 +517,12 @@ void MavLinkConnectionImpl::drainQueue()
         if (!hasMsg)
         {
             return;
+        }        
+        
+        if (receiveLog_ != nullptr) {
+            receiveLog_->write(message);
         }
+
         // publish the message from this thread, this is safer than publishing from the readPackets thread
         // as it ensures we don't lose messages if the listener is slow.
         if (snapshot_stale) {
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
index 3f43251674..3256ae41d6 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
@@ -36,7 +36,7 @@ namespace mavlinkcom_impl {
         static std::shared_ptr<MavLinkConnection>  connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort);
         static std::shared_ptr<MavLinkConnection>  connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort);
         static std::shared_ptr<MavLinkConnection>  connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort);
-        void acceptTcp(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, const std::string& localAddr, int listeningPort);
+        std::string acceptTcp(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, const std::string& localAddr, int listeningPort);
 
         std::string getName();
         int getTargetComponentId();
@@ -45,6 +45,8 @@ namespace mavlinkcom_impl {
         void startListening(std::shared_ptr<MavLinkConnection> parent, const std::string& nodeName, std::shared_ptr<Port>  connectedPort);
         void startLoggingSendMessage(std::shared_ptr<MavLinkLog> log);
         void stopLoggingSendMessage();
+        void startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log);
+        void stopLoggingReceiveMessage();
         void close();
         bool isOpen();
         void sendMessage(const MavLinkMessageBase& msg);
@@ -74,6 +76,7 @@ namespace mavlinkcom_impl {
         std::string accept_node_name_;
         std::shared_ptr<TcpClientPort> server_;
         std::shared_ptr<MavLinkLog> sendLog_;
+        std::shared_ptr<MavLinkLog> receiveLog_;
 
         struct MessageHandlerEntry {
         public:
diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
index 43cf5097f3..be5067fa63 100644
--- a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
@@ -585,9 +585,8 @@ void MavLinkVehicleImpl::releaseControl()
     control_requested_ = false;
     control_request_sent_ = false;
     vehicle_state_.controls.offboard = false;
-    MavCmdNavGuidedEnable cmd{};
-    cmd.Enable = 0;
-    sendCommand(cmd);
+    setMode(static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
+        static_cast<int>(PX4_CUSTOM_MAIN_MODE_POSCTL), 0, false);
 }
 
 void MavLinkVehicleImpl::checkOffboard()
@@ -609,15 +608,14 @@ void MavLinkVehicleImpl::checkOffboard()
             offboardIdle();
         }
 
-        Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n");		
-        // now the command should succeed.
-        bool r = false;
-        MavCmdNavGuidedEnable cmd{};
-        cmd.Enable = 1;
+        Utils::log("MavLinkVehicleImpl::checkOffboard: sending MavCmdNavGuidedEnable \n");
         // Note: we can't wait for ACK here, I've tried it.  The ACK takes too long to get back to
         // us by which time the PX4 times out offboard mode!!
-        sendCommand(cmd);
-        control_request_sent_ = true;
+        setMode(static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_CUSTOM_MODE_ENABLED),
+            static_cast<int>(PX4_CUSTOM_MAIN_MODE_OFFBOARD), 0, false);
+        control_request_sent_ = true;        
+        // assume this was successful, we'll find out if so in the next heartbeat.
+        vehicle_state_.controls.offboard = true;
     }
 }
 
@@ -679,11 +677,13 @@ void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, b
     writeMessage(msg);
 }
 
-AsyncResult<bool> MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode)
+AsyncResult<bool> MavLinkVehicleImpl::setMode(int mode, int customMode, int customSubMode, bool waitForAck)
 {
     // this mode change take precedence over offboard mode.
-    control_requested_ = false;
-    control_request_sent_ = false;
+    if (customMode != static_cast<int>(PX4_CUSTOM_MAIN_MODE_OFFBOARD)) {
+        control_requested_ = false;
+        control_request_sent_ = false;
+    }
 
     if ((vehicle_state_.mode & static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED)) != 0) {
         mode |= static_cast<int>(MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED); // must preserve this flag.
@@ -696,7 +696,13 @@ AsyncResult<bool> MavLinkVehicleImpl::setMode(int mode, int customMode, int cust
     cmd.Mode = static_cast<float>(mode);
     cmd.CustomMode = static_cast<float>(customMode);
     cmd.CustomSubmode = static_cast<float>(customSubMode);
-    return sendCommandAndWaitForAck(cmd);
+    if (waitForAck) {
+        return sendCommandAndWaitForAck(cmd);
+    }
+    else {
+        sendCommand(cmd);
+        return AsyncResult<bool>::Completed(true);
+    }
 }
 
 AsyncResult<bool>  MavLinkVehicleImpl::setPositionHoldMode()
diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
index 57ab49a435..23b7b9ce25 100644
--- a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
+++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
@@ -39,7 +39,7 @@ namespace mavlinkcom_impl {
 		AsyncResult<MavLinkHomePosition> waitForHomePosition();
 		AsyncResult<bool> allowFlightControlOverUsb();
 		AsyncResult<bool> waitForAltitude(float z, float dz, float dvz);
-        AsyncResult<bool>  setMode(int modeFlags, int customMode = 0, int customSubMode = 0);
+		AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
 
 		// request OFFBOARD control.  
         void requestControl();
diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py
new file mode 100644
index 0000000000..cf1530194f
--- /dev/null
+++ b/PythonClient/multirotor/stability_test.py
@@ -0,0 +1,83 @@
+import os
+import setup_path 
+import airsim
+import time
+import numpy as np
+import sys
+
+script_dir = os.path.dirname(__file__)
+
+client = airsim.MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+
+def play_sound(wavfile):
+    import speaker
+    import wav_reader
+    reader = wav_reader.WavReader()
+    reader.open(wavfile, 512, speaker.Speaker())
+    while True:
+        buffer = reader.read()
+        if buffer is None:
+            break
+
+class Numbers:
+    def __init__(self, name):
+        self.data = []
+        self.name = name
+
+    def add(self, x):
+        self.data += [x]
+
+    def is_unstable(self, amount):
+        a = np.array(self.data)
+        minimum = a.min()
+        maximum = a.max()
+        mean = np.mean(a)
+        stddev = np.std(a)
+        print("{}: min={}, max={}, mean={}, stddev={}".format(self.name, minimum, maximum, mean, stddev))
+        return (maximum - minimum) > amount
+
+while True:
+    x = Numbers("x")
+    y = Numbers("y")
+    z = Numbers("z")
+        
+    print("arming the drone...")
+    client.armDisarm(True)
+
+    while client.getMultirotorState().landed_state == airsim.LandedState.Landed:
+        print("taking off...")
+        client.takeoffAsync().join()
+        time.sleep(1)
+
+    # fly for a minute
+    start = time.time()
+    while time.time() < start + 20:
+        state = client.getMultirotorState()
+        x_val = state.kinematics_estimated.position.x_val
+        y_val = state.kinematics_estimated.position.y_val
+        z_val = state.kinematics_estimated.position.z_val
+        x.add(x_val)
+        y.add(y_val)
+        z.add(z_val)
+        print("x: {}, y: {}, z: {}".format(x_val, y_val, z_val))
+        time.sleep(1)
+
+    print("landing...")
+    client.landAsync().join()
+    
+    print("disarming the drone...")
+    client.armDisarm(False)
+
+    # more than 50 centimeter drift is unacceptable.
+    a = x.is_unstable(0.5)
+    b = y.is_unstable(0.5)
+    c = z.is_unstable(0.5)
+
+    if a or b or c:
+        play_sound(os.path.join(script_dir, "Error.wav"))
+
+    time.sleep(5)
+
+    
diff --git a/build.cmd b/build.cmd
index 82682d8f2e..61f798ce7f 100644
--- a/build.cmd
+++ b/build.cmd
@@ -23,15 +23,31 @@ if "%VisualStudioVersion%" lss "16.0" (
 
 if "%1"=="" goto noargs
 if "%1"=="--no-full-poly-car" set "noFullPolyCar=y"
-if "%1"=="--Debug" set "buildMode=--Debug"
-if "%1"=="--Release" set "buildMode=--Release"
+if "%1"=="--Debug" set "buildMode=Debug"
+if "%1"=="--Release" set "buildMode=Release"
+if "%1"=="--RelWithDebInfo" set "buildMode=RelWithDebInfo"
 
 if "%2"=="" goto noargs
-if "%2"=="--Debug" set "buildMode=--Debug"
-if "%2"=="--Release" set "buildMode=--Release"
+if "%2"=="--Debug" set "buildMode=Debug"
+if "%2"=="--Release" set "buildMode=Release"
+if "%2"=="--RelWithDebInfo" set "buildMode=RelWithDebInfo"
 
 :noargs
 
+set powershell=powershell
+where powershell > nul 2>&1
+if ERRORLEVEL 1 goto :pwsh
+echo found Powershell && goto start
+:pwsh
+set powershell=pwsh
+where pwsh > nul 2>&1
+if ERRORLEVEL 1 goto :nopwsh
+echo found pwsh && goto start
+:nopwsh
+echo Powershell or pwsh not found, please install it.
+goto :eof
+
+:start
 chdir /d %ROOT_DIR% 
 
 REM //---------- Check cmake version ----------
@@ -48,7 +64,7 @@ if ERRORLEVEL 1 (
 REM //---------- get rpclib ----------
 IF NOT EXIST external\rpclib mkdir external\rpclib
 IF NOT EXIST external\rpclib\rpclib-2.2.1 (
-	REM //leave some blank lines because powershell shows download banner at top of console
+	REM //leave some blank lines because %powershell% shows download banner at top of console
 	ECHO(
 	ECHO(   
 	ECHO(   
@@ -56,13 +72,13 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1 (
 	ECHO Downloading rpclib
 	ECHO *****************************************************************************************
 	@echo on
-	powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }"
+	%powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }"
 	@echo off
 	
 	REM //remove any previous versions
 	rmdir external\rpclib /q /s
 
-	powershell -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }"
+	%powershell% -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }"
 	del external\rpclib.zip /q
 	
 	REM //Don't fail the build if the high-poly car is unable to be downloaded
@@ -79,13 +95,11 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1\build mkdir external\rpclib\rpclib-2.2
 cd external\rpclib\rpclib-2.2.1\build
 cmake -G"Visual Studio 16 2019" ..
 
-if "%buildMode%" == "--Debug" (
-cmake --build . --config Debug
-) else if "%buildMode%" == "--Release" (
+if "%buildMode%" == "" (
+cmake --build . 
 cmake --build . --config Release
 ) else (
-cmake --build .
-cmake --build . --config Release
+cmake --build . --config %buildMode%
 )
 
 if ERRORLEVEL 1 goto :buildfailed
@@ -98,20 +112,18 @@ set RPCLIB_TARGET_INCLUDE=AirLib\deps\rpclib\include
 if NOT exist %RPCLIB_TARGET_INCLUDE% mkdir %RPCLIB_TARGET_INCLUDE%
 robocopy /MIR external\rpclib\rpclib-2.2.1\include %RPCLIB_TARGET_INCLUDE%
 
-if "%buildMode%" == "--Debug" (
+if "%buildMode%" == "" (
 robocopy /MIR external\rpclib\rpclib-2.2.1\build\Debug %RPCLIB_TARGET_LIB%\Debug
-) else if "%buildMode%" == "--Release" (
 robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Release
 ) else (
-robocopy /MIR external\rpclib\rpclib-2.2.1\build\Debug %RPCLIB_TARGET_LIB%\Debug
-robocopy /MIR external\rpclib\rpclib-2.2.1\build\Release %RPCLIB_TARGET_LIB%\Release
+robocopy /MIR external\rpclib\rpclib-2.2.1\build\%buildMode% %RPCLIB_TARGET_LIB%\%buildMode%
 )
 
 REM //---------- get High PolyCount SUV Car Model ------------
 IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv mkdir Unreal\Plugins\AirSim\Content\VehicleAdv
 IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
     IF NOT DEFINED noFullPolyCar (
-        REM //leave some blank lines because powershell shows download banner at top of console
+        REM //leave some blank lines because %powershell% shows download banner at top of console
         ECHO(   
         ECHO(   
         ECHO(   
@@ -123,12 +135,12 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
         IF EXIST suv_download_tmp rmdir suv_download_tmp /q /s
         mkdir suv_download_tmp
         @echo on
-        REM powershell -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }"
-        REM powershell -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }"
-        powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }"
+        REM %powershell% -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }"
+        REM %powershell% -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }"
+        %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }"
         @echo off
 		rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV
-        powershell -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }"
+        %powershell% -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }"
         rmdir suv_download_tmp /q /s
         
         REM //Don't fail the build if the high-poly car is unable to be downloaded
@@ -142,9 +154,9 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
 REM //---------- get Eigen library ----------
 IF NOT EXIST AirLib\deps mkdir AirLib\deps
 IF NOT EXIST AirLib\deps\eigen3 (
-    powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }"
-    powershell -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }"
-    powershell -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }"
+    %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }"
+    %powershell% -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }"
+    %powershell% -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }"
     REM move AirLib\deps\eigen* AirLib\deps\del_eigen
     mkdir AirLib\deps\eigen3
     move AirLib\deps\del_eigen\Eigen AirLib\deps\eigen3\Eigen
@@ -155,16 +167,13 @@ IF NOT EXIST AirLib\deps\eigen3 goto :buildfailed
 
 
 REM //---------- now we have all dependencies to compile AirSim.sln which will also compile MavLinkCom ----------
-if "%buildMode%" == "--Debug" (
+if "%buildMode%" == "" (
 msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln
 if ERRORLEVEL 1 goto :buildfailed
-) else if "%buildMode%" == "--Release" (
-msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln
+msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln 
 if ERRORLEVEL 1 goto :buildfailed
 ) else (
-msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln
-if ERRORLEVEL 1 goto :buildfailed
-msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln 
+msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=%buildMode% AirSim.sln
 if ERRORLEVEL 1 goto :buildfailed
 )
 
diff --git a/docs/px4_logging.md b/docs/px4_logging.md
index 9c91c1f55d..9962380306 100644
--- a/docs/px4_logging.md
+++ b/docs/px4_logging.md
@@ -4,7 +4,24 @@ Thanks to [Chris Lovett](https://github.com/clovett) for developing various tool
 
 ## Logging MavLink Messages
 
-The following command will connect MavLinkTest app to the Simulator and enable logging
+AirSim can capture mavlink log files if you add the following to the PX4 section of your `settings.json` file:
+
+```json
+{
+    "SettingsVersion": 1.2,
+    "SimMode": "Multirotor",
+    "Vehicles": {
+        "PX4": {
+            ...,
+            "Logs": "c:/temp/mavlink"
+        }
+    }
+}
+```
+
+AirSim will create a timestamped log file in this folder for each "armed/disarmed" flight session.
+
+You can also create log files using the `MavLinkTest` app to the Simulator and enable logging
 of all mavlink commands to and from the PX4.
 ```
 MavLinkTest -server:127.0.0.1:14550 -logdir:d:\temp
diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md
index d0b242a20e..7a117aee93 100644
--- a/docs/px4_sitl.md
+++ b/docs/px4_sitl.md
@@ -3,6 +3,9 @@
 The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (SITL) version of their stack that runs in Linux. If you are on Windows then you must
 use the [Cygwin Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) as the [Bash On Windows](https://dev.px4.io/master/en/setup/dev_env_windows_bash_on_win.html) toolchain no longer works for SITL.
 
+If you are using WSL2 please read these [additional
+instructions](px4_sitl_wsl2.md).
+
 **Note** that every time you stop the unreal app you have to restart the `px4` app.
 
 
@@ -136,6 +139,11 @@ Local position: x=-0.0326988, y=0.00656854, z=5.48506
 
 If the z coordinate is large like this then takeoff might not work as expected.  Resetting the SITL and simulation should fix that problem.
 
+## WSL 2
+
+Windows Subsystem for Linux version 2 operates in a Virtual Machine.  This requires
+additional setup - see [additional instructions](px4_sitl_wsl2.md).
+
 ## No Remote Control
 
 Notice the above setting is provided in the `params` section of the `settings.json` file:
diff --git a/docs/px4_sitl_wsl2.md b/docs/px4_sitl_wsl2.md
new file mode 100644
index 0000000000..03baaa8936
--- /dev/null
+++ b/docs/px4_sitl_wsl2.md
@@ -0,0 +1,106 @@
+# PX4 Software-in-Loop with WSL 2
+
+The [Windows subsystem for Linux version
+2](https://docs.microsoft.com/en-us/windows/wsl/install-win10) uses a Virtual Machine which has a
+separate IP address from your Windows host machine. This means PX4 cannot find AirSim using
+"localhost" which is the default behavior for PX4.
+
+You will notice that on Windows `ipconfig` returns a new ethernet adapter for WSL like this (notice
+the vEthernet has `(WSL)` in the name:
+
+```plain
+Ethernet adapter vEthernet (WSL):
+
+   Connection-specific DNS Suffix  . :
+   Link-local IPv6 Address . . . . . : fe80::1192:f9a5:df88:53ba%44
+   IPv4 Address. . . . . . . . . . . : 172.31.64.1
+   Subnet Mask . . . . . . . . . . . : 255.255.240.0
+   Default Gateway . . . . . . . . . :
+```
+
+This address `172.31.64.1` is the address that WSL 2 can use to reach your Windows host machine.
+
+Starting with this [PX4 Change
+Request](https://github.com/PX4/PX4-Autopilot/commit/1719ff9892f3c3d034f2b44e94d15527ab09cec6) PX4
+in SITL mode can now connect to AirSim on a different (remote) IP address.  To enable this make sure
+you have a version of PX4 containing this fix and set the following environment variable in linux:
+
+```shell
+export PX4_SIM_HOST_ADDR=172.31.64.1
+```
+
+**Note:** Be sure to update the above address `172.31.64.1` to match what you see from your
+`ipconfig` command.
+
+Open incoming port 4560 using your Windows Firewall settings.
+
+Now on the linux side run `ip address show` and copy the `eth0 inet` address, it should be something
+like `172.31.66.156`.  This is the address Windows needs to know in order to find PX4.
+
+Edit your [AirSim settings](settings.md) file and add `LocalHostIp` to tell AirSim to use the WSL
+ethernet adapter address instead of the default `localhost`.  This will cause AirSim to open the TCP
+port on that adapter which is the address that the PX4 app will be looking for.  Also tell AirSim
+to connect the `ControlIp` UDP channel by setting `ControlIp` to the magic string `remote`.
+This resolves to the WSL 2 remote ip address found in the TCP socket.
+
+```json
+{
+    "SettingsVersion": 1.2,
+    "SimMode": "Multirotor",
+    "Vehicles": {
+        "PX4": {
+            "VehicleType": "PX4Multirotor",
+            "UseSerial": false,
+            "UseTcp": true,
+            "TcpPort": 4560,
+            "ControlIp": "remote",
+            "ControlPort": 14580,
+            "LocalHostIp": "172.31.64.1",
+            "Sensors":{
+                "Barometer":{
+                    "SensorType": 1,
+                    "Enabled": true,
+                    "PressureFactorSigma": 0.0001825
+                }
+            },
+            "Parameters": {
+                "NAV_RCL_ACT": 0,
+                "NAV_DLL_ACT": 0,
+                "COM_OBL_ACT": 1,
+                "LPE_LAT": 47.641468,
+                "LPE_LON": -122.140165
+            }
+        }
+    }
+}
+```
+
+The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
+noise generation.  This setting clamps that down a bit.
+
+Lastly, please edit the Linux file in `ROMFS/px4fmu_common/init.d-posix/rcS` and make sure
+it is looking for the `PX4_SIM_HOST_ADDR` environment variable and is passing that through to the
+PX4 simulator like this:
+
+```shell
+# If PX4_SIM_HOST_ADDR environment variable is empty use localhost.
+if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
+    echo "PX4 SIM HOST: localhost"
+    simulator start -c $simulator_tcp_port
+else
+    echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
+    simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port 
+fi
+```
+
+**Note:** this code might already be there depending on the version of PX4 you are using.
+
+**Note:** please be patient when waiting for the message:
+
+```
+INFO  [simulator] Simulator connected on TCP port 4560.
+```
+
+It can take a little longer to establish the remote connection than it does with `localhost`.
+
+Now you can proceed with the steps shown in [Setting up PX4 Software-in-Loop](px4_sitl.md).
diff --git a/mkdocs.yml b/mkdocs.yml
index aa373a2007..e47e8e57be 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -85,6 +85,7 @@ nav:
     - "MavLink and PX4":
       - "PX4 Setup for AirSim": 'px4_setup.md'
       - "PX4 in SITL": 'px4_sitl.md'
+      - "PX4 SITL with WSL 2": 'px4_sitl_wsl2.md'
       - "PX4 Multi-vehicle in SITL": 'px4_multi_vehicle.md'
       - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ'
       - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0'

From b30ee91cd88f693bbc396df8260f9a609ee7adad Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 15 Apr 2021 18:20:18 -0700
Subject: [PATCH 02/37] enable throttling to 250 hz and fix bug in lockstep,
 should not send HIL_GPS until lockstep is satisfied.

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 69 +++++++++----------
 1 file changed, 33 insertions(+), 36 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index dd9912cddd..58f0af295c 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -90,22 +90,24 @@ class MavLinkMultirotorApi : public MultirotorApiBase
 
     unsigned long long getSimTime() {
         // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
-        //if (lock_step_enabled_) {
+        if (lock_step_enabled_) {
             if (sim_time_us_ == 0) {
                 sim_time_us_ = clock()->nowNanos() / 1000;
             }
             return sim_time_us_;
-        // }
-        //else {
-        //    return clock()->nowNanos() / 1000;
-        //}
+        }
+        else {
+            return clock()->nowNanos() / 1000;
+        }
     }
 
     void advanceTime() {
         if (lock_step_enabled_) {
-            // sim_time_us_ += ticks_per_update_;
+            sim_time_us_ += ticks_per_update_;
+        }
+        else {
+            sim_time_us_ = clock()->nowNanos() / 1000;
         }
-        sim_time_us_ = clock()->nowNanos() / 1000;
     }
 
     //update sensors in PX4 stack
@@ -117,14 +119,28 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
                 return;
 
-            //auto now = clock()->nowNanos() / 1000;
+            auto now = clock()->nowNanos() / 1000;
+            if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now)
+            {
+                // then update() is being called too often, so we just skip this one.
+                // TODO: I think this needs to be aware of AirSim ClockSpeed...but perhasp clock() has already taken that into account?
+                return;
+            }
+
+            if (lock_step_enabled_) {
+                if (last_hil_sensor_time_ + 100000 < now) {
+                    // if 100 ms passes then something is terribly wrong, reset lockstep mode
+                    lock_step_enabled_ = false;
+                    addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
+                }
+
+                if (!received_actuator_controls_) {
+                    // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
+                    return;
+                }
+            }
 
-            //if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now)
-            //{
-            //    // then update() is being called too often, so we just skip this one.
-            //    // TODO: I think this needs to be aware of AirSim ClockSpeed...
-            //    // return;
-            //}
+            last_hil_sensor_time_ = now;
 
             advanceTime();
 
@@ -1511,7 +1527,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             // if the timestamps match then it means we are in lockstep mode.
             if (!lock_step_enabled_) {
                 // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
-                if (hil_sensor_clock_ == HilActuatorControlsMessage.time_usec) {
+                if (getSimTime() == HilActuatorControlsMessage.time_usec) {
                     addStatusMessage("Enabling lockstep mode");
                     lock_step_enabled_ = true;
                 }
@@ -1569,25 +1585,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
         
-        auto now = getSimTime();
-        if (lock_step_enabled_) {
-            if (last_hil_sensor_time_ + 100000 < now) {
-                // if 100 ms passes then something is terribly wrong, reset lockstep mode
-                lock_step_enabled_ = false;
-                addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
-            }
-
-            if (!received_actuator_controls_) {
-                // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
-                return;
-            }
-        }
-
-        hil_sensor_clock_ = now;
-
         mavlinkcom::MavLinkHilSensor hil_sensor;
-        last_hil_sensor_time_ = now;
-        hil_sensor.time_usec = hil_sensor_clock_;
+        hil_sensor.time_usec = getSimTime();
 
         hil_sensor.xacc = acceleration.x();
         hil_sensor.yacc = acceleration.y();
@@ -1679,7 +1678,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
 
         mavlinkcom::MavLinkHilGps hil_gps;
-        hil_gps.time_usec = hil_sensor_clock_;
+        hil_gps.time_usec = getSimTime();
         hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
         hil_gps.lon = static_cast<int32_t>(geo_point.longitude * 1E7);
         hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);
@@ -1723,7 +1722,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         last_sys_time_ = 0;
         last_gps_time_ = 0;
         last_hil_sensor_time_ = 0;
-        hil_sensor_clock_ = 0;
         is_api_control_enabled_ = false;
         thrust_controller_ = PidController();
         Utils::setValue(rotor_controls_, 0.0f);
@@ -1826,7 +1824,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     unsigned long long sim_time_us_ = 0;
     uint64_t last_gps_time_ = 0;
     uint64_t last_hil_sensor_time_ = 0;
-    uint64_t hil_sensor_clock_ = 0;
 
     //additional variables required for MultirotorApiBase implementation
     //this is optional for methods that might not use vehicle commands

From 1f3368b288c40e9585f487a730311c537f560714 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 00:49:37 -0700
Subject: [PATCH 03/37] add mavlink 2 support

---
 MavLinkCom/include/MavLinkLog.hpp |  4 +++
 MavLinkCom/src/MavLinkLog.cpp     | 46 +++++++++++++++++++++++++++----
 2 files changed, 44 insertions(+), 6 deletions(-)

diff --git a/MavLinkCom/include/MavLinkLog.hpp b/MavLinkCom/include/MavLinkLog.hpp
index 63a51f54b2..3c22f95b84 100644
--- a/MavLinkCom/include/MavLinkLog.hpp
+++ b/MavLinkCom/include/MavLinkLog.hpp
@@ -7,8 +7,11 @@
 #include <string>
 #include <stdio.h>
 #include <cstdint>
+#include <mutex>
 #include "MavLinkMessageBase.hpp"
 
+#define MAVLINK_STX_MAVLINK1 0xFE          // marker for old protocol
+
 namespace mavlinkcom
 {
     // This abstract class defines the interface for logging MavLinkMessages.
@@ -27,6 +30,7 @@ namespace mavlinkcom
         bool reading_;
         bool writing_;
         bool json_;
+        std::mutex log_lock_;
     public:
         MavLinkFileLog();
         virtual ~MavLinkFileLog();
diff --git a/MavLinkCom/src/MavLinkLog.cpp b/MavLinkCom/src/MavLinkLog.cpp
index 9ee7d6dc01..9c8fb97c12 100644
--- a/MavLinkCom/src/MavLinkLog.cpp
+++ b/MavLinkCom/src/MavLinkLog.cpp
@@ -8,6 +8,8 @@
 using namespace mavlinkcom;
 using namespace mavlink_utils;
 
+#define MAVLINK_STX_MAVLINK1 0xFE          // marker for old protocol
+
 uint64_t MavLinkFileLog::getTimeStamp()
 {
     return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now().time_since_epoch()).count();
@@ -101,7 +103,10 @@ void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t times
 			if (strongTypedMsg != nullptr) {
                 strongTypedMsg->timestamp = timestamp;
 				std::string line = strongTypedMsg->toJSon();
-				fprintf(ptr_, "    %s\n", line.c_str());
+				{
+					std::lock_guard<std::mutex> lock(log_lock_);
+					fprintf(ptr_, "    %s\n", line.c_str());
+				}
                 delete strongTypedMsg;
 			}
 		}
@@ -112,14 +117,29 @@ void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t times
 			// for compatibility with QGroundControl we have to save the time field in big endian.
             // todo: mavlink2 support?
             timestamp = FlipEndianness(timestamp);
+
+			std::lock_guard<std::mutex> lock(log_lock_);
 			fwrite(&timestamp, sizeof(uint64_t), 1, ptr_);
 			fwrite(&msg.magic, 1, 1, ptr_);
 			fwrite(&msg.len, 1, 1, ptr_);
 			fwrite(&msg.seq, 1, 1, ptr_);
 			fwrite(&msg.sysid, 1, 1, ptr_);
 			fwrite(&msg.compid, 1, 1, ptr_);
-            uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink1 msgid
-			fwrite(&msgid, 1, 1, ptr_);
+
+			if (msg.magic == MAVLINK_STX_MAVLINK1) {
+				uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid
+				fwrite(&msgid, 1, 1, ptr_);
+			}
+			else {
+				// 24 bits.
+				uint8_t msgid = msg.msgid & 0xFF;
+				fwrite(&msgid, 1, 1, ptr_);
+				msgid = (msg.msgid >> 8) & 0xFF;
+				fwrite(&msgid, 1, 1, ptr_);
+				msgid = (msg.msgid >> 16) & 0xFF;
+				fwrite(&msgid, 1, 1, ptr_);
+			}
+
 			fwrite(&msg.payload64, 1, msg.len, ptr_);
 			fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_);
 		}
@@ -162,9 +182,23 @@ bool MavLinkFileLog::read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp)
 		if (s == 0) {
 			return false;
 		}
-        uint8_t msgid = 0;
-		s = fread(&msgid, 1, 1, ptr_);
-        msg.msgid = msgid;
+
+		if (msg.magic == MAVLINK_STX_MAVLINK1) {
+			uint8_t msgid = 0;
+			s = fread(&msgid, 1, 1, ptr_);
+			msg.msgid = msgid;
+		}
+		else {
+			// 24 bits.
+			uint8_t msgid = 0;
+			s = fread(&msgid, 1, 1, ptr_);
+			msg.msgid = msgid;
+			s = fread(&msgid, 1, 1, ptr_);
+			msg.msgid |= (msgid << 8);
+			s = fread(&msgid, 1, 1, ptr_);
+			msg.msgid |= (msgid << 16);
+		}
+
 		if (s < 1) {
 			return false;
 		}

From 1c15604a3e2b14cb19fefcd6d015caca8d74e8d1 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 10:34:51 -0700
Subject: [PATCH 04/37] make test terminate when it finds a problem, so you can
 capture the related logs.

---
 PythonClient/multirotor/stability_test.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py
index cf1530194f..9612892dff 100644
--- a/PythonClient/multirotor/stability_test.py
+++ b/PythonClient/multirotor/stability_test.py
@@ -38,7 +38,9 @@ def is_unstable(self, amount):
         print("{}: min={}, max={}, mean={}, stddev={}".format(self.name, minimum, maximum, mean, stddev))
         return (maximum - minimum) > amount
 
+iteration = 0
 while True:
+    iteration  += 1
     x = Numbers("x")
     y = Numbers("y")
     z = Numbers("z")
@@ -51,9 +53,9 @@ def is_unstable(self, amount):
         client.takeoffAsync().join()
         time.sleep(1)
 
-    # fly for a minute
+    # fly for 2 minutes
     start = time.time()
-    while time.time() < start + 20:
+    while time.time() < start + 120:
         state = client.getMultirotorState()
         x_val = state.kinematics_estimated.position.x_val
         y_val = state.kinematics_estimated.position.y_val
@@ -71,12 +73,14 @@ def is_unstable(self, amount):
     client.armDisarm(False)
 
     # more than 50 centimeter drift is unacceptable.
+    print("Results for iteration {}".format(iteration))
     a = x.is_unstable(0.5)
     b = y.is_unstable(0.5)
     c = z.is_unstable(0.5)
 
     if a or b or c:
         play_sound(os.path.join(script_dir, "Error.wav"))
+        break
 
     time.sleep(5)
 

From 006d4e2ac54f31e33ed595c6cdfbe2d5483027fd Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 15:41:46 -0700
Subject: [PATCH 05/37] Merge settings improvements from PR 3546

---
 AirLib/include/common/AirSimSettings.hpp      | 151 ++++++------------
 .../common/common_utils/FileSystem.hpp        |   3 +-
 AirLib/include/physics/PhysicsEngineBase.hpp  |   2 +-
 AirLib/include/sensors/SensorBase.hpp         |   2 +-
 AirLib/include/sensors/SensorFactory.hpp      |  21 ++-
 .../sensors/barometer/BarometerSimple.hpp     |   4 +-
 .../barometer/BarometerSimpleParams.hpp       |  15 +-
 .../sensors/distance/DistanceSimple.hpp       |   2 +-
 .../sensors/distance/DistanceSimpleParams.hpp |  25 +--
 .../include/sensors/gps/GpsSimpleParams.hpp   |  13 +-
 .../include/sensors/imu/ImuSimpleParams.hpp   |  22 ++-
 .../magnetometer/MagnetometerSimpleParams.hpp |  10 +-
 .../include/vehicles/car/api/CarApiBase.hpp   |   8 +-
 .../vehicles/multirotor/MultiRotorParams.hpp  |   8 +-
 .../mavlink/MavLinkMultirotorApi.hpp          |  29 +++-
 MavLinkCom/include/MavLinkMessageBase.hpp     |  23 +--
 MavLinkCom/src/MavLinkMessageBase.cpp         |   6 +-
 MavLinkCom/src/impl/MavLinkConnectionImpl.cpp |   3 +-
 18 files changed, 185 insertions(+), 162 deletions(-)

diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index fd3b663d1b..b0b3245d18 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -193,7 +193,8 @@ struct AirSimSettings {
     struct SensorSetting {
         SensorBase::SensorType sensor_type;
         std::string sensor_name;
-        bool enabled;
+        bool enabled = true;
+        Settings settings; // imported json data that needs to be parsed by specific sensors.
     };
 
     struct BarometerSetting : SensorSetting {
@@ -209,15 +210,10 @@ struct AirSimSettings {
     };
 
     struct DistanceSetting : SensorSetting {
-        // shared defaults
-        real_T min_distance = 20.0f / 100; //m
-        real_T max_distance = 4000.0f / 100; //m
-        Vector3r position = VectorMath::nanVector();
-        Rotation rotation = Rotation::nanRotation();
-        bool draw_debug_points = false;
     };
 
     struct LidarSetting : SensorSetting {
+        // TODO: move this to use the "Settings settings" instead.
 
         // shared defaults
         uint number_of_channels = 16;
@@ -257,7 +253,7 @@ struct AirSimSettings {
         Rotation rotation = Rotation::nanRotation();
 
         std::map<std::string, CameraSetting> cameras;
-        std::map<std::string, std::unique_ptr<SensorSetting>> sensors;
+        std::map<std::string, std::shared_ptr<SensorSetting>> sensors;
 
         RCSettings rc;
     };
@@ -375,7 +371,7 @@ struct AirSimSettings {
     CameraDirectorSetting camera_director;
     float speed_unit_factor =  1.0f;
     std::string speed_unit_label = "m\\s";
-    std::map<std::string, std::unique_ptr<SensorSetting>> sensor_defaults;
+    std::map<std::string, std::shared_ptr<SensorSetting>> sensor_defaults;
     Vector3r wind = Vector3r::Zero();
 
     std::string settings_text_ = "";
@@ -411,7 +407,7 @@ struct AirSimSettings {
         loadPawnPaths(settings_json, pawn_paths);
         loadOtherSettings(settings_json);
         loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults);
-        loadVehicleSettings(simmode_name, settings_json, vehicles);
+        loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults);
 
         //this should be done last because it depends on vehicles (and/or their type) we have
         loadRecordingSetting(settings_json);
@@ -466,6 +462,19 @@ struct AirSimSettings {
         return it->second.get();
     }
 
+    static Vector3r createVectorSetting(const Settings& settings_json, const Vector3r& default_vec)
+    {
+        return Vector3r(settings_json.getFloat("X", default_vec.x()),
+            settings_json.getFloat("Y", default_vec.y()),
+            settings_json.getFloat("Z", default_vec.z()));
+    }
+    static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
+    {
+        return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
+            settings_json.getFloat("Pitch", default_rot.pitch),
+            settings_json.getFloat("Roll", default_rot.roll));
+    }
+
 private:
     void checkSettingsVersion(const Settings& settings_json)
     {
@@ -756,21 +765,9 @@ struct AirSimSettings {
         return vehicle_setting_p;
     }
 
-    static Vector3r createVectorSetting(const Settings& settings_json, const Vector3r& default_vec)
-    {
-        return Vector3r(settings_json.getFloat("X", default_vec.x()),
-            settings_json.getFloat("Y", default_vec.y()),
-            settings_json.getFloat("Z", default_vec.z()));
-    }
-    static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
-    {
-        return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
-            settings_json.getFloat("Pitch", default_rot.pitch),
-            settings_json.getFloat("Roll", default_rot.roll));
-    }
-
     static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name,  const Settings& settings_json,
-        const std::string vehicle_name)
+        const std::string vehicle_name,
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
     {
         auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
@@ -814,7 +811,7 @@ struct AirSimSettings {
         vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation);
 
         loadCameraSettings(settings_json, vehicle_setting->cameras);
-        loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors);
+        loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults);
 
         return vehicle_setting;
     }
@@ -856,7 +853,8 @@ struct AirSimSettings {
     }
 
     static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json,
-        std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles)
+        std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles,
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
     {
         initializeVehicleSettings(simmode_name, vehicles);
 
@@ -872,7 +870,7 @@ struct AirSimSettings {
             for (const auto& key : keys) {
                 msr::airlib::Settings child;
                 vehicles_child.getChild(key, child);
-                vehicles[key] = createVehicleSetting(simmode_name, child, key);
+                vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults);
             }
         }
     }
@@ -950,7 +948,7 @@ struct AirSimSettings {
 
     static void initializeNoiseSettings(std::map<int, NoiseSetting>&  noise_settings)
     {
-        int image_count = Utils::toNumeric(ImageType::Count);
+        const int image_count = Utils::toNumeric(ImageType::Count);
         noise_settings.clear();
         for (int i = -1; i < image_count; ++i)
             noise_settings[i] = NoiseSetting();
@@ -1204,48 +1202,6 @@ struct AirSimSettings {
         clock_speed = settings_json.getFloat("ClockSpeed", 1.0f);
     }
 
-    static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json)
-    {
-        unused(barometer_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
-    }
-
-    static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json)
-    {
-        unused(imu_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
-    }
-
-    static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json)
-    {
-        unused(gps_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
-    }
-
-    static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json)
-    {
-        unused(magnetometer_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
-    }
-
-    static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json)
-    {
-        distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance);
-        distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
-        distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points);
-
-        distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
-        distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
-    }
-
     static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json)
     {
         lidar_setting.number_of_channels = settings_json.getInt("NumberOfChannels", lidar_setting.number_of_channels);
@@ -1264,30 +1220,30 @@ struct AirSimSettings {
         lidar_setting.rotation = createRotationSetting(settings_json, lidar_setting.rotation);
     }
 
-    static std::unique_ptr<SensorSetting> createSensorSetting(
+    static std::shared_ptr<SensorSetting> createSensorSetting(
         SensorBase::SensorType sensor_type, const std::string& sensor_name,
         bool enabled)
     {
-        std::unique_ptr<SensorSetting> sensor_setting;
+        std::shared_ptr<SensorSetting> sensor_setting;
 
         switch (sensor_type) {
         case SensorBase::SensorType::Barometer:
-            sensor_setting = std::unique_ptr<SensorSetting>(new BarometerSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new BarometerSetting());
             break;
         case SensorBase::SensorType::Imu:
-            sensor_setting = std::unique_ptr<SensorSetting>(new ImuSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new ImuSetting());
             break;
         case SensorBase::SensorType::Gps:
-            sensor_setting = std::unique_ptr<SensorSetting>(new GpsSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new GpsSetting());
             break;
         case SensorBase::SensorType::Magnetometer:
-            sensor_setting = std::unique_ptr<SensorSetting>(new MagnetometerSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new MagnetometerSetting());
             break;
         case SensorBase::SensorType::Distance:
-            sensor_setting = std::unique_ptr<SensorSetting>(new DistanceSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new DistanceSetting());
             break;
         case SensorBase::SensorType::Lidar:
-            sensor_setting = std::unique_ptr<SensorSetting>(new LidarSetting());
+            sensor_setting = std::shared_ptr<SensorSetting>(new LidarSetting());
             break;
         default:
             throw std::invalid_argument("Unexpected sensor type");
@@ -1303,35 +1259,32 @@ struct AirSimSettings {
     static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json)
     {
         sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled);
+        
+        // pass the json Settings property bag through to the specific sensor params object where it is
+        // extracted there.  This way default values can be kept in one place.  For example, see the 
+        // BarometerSimpleParams::initializeFromSettings method.
+        sensor_setting->settings = settings_json;
 
+        // TODO: delete this when we move initializeLidarSetting to LidarSimpleParams.hpp 
         switch (sensor_setting->sensor_type) {
-        case SensorBase::SensorType::Barometer:
-            initializeBarometerSetting(*static_cast<BarometerSetting*>(sensor_setting), settings_json);
-            break;
-        case SensorBase::SensorType::Imu:
-            initializeImuSetting(*static_cast<ImuSetting*>(sensor_setting), settings_json);
-            break;
-        case SensorBase::SensorType::Gps:
-            initializeGpsSetting(*static_cast<GpsSetting*>(sensor_setting), settings_json);
-            break;
-        case SensorBase::SensorType::Magnetometer:
-            initializeMagnetometerSetting(*static_cast<MagnetometerSetting*>(sensor_setting), settings_json);
-            break;
-        case SensorBase::SensorType::Distance:
-            initializeDistanceSetting(*static_cast<DistanceSetting*>(sensor_setting), settings_json);
-            break;
         case SensorBase::SensorType::Lidar:
             initializeLidarSetting(*static_cast<LidarSetting*>(sensor_setting), settings_json);
             break;
-        default:
-            throw std::invalid_argument("Unexpected sensor type");
         }
     }
 
     // creates and intializes sensor settings from json
     static void loadSensorSettings( const Settings& settings_json, const std::string& collectionName,
-        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors,
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+
     {
+        // start with defaults.
+        for (auto ptr = sensor_defaults.begin(); ptr != sensor_defaults.end(); ptr++) {
+            auto key = ptr->first;
+            sensors[key] = sensor_defaults[key];
+        }
+
         msr::airlib::Settings sensors_child;
         if (settings_json.getChild(collectionName, sensors_child)) {
             std::vector<std::string> keys;
@@ -1352,7 +1305,7 @@ struct AirSimSettings {
 
     // creates default sensor list when none specified in json
     static void createDefaultSensorSettings(const std::string& simmode_name,
-        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
     {
         if (simmode_name == kSimModeTypeMultirotor) {
             sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true);
@@ -1371,11 +1324,11 @@ struct AirSimSettings {
     // loads or creates default sensor list
     static void loadDefaultSensorSettings(const std::string& simmode_name,
         const Settings& settings_json,
-        std::map<std::string, std::unique_ptr<SensorSetting>>& sensors)
+        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
     {
         msr::airlib::Settings sensors_child;
         if (settings_json.getChild("DefaultSensors", sensors_child))
-            loadSensorSettings(settings_json, "DefaultSensors", sensors);
+            loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors);
         else
             createDefaultSensorSettings(simmode_name, sensors);
     }
diff --git a/AirLib/include/common/common_utils/FileSystem.hpp b/AirLib/include/common/common_utils/FileSystem.hpp
index d6e7fba778..2d3ae8f994 100644
--- a/AirLib/include/common/common_utils/FileSystem.hpp
+++ b/AirLib/include/common/common_utils/FileSystem.hpp
@@ -104,8 +104,7 @@ class FileSystem
                 break;
         }
         if (i < 0) return "";
-        auto ui = static_cast<uint>(i);
-        return str.substr(ui, len - ui);
+        return str.substr(i, len - i);
     }
 
     static std::string getLogFolderPath(bool folder_timestamp, const std::string& parent = "")
diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp
index 2dff630ab4..a8b09f28ba 100644
--- a/AirLib/include/physics/PhysicsEngineBase.hpp
+++ b/AirLib/include/physics/PhysicsEngineBase.hpp
@@ -44,7 +44,7 @@ class PhysicsEngineBase : public UpdatableObject {
     virtual void erase_remove(TUpdatableObjectPtr obj) { 
         members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
 
-    virtual void setWind(const Vector3r& wind) {};
+    virtual void setWind(const Vector3r& wind) { unused(wind); };
 
 private:
     MembersContainer members_;
diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp
index 765cfabcf1..44bd8531aa 100644
--- a/AirLib/include/sensors/SensorBase.hpp
+++ b/AirLib/include/sensors/SensorBase.hpp
@@ -59,7 +59,7 @@ class SensorBase : public UpdatableObject  {
 
 private:
     //ground truth can be shared between many sensors
-    GroundTruth ground_truth_;
+    GroundTruth ground_truth_ = { nullptr, nullptr };
     std::string name_ = "";
 };
 
diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp
index ed69b4abb7..0c93412ec5 100644
--- a/AirLib/include/sensors/SensorFactory.hpp
+++ b/AirLib/include/sensors/SensorFactory.hpp
@@ -19,18 +19,18 @@ class SensorFactory {
 public:
 
     // creates one sensor from settings
-    virtual std::unique_ptr<SensorBase> createSensorFromSettings(
+    virtual std::shared_ptr<SensorBase> createSensorFromSettings(
         const AirSimSettings::SensorSetting* sensor_setting) const
     {
         switch (sensor_setting->sensor_type) {
         case SensorBase::SensorType::Imu:
-            return std::unique_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
+            return std::shared_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
         case SensorBase::SensorType::Magnetometer:
-            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
+            return std::shared_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
         case SensorBase::SensorType::Gps:
-            return std::unique_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
+            return std::shared_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
         case SensorBase::SensorType::Barometer:
-            return std::unique_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
+            return std::shared_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
         default:
             throw new std::invalid_argument("Unexpected sensor type");
         }
@@ -38,9 +38,9 @@ class SensorFactory {
 
     // creates sensor-collection
     virtual void createSensorsFromSettings(
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
+        const std::map<std::string, std::shared_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
         SensorCollection& sensors,
-        vector<unique_ptr<SensorBase>>& sensor_storage) const
+        vector<shared_ptr<SensorBase>>& sensor_storage) const
     {
         for (const auto& sensor_setting_pair : sensors_settings) {
             const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
@@ -49,11 +49,10 @@ class SensorFactory {
             if (sensor_setting == nullptr || !sensor_setting->enabled)
                 continue;
 
-            std::unique_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
+            std::shared_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
             if (sensor) {
-                    SensorBase* sensor_temp = sensor.get();
-                    sensor_storage.push_back(std::move(sensor));
-                    sensors.insert(sensor_temp, sensor_setting->sensor_type);
+                sensor_storage.push_back(sensor);
+                sensors.insert(sensor.get(), sensor_setting->sensor_type);
             }
         }
     }
diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp
index 399eb86cd7..515ae21e57 100644
--- a/AirLib/include/sensors/barometer/BarometerSimple.hpp
+++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp
@@ -27,7 +27,7 @@ class BarometerSimple  : public BarometerBase {
         //GM process that would do random walk for pressure factor
         pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
 
         //initialize frequency limiter
@@ -76,7 +76,7 @@ class BarometerSimple  : public BarometerBase {
         auto altitude = ground_truth.environment->getState().geo_point.altitude;
         auto pressure = EarthUtils::getStandardPressure(altitude);
 
-        //add drift in pressure, about 10m change per hour
+        //add drift in pressure, about 10m change per hour using default settings.
         pressure_factor_.update();
         pressure += pressure * pressure_factor_.getOutput();
 
diff --git a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
index 62082342dd..45b4f6601c 100644
--- a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
+++ b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
@@ -5,6 +5,7 @@
 #define msr_airlib_BarometerSimpleParams_hpp
 
 #include "common/Common.hpp"
+#include "common/AirSimSettings.hpp"
 
 
 namespace msr { namespace airlib {
@@ -33,14 +34,14 @@ struct BarometerSimpleParams {
 
          real_T correlated_noise_sigma = 0.27f;
          real_T correlated_noise_tau = 0.87f;
-         real_T unnorrelated_noise_sigma = 0.24f;
+         real_T uncorrelated_noise_sigma = 0.24f;
     */
 
     //Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
     //http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
-    real_T unnorrelated_noise_sigma = 0.027f * 100;
+    real_T uncorrelated_noise_sigma = 0.027f * 100;
     //jMavSim uses below
-    //real_T unnorrelated_noise_sigma = 0.1f;
+    //real_T uncorrelated_noise_sigma = 0.1f;
 
     //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
     real_T update_latency = 0.0f;    //sec
@@ -49,7 +50,13 @@ struct BarometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma);
+        pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau);
+        uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 
diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp
index 45adcdddc3..63de52c15a 100644
--- a/AirLib/include/sensors/distance/DistanceSimple.hpp
+++ b/AirLib/include/sensors/distance/DistanceSimple.hpp
@@ -22,7 +22,7 @@ class DistanceSimple : public DistanceBase {
         // initialize params
         params_.initializeFromSettings(setting);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
 
 
diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
index 3af863297e..f96051b052 100644
--- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
+++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
@@ -31,13 +31,13 @@ struct DistanceSimpleParams {
 
      real_T correlated_noise_sigma = 0.27f;
      real_T correlated_noise_tau = 0.87f;
-     real_T unnorrelated_noise_sigma = 0.24f;
+     real_T uncorrelated_noise_sigma = 0.24f;
 
 */
     //TODO: update sigma based on documentation, maybe as a function increasing with measured distance
-    real_T unnorrelated_noise_sigma = 0.002f * 100;
+    real_T uncorrelated_noise_sigma = 0.002f * 100;
     //jMavSim uses below
-    //real_T unnorrelated_noise_sigma = 0.1f;
+    //real_T uncorrelated_noise_sigma = 0.1f;
 
     //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
     real_T update_latency = 0.0f;    //sec
@@ -46,14 +46,17 @@ struct DistanceSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
     {
-        std::string simmode_name = AirSimSettings::singleton().simmode_name;
+        const auto& settings_json = settings.settings;
+        min_distance = settings_json.getFloat("MinDistance", min_distance);
+        max_distance = settings_json.getFloat("MaxDistance", max_distance);
+        draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
 
-        min_distance = settings.min_distance;
-        max_distance = settings.max_distance;
+        auto position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
+        auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
 
-        draw_debug_points = settings.draw_debug_points;
+        std::string simmode_name = AirSimSettings::singleton().simmode_name;
 
-        relative_pose.position = settings.position;
+        relative_pose.position = position;
         if (std::isnan(relative_pose.position.x()))
             relative_pose.position.x() = 0;
         if (std::isnan(relative_pose.position.y()))
@@ -66,9 +69,9 @@ struct DistanceSimpleParams {
         }
 
         float pitch, roll, yaw;
-        pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
-        roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
-        yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
+        pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
+        roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
+        yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
         relative_pose.orientation = VectorMath::toQuaternion(
             Utils::degreesToRadians(pitch),   //pitch - rotation around Y axis
             Utils::degreesToRadians(roll),    //roll  - rotation around X axis
diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
index 43e38277a5..18e9949dd8 100644
--- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp
+++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
@@ -21,7 +21,18 @@ struct GpsSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        eph_time_constant = json.getFloat("EPH_TimeConstant", eph_time_constant);
+        epv_time_constant = json.getFloat("EPV_TimeConstant", epv_time_constant);
+        eph_initial = json.getFloat("EphInitial", eph_initial);
+        epv_initial = json.getFloat("EpvInitial", epv_initial);
+        eph_final = json.getFloat("EphFinal", eph_final);
+        epv_final = json.getFloat("EpvFinal", epv_final);
+        eph_min_3d = json.getFloat("EphMin3d", eph_min_3d);
+        eph_min_2d = json.getFloat("EphMin2d", eph_min_2d);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 
diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
index 6cbe166a4e..8dfb7529a8 100644
--- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp
+++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
@@ -25,7 +25,7 @@ struct ImuSimpleParams {
     For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf
     */
     struct Gyroscope {
-        //angule random walk (ARW)
+        //angular random walk (ARW)
         real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
         //Bias Stability (tau = 500s)
         real_T tau = 500;
@@ -46,7 +46,25 @@ struct ImuSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::ImuSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        float arw = json.getFloat("AngularRandomWalk", Utils::nan<float>());
+        if (!std::isnan(arw)) {
+            gyro.arw = arw / sqrt(3600.0f) * M_PIf / 180; // //deg/sqrt(hour) converted to rad/sqrt(sec)
+        }
+        gyro.tau = json.getFloat("GyroBiasStabilityTau", gyro.tau);
+        float bias_stability = json.getFloat("GyroBiasStability", Utils::nan<float>());
+        if (!std::isnan(bias_stability)) {
+            gyro.bias_stability = bias_stability / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
+        }
+        auto vrw = json.getFloat("VelocityRandomWalk", Utils::nan<float>());
+        if (!std::isnan(vrw)) {
+            accel.vrw = vrw * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 
+        }
+        accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau);
+        bias_stability = json.getFloat("AccelBiasStability", Utils::nan<float>());
+        if (!std::isnan(bias_stability)) {
+            accel.bias_stability = bias_stability * 1E-6f * 9.80665f; //ug converted to m/s^2
+        }
     }
 };
 
diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
index 652cb3257c..b0a7aab325 100644
--- a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
+++ b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
@@ -34,7 +34,15 @@ struct MagnetometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        float noise = json.getFloat("NoiseSigma", noise_sigma.x());
+        noise_sigma = Vector3r(noise, noise, noise);
+        scale_factor = json.getFloat("ScaleFactor", scale_factor);
+        float bias = json.getFloat("NoiseBias", noise_bias.x());
+        noise_bias = Vector3r(bias, bias, bias);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 
diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp
index 3f012b51e7..2cbbb1cfb9 100644
--- a/AirLib/include/vehicles/car/api/CarApiBase.hpp
+++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp
@@ -131,11 +131,7 @@ class CarApiBase : public VehicleApiBase  {
 
     void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
     {
-        // use sensors from vehicle settings; if empty list, use default sensors.
-        // note that the vehicle settings completely override the default sensor "list";
-        // there is no piecemeal add/remove/update per sensor.
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensor_settings
-            = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults;
+        const auto& sensor_settings = vehicle_setting->sensors;
 
         sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
     }
@@ -149,7 +145,7 @@ class CarApiBase : public VehicleApiBase  {
 
     std::shared_ptr<const SensorFactory> sensor_factory_;
     SensorCollection sensors_; //maintains sensor type indexed collection of sensors
-    vector<unique_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
+    vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
 
 protected:
     virtual void resetImplementation() override
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
index f04635e44a..75f24ea1c6 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
@@ -85,11 +85,7 @@ class MultiRotorParams {
 
     void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
     {
-        // use sensors from vehicle settings; if empty list, use default sensors.
-        // note that the vehicle settings completely override the default sensor "list";
-        // there is no piecemeal add/remove/update per sensor.
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensor_settings
-            = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults;
+        const auto& sensor_settings = vehicle_setting->sensors;
 
         getSensorFactory()->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
     }
@@ -412,7 +408,7 @@ class MultiRotorParams {
 private:
     Params params_;
     SensorCollection sensors_; //maintains sensor type indexed collection of sensors
-    vector<unique_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
+    vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
 };
 
 }} //namespace
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 58f0af295c..b9961cd10e 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -127,6 +127,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 return;
             }
 
+            update_count_++;
+            if (last_fps_time_ == 0 || last_fps_time_ + 1000000 < now) {
+                // compute update rate once per second.
+                update_rate_ = update_count_;
+                update_count_ = 0;
+                last_fps_time_ = now;
+                sendTelemetry(1);
+            }
+
             if (lock_step_enabled_) {
                 if (last_hil_sensor_time_ + 100000 < now) {
                     // if 100 ms passes then something is terribly wrong, reset lockstep mode
@@ -556,7 +565,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
 
     virtual void sendTelemetry(float last_interval = -1) override
     {
-        if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) {
+        if ((logviewer_proxy_ == nullptr && log_ == nullptr) || connection_ == nullptr || mav_vehicle_ == nullptr) {
             return;
         }
         mavlinkcom::MavLinkTelemetry data;
@@ -599,7 +608,17 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
 
         data.renderTime = static_cast<int64_t>(last_interval * 1000000);// microseconds
-        logviewer_proxy_->sendMessage(data);
+        data.udpateRateHz = update_rate_;
+
+        if (logviewer_proxy_ != nullptr) {
+            logviewer_proxy_->sendMessage(data);
+        }
+
+        if (log_ != nullptr) {
+            mavlinkcom::MavLinkMessage msg;
+            data.encode(msg);
+            log_->write(msg);
+        }
     }
 
     virtual float getCommandPeriod() const override
@@ -1722,6 +1741,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         last_sys_time_ = 0;
         last_gps_time_ = 0;
         last_hil_sensor_time_ = 0;
+        update_rate_ = 0;
+        update_count_ = 0;
+        last_fps_time_ = 0;
         is_api_control_enabled_ = false;
         thrust_controller_ = PidController();
         Utils::setValue(rotor_controls_, 0.0f);
@@ -1824,6 +1846,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     unsigned long long sim_time_us_ = 0;
     uint64_t last_gps_time_ = 0;
     uint64_t last_hil_sensor_time_ = 0;
+    uint64_t last_fps_time_ = 0;
+    uint32_t update_count_ = 0;
+    uint32_t update_rate_ = 0;
 
     //additional variables required for MultirotorApiBase implementation
     //this is optional for methods that might not use vehicle commands
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index e17c09aef5..b49e7d9ef1 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -132,15 +132,19 @@ namespace mavlinkcom
     class MavLinkTelemetry : public MavLinkMessageBase {
     public:
         const static uint8_t kMessageId = 204; // in the user range 180-229.
-        MavLinkTelemetry() : wifiInterfaceName(nullptr) { msgid = kMessageId; }
-        uint32_t messagesSent;		 // number of messages sent since the last telemetry message
-        uint32_t messagesReceived;	 // number of messages received since the last telemetry message
-        uint32_t messagesHandled;	 // number of messages handled since the last telemetry message
-        uint32_t crcErrors;			 // # crc errors detected in mavlink stream since the last telemetry message
-        uint32_t handlerMicroseconds; // total time spent in the handlers in microseconds since the last telemetry message
-        uint32_t renderTime;         // total time spent rendering frames since the last telemetry message
-        const char* wifiInterfaceName; // the name of the wifi interface we are measuring RSSI on.
-        int32_t wifiRssi;            // if this device is communicating over wifi this is the signal strength.
+        const static int MessageLength = 8 * 4;
+        MavLinkTelemetry() { msgid = kMessageId; }
+        uint32_t messagesSent = 0;		  // number of messages sent since the last telemetry message
+        uint32_t messagesReceived = 0;	  // number of messages received since the last telemetry message
+        uint32_t messagesHandled = 0;	  // number of messages handled since the last telemetry message
+        uint32_t crcErrors = 0;			  // # crc errors detected in mavlink stream since the last telemetry message
+        uint32_t handlerMicroseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
+        uint32_t renderTime = 0;          // total time spent rendering frames since the last telemetry message
+        int32_t wifiRssi = 0;             // if this device is communicating over wifi this is the signal strength.
+        uint32_t udpateRateHz = 0;        // HIL_SENSOR update rate in hertz
+ 
+        // not serialized
+        const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on.
         virtual std::string toJSon() {
 
             std::ostringstream result;
@@ -152,6 +156,7 @@ namespace mavlinkcom
             result << "\"handlerMicroseconds\":" << this->handlerMicroseconds << ",";
             result << "\"renderTime\":" << this->renderTime;
             result << "\"wifiRssi\":" << this->wifiRssi;
+            result << "\"udpateRateHz\":" << this->udpateRateHz;
             result << "}";
             return result.str();
         }
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 5601d5e522..4f811bb4ac 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -242,7 +242,8 @@ int MavLinkTelemetry::pack(char* buffer) const {
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->handlerMicroseconds), 16);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->renderTime), 20);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->wifiRssi), 24);    
-    return 28;
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->udpateRateHz), 28);
+    return MavLinkTelemetry::MessageLength;
 }
 
 int MavLinkTelemetry::unpack(const char* buffer) {
@@ -253,7 +254,8 @@ int MavLinkTelemetry::unpack(const char* buffer) {
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->handlerMicroseconds), 16);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->renderTime), 20);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->wifiRssi), 24);
-    return 28;
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->udpateRateHz), 28);
+    return MavLinkTelemetry::MessageLength;
 }
 
 
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
index 1202aa1c40..e55f37e059 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
@@ -290,7 +290,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
         msglen = entry->min_msg_len;
     }
     if (msg.msgid == MavLinkTelemetry::kMessageId) {
-        msglen = 28; // mavlink doesn't know about our custom telemetry message.
+        msglen = MavLinkTelemetry::MessageLength; // mavlink doesn't know about our custom telemetry message.
     }
 
     if (len != msglen) {
@@ -602,6 +602,7 @@ void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result)
     telemetry_.messagesReceived = 0;
     telemetry_.messagesSent = 0;
     telemetry_.renderTime = 0;
+    telemetry_.udpateRateHz = 0;
     if (telemetry_.wifiInterfaceName != nullptr) {
         telemetry_.wifiRssi = port->getRssi(telemetry_.wifiInterfaceName);
     }

From 3b82fef109182a9e56da7d930be9be14b94292c7 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 15:54:25 -0700
Subject: [PATCH 06/37] fix build and update docs.

---
 .../UnitySensors/UnitySensorFactory.cpp       |  6 +-
 .../Source/UnitySensors/UnitySensorFactory.h  |  2 +-
 docs/px4_setup.md                             | 12 ++-
 docs/px4_sitl.md                              | 11 ++-
 docs/sensors.md                               | 96 ++++++++++++++-----
 5 files changed, 98 insertions(+), 29 deletions(-)

diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
index 85dea1a5e4..75ff5b739b 100644
--- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
+++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
@@ -12,7 +12,7 @@ void UnitySensorFactory::setActor(std::string vehicle_name, const NedTransform*
 	ned_transform_ = ned_transform;
 }
 
-std::unique_ptr<msr::airlib::SensorBase> UnitySensorFactory::createSensorFromSettings(const AirSimSettings::SensorSetting * sensor_setting) const
+std::shared_ptr<msr::airlib::SensorBase> UnitySensorFactory::createSensorFromSettings(const AirSimSettings::SensorSetting * sensor_setting) const
 {
 	
 	using SensorBase = msr::airlib::SensorBase;
@@ -20,10 +20,10 @@ std::unique_ptr<msr::airlib::SensorBase> UnitySensorFactory::createSensorFromSet
 	switch (sensor_setting->sensor_type)
 	{
 	case SensorBase::SensorType::Distance:
-		return std::unique_ptr<UnityDistanceSensor>(new UnityDistanceSensor(vehicle_name_, ned_transform_));
+		return std::make_shared<UnityDistanceSensor>(vehicle_name_, ned_transform_);
 	default:
 		return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting);
 	}
 
-	return std::unique_ptr<msr::airlib::SensorBase>();
+	return std::shared_ptr<msr::airlib::SensorBase>();
 }
diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
index 08600898e2..75565a7b3d 100644
--- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
+++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
@@ -10,7 +10,7 @@ class UnitySensorFactory : public SensorFactory
 public:
 	UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform);
 	void setActor(std::string vehicle_name, const NedTransform* ned_transform);
-	virtual std::unique_ptr<msr::airlib::SensorBase> createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override;
+	virtual std::shared_ptr<msr::airlib::SensorBase> createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override;
 
 private:
 	std::string vehicle_name_;
diff --git a/docs/px4_setup.md b/docs/px4_setup.md
index 359c75ca37..db48c736e5 100644
--- a/docs/px4_setup.md
+++ b/docs/px4_setup.md
@@ -39,7 +39,14 @@ See also [initial firmware setup video](https://dev.px4.io/starting-initial-conf
         "Vehicles": {
             "PX4": {
                 "VehicleType": "PX4Multirotor",
-                "UseSerial": true,
+                "UseSerial": true,,
+                "Sensors":{
+                    "Barometer":{
+                        "SensorType": 1,
+                        "Enabled": true,
+                        "PressureFactorSigma": 0.0001825
+                    }
+                },
                 "Parameters": {
                     "NAV_RCL_ACT": 0,
                     "NAV_DLL_ACT": 0,
@@ -52,6 +59,9 @@ See also [initial firmware setup video](https://dev.px4.io/starting-initial-conf
     }
 ```
 
+The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
+noise generation.  This setting clamps that down a bit.
+
 After above setup you should be able to use RC to fly with AirSim. You can usually arm the vehicle by lowering and bringing two sticks of RC together in-wards. You don't need QGroundControl after the initial setup. Typically the Stabilized (instead of Manual) mode gives better experience for beginners.  See [PX4 Basic Flying Guide](https://docs.px4.io/master/en/flying/basic_flying.html).
 
 You can also control the drone from [Python APIs](apis.md).
diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md
index 7a117aee93..4c82b0ce73 100644
--- a/docs/px4_sitl.md
+++ b/docs/px4_sitl.md
@@ -52,7 +52,14 @@ The default ports have changed recently, so check them closely to make sure AirS
                 "UseSerial": false,
                 "UseTcp": true,
                 "TcpPort": 4560,
-                "ControlPort": 14580,
+                "ControlPort": 14580,,
+                "Sensors":{
+                    "Barometer":{
+                        "SensorType": 1,
+                        "Enabled": true,
+                        "PressureFactorSigma": 0.0001825
+                    }
+                },
                 "Parameters": {
                     "NAV_RCL_ACT": 0,
                     "NAV_DLL_ACT": 0,
@@ -65,6 +72,8 @@ The default ports have changed recently, so check them closely to make sure AirS
     }
     ```
     Notice the PX4 `[simulator]` is using TCP, which is why we need to add: `"UseTcp": true,`.
+    The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
+    noise generation.  This setting clamps that down a bit.
 
 6. Now run your Unreal AirSim environment and it should connect to SITL PX4 via TCP.
 You should see a bunch of messages from the SITL PX4 window.
diff --git a/docs/sensors.md b/docs/sensors.md
index 5309399158..2ef22a605a 100644
--- a/docs/sensors.md
+++ b/docs/sensors.md
@@ -38,39 +38,84 @@ The default sensor list can be configured in settings json:
 ```json
 "DefaultSensors": {
     "Barometer": {
-         "SensorType": 1,
-         "Enabled" : true
+        "SensorType": 1,
+        "Enabled" : true,
+        "PressureFactorSigma": 0.001825,
+        "PressureFactorTau": 3600,
+        "UncorrelatedNoiseSigma": 2.7,
+        "UpdateLatency": 0,
+        "UpdateFrequency": 50,
+        "StartupDelay": 0
+
     },
     "Imu": {
-         "SensorType": 2,
-         "Enabled" : true
+        "SensorType": 2,
+        "Enabled" : true,
+        "AngularRandomWalk": 0.3,
+        "GyroBiasStabilityTau": 500,
+        "GyroBiasStability": 4.6,
+        "VelocityRandomWalk": 0.24,
+        "AccelBiasStabilityTau": 800,
+        "AccelBiasStability": 36
     },
     "Gps": {
-         "SensorType": 3,
-         "Enabled" : true
+        "SensorType": 3,
+        "Enabled" : true,
+        "EphTimeConstant": 0.9,
+        "EpvTimeConstant": 0.9,
+        "EphInitial": 25,
+        "EpvInitial": 25,
+        "EphFinal": 0.1,
+        "EpvFinal": 0.1,
+        "EphMin3d": 3,
+        "EphMin2d": 4,
+        "UpdateLatency": 0.2,
+        "UpdateFrequency": 50,
+        "StartupDelay": 1
     },
     "Magnetometer": {
-         "SensorType": 4,
-         "Enabled" : true
+        "SensorType": 4,
+        "Enabled" : true,
+        "NoiseSigma": 0.005,
+        "ScaleFactor": 1,
+        "NoiseBias": 0,
+        "UpdateLatency": 0,
+        "UpdateFrequency": 50,
+        "StartupDelay": 0
     },
     "Distance": {
-         "SensorType": 5,
-         "Enabled" : true
+        "SensorType": 5,
+        "Enabled" : true,
+        "MinDistance": 0.2,
+        "MaxDistance": 40,
+        "X": 0, "Y": 0, "Z": -1,
+        "Yaw": 0, "Pitch": 0, "Roll": 0,
+        "DrawDebugPoints": false
     },
     "Lidar2": {
-         "SensorType": 6,
-         "Enabled" : true,
-         "NumberOfChannels": 4,
-         "PointsPerSecond": 10000
+        "SensorType": 6,
+        "Enabled" : true,
+        "NumberOfChannels": 16,
+        "RotationsPerSecond": 10,
+        "PointsPerSecond": 100000,
+        "X": 0, "Y": 0, "Z": -1,
+        "Roll": 0, "Pitch": 0, "Yaw" : 0,
+        "VerticalFOVUpper": -15,
+        "VerticalFOVLower": -25,
+        "HorizontalFOVStart": -20,
+        "HorizontalFOVEnd": 20,
+        "DrawDebugPoints": true,
+        "DataFrame": "SensorLocalFrame"
     }
 },
 ```
 
 ## Configuring vehicle-specific sensor list
 
-If a vehicle provides its sensor list, it **must** provide the whole list. Selective add/remove/update of the default sensor list is **NOT** supported.
-A vehicle specific sensor list can be specified in the vehicle settings part of the json.
-e.g.,
+A vehicle can override a subset of the default sensors listed above. A Lidar and Distance sensor are
+not added to a vehicle by default, so those you need to add this way. Each sensor must have a valid
+"SensorType" and a subset of the properties can be defined that override the default values shown
+above and you can set Enabled to false to disable a specific type of sensor.
 
 ```json
 "Vehicles": {
@@ -80,6 +125,11 @@ e.g.,
         "AutoCreate": true,
         ...
         "Sensors": {
+            "Barometer":{
+                "SensorType": 1,
+                "Enabled": true,
+                "PressureFactorSigma": 0.0001825
+            },
             "MyLidar1": {
                 "SensorType": 6,
                 "Enabled" : true,
@@ -146,7 +196,7 @@ Be default, the points hit by distance sensor are not drawn on the viewport. To
 ## Sensor APIs
 Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py) or [`hello_drone.cpp`](https://github.com/Microsoft/AirSim/blob/master/HelloDrone/main.cpp) for example usage, or see follow below for the full API.
 
-##### Barometer
+### Barometer
 ```cpp
 msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name, const std::string& vehicle_name);
 ```
@@ -155,7 +205,7 @@ msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer
 barometer_data = client.getBarometerData(barometer_name = "", vehicle_name = "")
 ```
 
-##### IMU
+### IMU
 ```cpp
 msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = "");
 ```
@@ -164,7 +214,7 @@ msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const
 imu_data = client.getImuData(imu_name = "", vehicle_name = "")
 ```
 
-##### GPS
+### GPS
 ```cpp
 msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "");
 ```
@@ -172,7 +222,7 @@ msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const
 gps_data = client.getGpsData(gps_name = "", vehicle_name = "")
 ```
 
-##### Magnetometer
+### Magnetometer
 ```cpp
 msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "");
 ```
@@ -180,7 +230,7 @@ msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& mag
 magnetometer_data = client.getMagnetometerData(magnetometer_name = "", vehicle_name = "")
 ```
 
-##### Distance sensor
+### Distance sensor
 ```cpp
 msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "");
 ```
@@ -188,5 +238,5 @@ msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distanc
 distance_sensor_data = client.getDistanceSensorData(distance_sensor_name = "", vehicle_name = "")
 ```
 
-##### Lidar
+### Lidar
 See the [lidar page](lidar.md) for Lidar API.

From 30ab123d36818e3402e66504228865aba5e8bf66 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 15:56:18 -0700
Subject: [PATCH 07/37] fix unreal build

---
 .../AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp     | 6 +++---
 .../AirSim/Source/UnrealSensors/UnrealSensorFactory.h       | 2 +-
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp
index 4686633210..ed87adfdd3 100644
--- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp
+++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.cpp
@@ -9,17 +9,17 @@ UnrealSensorFactory::UnrealSensorFactory(AActor* actor, const NedTransform* ned_
     setActor(actor, ned_transform);
 }
 
-std::unique_ptr<msr::airlib::SensorBase> UnrealSensorFactory::createSensorFromSettings(
+std::shared_ptr<msr::airlib::SensorBase> UnrealSensorFactory::createSensorFromSettings(
     const AirSimSettings::SensorSetting* sensor_setting) const
 {
     using SensorBase = msr::airlib::SensorBase;
 
     switch (sensor_setting->sensor_type) {
     case SensorBase::SensorType::Distance:
-        return std::unique_ptr<UnrealDistanceSensor>(new UnrealDistanceSensor(
+        return std::shared_ptr<UnrealDistanceSensor>(new UnrealDistanceSensor(
             *static_cast<const AirSimSettings::DistanceSetting*>(sensor_setting), actor_, ned_transform_));
     case SensorBase::SensorType::Lidar:
-        return std::unique_ptr<UnrealLidarSensor>(new UnrealLidarSensor(
+        return std::shared_ptr<UnrealLidarSensor>(new UnrealLidarSensor(
             *static_cast<const AirSimSettings::LidarSetting*>(sensor_setting), actor_, ned_transform_));
     default:
         return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting);
diff --git a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h
index 7034679a76..c49ba7da17 100644
--- a/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h
+++ b/Unreal/Plugins/AirSim/Source/UnrealSensors/UnrealSensorFactory.h
@@ -17,7 +17,7 @@ class UnrealSensorFactory : public msr::airlib::SensorFactory {
     UnrealSensorFactory(AActor* actor, const NedTransform* ned_transform);
     virtual ~UnrealSensorFactory() {}
     void setActor(AActor* actor, const NedTransform* ned_transform);
-    virtual std::unique_ptr<msr::airlib::SensorBase> createSensorFromSettings(
+    virtual std::shared_ptr<msr::airlib::SensorBase> createSensorFromSettings(
         const AirSimSettings::SensorSetting* sensor_setting) const override;
 
 private:

From b0b5de38fcbc0b46b63e4a56e5175744ec09b410 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 18:44:04 -0700
Subject: [PATCH 08/37] compute checksum on telemetry messages.

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 32 +++++----
 MavLinkCom/include/MavLinkMessageBase.hpp     |  3 +
 MavLinkCom/src/MavLinkLog.cpp                 | 11 ++-
 MavLinkCom/src/MavLinkMessageBase.cpp         | 70 +++++++++++++++++++
 MavLinkCom/src/impl/MavLinkConnectionImpl.cpp | 14 ++--
 MavLinkCom/src/impl/MavLinkConnectionImpl.hpp |  1 -
 6 files changed, 106 insertions(+), 25 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index b9961cd10e..bee2d69d38 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -120,6 +120,19 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 return;
 
             auto now = clock()->nowNanos() / 1000;
+            if (lock_step_enabled_) {
+                if (last_hil_sensor_time_ + 100000 < now) {
+                    // if 100 ms passes then something is terribly wrong, reset lockstep mode
+                    lock_step_enabled_ = false;
+                    addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
+                }
+
+                if (!received_actuator_controls_) {
+                    // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
+                    return;
+                }
+            }
+
             if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now)
             {
                 // then update() is being called too often, so we just skip this one.
@@ -127,6 +140,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 return;
             }
 
+            last_hil_sensor_time_ = now;
+
             update_count_++;
             if (last_fps_time_ == 0 || last_fps_time_ + 1000000 < now) {
                 // compute update rate once per second.
@@ -136,21 +151,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 sendTelemetry(1);
             }
 
-            if (lock_step_enabled_) {
-                if (last_hil_sensor_time_ + 100000 < now) {
-                    // if 100 ms passes then something is terribly wrong, reset lockstep mode
-                    lock_step_enabled_ = false;
-                    addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
-                }
-
-                if (!received_actuator_controls_) {
-                    // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
-                    return;
-                }
-            }
-
-            last_hil_sensor_time_ = now;
-
             advanceTime();
 
             //send sensor updates
@@ -616,7 +616,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
 
         if (log_ != nullptr) {
             mavlinkcom::MavLinkMessage msg;
+            msg.magic = MAVLINK_STX_MAVLINK1;
             data.encode(msg);
+            msg.update_checksum();
             log_->write(msg);
         }
     }
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index b49e7d9ef1..3f55ddd35e 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -41,6 +41,9 @@ namespace mavlinkcom
         uint8_t ck[2];          ///< incoming checksum bytes
         uint8_t signature[13];
         uint8_t protocol_version;
+
+        // initialize checksum
+        int update_checksum();
     };
 
     // This is the base class for all the strongly typed messages define in MavLinkMessages.hpp
diff --git a/MavLinkCom/src/MavLinkLog.cpp b/MavLinkCom/src/MavLinkLog.cpp
index 9c8fb97c12..b8824eeabd 100644
--- a/MavLinkCom/src/MavLinkLog.cpp
+++ b/MavLinkCom/src/MavLinkLog.cpp
@@ -9,6 +9,7 @@ using namespace mavlinkcom;
 using namespace mavlink_utils;
 
 #define MAVLINK_STX_MAVLINK1 0xFE          // marker for old protocol
+#define MAVLINK_STX 253					// marker for mavlink 2
 
 uint64_t MavLinkFileLog::getTimeStamp()
 {
@@ -118,15 +119,21 @@ void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t times
             // todo: mavlink2 support?
             timestamp = FlipEndianness(timestamp);
 
+			uint8_t magic = msg.magic;
+			if (magic != MAVLINK_STX_MAVLINK1) {
+				// has to be one or the other!
+				magic = MAVLINK_STX;
+			}
+
 			std::lock_guard<std::mutex> lock(log_lock_);
 			fwrite(&timestamp, sizeof(uint64_t), 1, ptr_);
-			fwrite(&msg.magic, 1, 1, ptr_);
+			fwrite(&magic, 1, 1, ptr_);
 			fwrite(&msg.len, 1, 1, ptr_);
 			fwrite(&msg.seq, 1, 1, ptr_);
 			fwrite(&msg.sysid, 1, 1, ptr_);
 			fwrite(&msg.compid, 1, 1, ptr_);
 
-			if (msg.magic == MAVLINK_STX_MAVLINK1) {
+			if (magic == MAVLINK_STX_MAVLINK1) {
 				uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid
 				fwrite(&msgid, 1, 1, ptr_);
 			}
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 4f811bb4ac..89df650fcc 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -20,6 +20,76 @@ STRICT_MODE_ON
 using namespace mavlink_utils;
 using namespace mavlinkcom;
 
+int MavLinkMessage::update_checksum()
+{
+    bool mavlink1 = protocol_version != 2;    
+    uint8_t header_len = MAVLINK_CORE_HEADER_LEN + 1;
+    uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1];
+    if (mavlink1) {
+        magic = MAVLINK_STX_MAVLINK1;
+        header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
+    }
+    else {
+        magic = MAVLINK_STX;
+    }
+
+    incompat_flags = 0;
+    compat_flags = 0;
+
+    // pack the payload buffer.
+    char* payload = reinterpret_cast<char*>(&payload64[0]);
+    int len = this->len;
+
+    // calculate checksum
+    const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msgid);
+    uint8_t crc_extra = 0;
+    int msglen = 0;
+    if (entry != nullptr) {
+        crc_extra = entry->crc_extra;
+        msglen = entry->min_msg_len;
+    }
+    if (msgid == MavLinkTelemetry::kMessageId) {
+        msglen = MavLinkTelemetry::MessageLength; // mavlink doesn't know about our custom telemetry message.
+    }
+
+    if (len != msglen) {
+        if (mavlink1) {
+            throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen));
+        }
+        else {
+            // mavlink2 supports trimming the payload of trailing zeros so the messages
+            // are variable length as a result.
+        }
+    }
+    len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen);
+    this->len = len;
+
+    // form the header as a byte array for the crc
+    buf[0] = this->magic;
+    buf[1] = this->len;
+    if (mavlink1) {
+        buf[2] = this->seq;
+        buf[3] = this->sysid;
+        buf[4] = this->compid;
+        buf[5] = this->msgid & 0xFF;
+    }
+    else {
+        buf[2] = this->incompat_flags;
+        buf[3] = this->compat_flags;
+        buf[4] = this->seq;
+        buf[5] = this->sysid;
+        buf[6] = this->compid;
+        buf[7] = this->msgid & 0xFF;
+        buf[8] = (this->msgid >> 8) & 0xFF;
+        buf[9] = (this->msgid >> 16) & 0xFF;
+    }
+
+    this->checksum = crc_calculate(&buf[1], header_len - 1);
+    crc_accumulate_buffer(&this->checksum, payload, len);
+    crc_accumulate(crc_extra, &this->checksum);
+
+    return len + header_len + 2;
+}
 void MavLinkMessageBase::decode(const MavLinkMessage& msg)
 {
     // unpack the message...
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
index e55f37e059..ffa94d6879 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
@@ -272,7 +272,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
 
     msg.seq = seqno;
     msg.incompat_flags = 0;
-    if (signing_) {
+    if (signing) {
         msg.incompat_flags |= MAVLINK_IFLAG_SIGNED;
     }
     msg.compat_flags = 0;
@@ -301,7 +301,7 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
             // mavlink2 supports trimming the payload of trailing zeros so the messages
             // are variable length as a result.
         }
-    }    
+    }
     len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen);
     msg.len = len;
 
@@ -335,12 +335,12 @@ int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
     mavlink_ck_b(&msg) = (uint8_t)(msg.checksum >> 8);
     STRICT_MODE_ON
 
-    if (signing_) {
+    if (signing) {
         mavlink_sign_packet(mavlink_status_.signing,
-            reinterpret_cast<uint8_t *>(msg.signature),
-            reinterpret_cast<const uint8_t *>(message_buf), header_len,
-            reinterpret_cast<const uint8_t *>(payload), msg.len,
-            reinterpret_cast<const uint8_t *>(payload) + msg.len);
+            reinterpret_cast<uint8_t*>(msg.signature),
+            reinterpret_cast<const uint8_t*>(message_buf), header_len,
+            reinterpret_cast<const uint8_t*>(payload), msg.len,
+            reinterpret_cast<const uint8_t*>(payload) + msg.len);
     }
 
     return msg.len + header_len + 2 + signature_len;
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
index 3256ae41d6..2a63f190cb 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.hpp
@@ -97,7 +97,6 @@ namespace mavlinkcom_impl {
         bool waiting_for_msg_ = false;
         bool supports_mavlink2_ = false;
         std::thread::id publish_thread_id_;
-        bool signing_ = false;
         mavlink_status_t mavlink_intermediate_status_;
         mavlink_status_t mavlink_status_;
         std::mutex telemetry_mutex_;

From 71ff20619be577fa244834e59b09521c212cb260 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Fri, 16 Apr 2021 19:01:25 -0700
Subject: [PATCH 09/37] fix compiler warning.

---
 Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp
index 164778352f..c715090080 100644
--- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp
+++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp
@@ -180,8 +180,8 @@ bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, c
         for (float k = 0; k < ncells_z; k++) {
             for (float j = 0; j < ncells_y; j++) {
                 int idx = i + ncells_x * (k + ncells_z * j);
-                FVector position = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame;
-                voxel_grid_[idx] = simmode_->GetWorld()->OverlapBlockingTestByChannel(position, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params);
+                FVector vposition = FVector((i - ncells_x /2) * scale_cm, (j - ncells_y /2) * scale_cm, (k - ncells_z /2) * scale_cm) + position_in_UE_frame;
+                voxel_grid_[idx] = simmode_->GetWorld()->OverlapBlockingTestByChannel(vposition, FQuat::Identity, ECollisionChannel::ECC_Pawn, FCollisionShape::MakeBox(FVector(scale_cm /2)), params);
             }
         }
     }

From 24d1bb6880d17c4751db846ffe77dc5c02ce0b15 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sat, 17 Apr 2021 19:39:29 -0700
Subject: [PATCH 10/37] Move telemetry to separate thread to get it out of the
 update() loop.  Rename the telemetry message fields to follow consistent
 naming scheme.

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 1030 +++++++++--------
 MavLinkCom/include/MavLinkMessageBase.hpp     |   40 +-
 MavLinkCom/src/MavLinkMessageBase.cpp         |   38 +-
 MavLinkCom/src/impl/MavLinkConnectionImpl.cpp |   37 +-
 4 files changed, 606 insertions(+), 539 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index bee2d69d38..29a77e8f88 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -39,587 +39,621 @@
 
 namespace msr { namespace airlib {
 
-class MavLinkMultirotorApi : public MultirotorApiBase
-{
-public: //methods
-    virtual ~MavLinkMultirotorApi()
+    class MavLinkMultirotorApi : public MultirotorApiBase
     {
-        closeAllConnection();
-        if (this->connect_thread_.joinable())
+    public: //methods
+        virtual ~MavLinkMultirotorApi()
         {
-            this->connect_thread_.join();
+            closeAllConnection();
+            if (this->connect_thread_.joinable())
+            {
+                this->connect_thread_.join();
+            }
+            if (this->telemetry_thread_.joinable()) {
+                this->telemetry_thread_.join();
+            }
         }
-    }
 
-    //non-base interface specific to MavLinKDroneController
-    void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
-    {
-        connection_info_ = connection_info;
-        sensors_ = sensors;
-        is_simulation_mode_ = is_simulation;
+        //non-base interface specific to MavLinKDroneController
+        void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
+        {
+            connection_info_ = connection_info;
+            sensors_ = sensors;
+            is_simulation_mode_ = is_simulation;
 
-        try {
-            openAllConnections();
-            is_ready_ = true;
-        }
-        catch (std::exception& ex) {
-            is_ready_ = false;
-            is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
+            try {
+                openAllConnections();
+                is_ready_ = true;
+            }
+            catch (std::exception& ex) {
+                is_ready_ = false;
+                is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
+            }
         }
-    }
 
-    Pose getMocapPose()
-    {
-        std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
-        return mocap_pose_;
-    }
+        Pose getMocapPose()
+        {
+            std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
+            return mocap_pose_;
+        }
 
-    virtual const SensorCollection& getSensors() const override
-    {
-        return *sensors_;
-    }
+        virtual const SensorCollection& getSensors() const override
+        {
+            return *sensors_;
+        }
 
-    //reset PX4 stack
-    virtual void resetImplementation() override
-    {
-        // note this is called AFTER "initialize" when we've connected to the drone
-        // so this method cannot reset any of that connection state.
-        MultirotorApiBase::resetImplementation();
-        was_reset_ = true;
-    }
+        //reset PX4 stack
+        virtual void resetImplementation() override
+        {
+            // note this is called AFTER "initialize" when we've connected to the drone
+            // so this method cannot reset any of that connection state.
+            MultirotorApiBase::resetImplementation();
+            was_reset_ = true;
+        }
 
-    unsigned long long getSimTime() {
-        // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
-        if (lock_step_enabled_) {
-            if (sim_time_us_ == 0) {
-                sim_time_us_ = clock()->nowNanos() / 1000;
+        unsigned long long getSimTime() {
+            // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
+            if (lock_step_enabled_) {
+                if (sim_time_us_ == 0) {
+                    sim_time_us_ = clock()->nowNanos() / 1000;
+                }
+                return sim_time_us_;
+            }
+            else {
+                return clock()->nowNanos() / 1000;
             }
-            return sim_time_us_;
-        }
-        else {
-            return clock()->nowNanos() / 1000;
         }
-    }
 
-    void advanceTime() {
-        if (lock_step_enabled_) {
-            sim_time_us_ += ticks_per_update_;
-        }
-        else {
+        void advanceTime() {
             sim_time_us_ = clock()->nowNanos() / 1000;
         }
-    }
 
-    //update sensors in PX4 stack
-    virtual void update() override
-    {
-        try {
-            MultirotorApiBase::update();
+        //update sensors in PX4 stack
+        virtual void update() override
+        {
+            try {
+                MultirotorApiBase::update();
 
-            if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
-                return;
+                if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
+                    return;
 
-            auto now = clock()->nowNanos() / 1000;
-            if (lock_step_enabled_) {
-                if (last_hil_sensor_time_ + 100000 < now) {
-                    // if 100 ms passes then something is terribly wrong, reset lockstep mode
-                    lock_step_enabled_ = false;
-                    addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
-                }
+                update_count_++;
 
-                if (!received_actuator_controls_) {
-                    // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
-                    return;
+                auto now = clock()->nowNanos() / 1000;
+                if (lock_step_enabled_) {
+                    if (last_update_time_ + 100000 < now) {
+                        // if 100 ms passes then something is terribly wrong, reset lockstep mode
+                        lock_step_enabled_ = false;
+                        lock_step_resets_++;
+                        addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
+                    }
+
+                    if (!received_actuator_controls_) {
+                        // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
+                        return;
+                    }
                 }
-            }
 
-            if (last_hil_sensor_time_ != 0 && last_hil_sensor_time_ + ticks_per_update_ > now)
-            {
-                // then update() is being called too often, so we just skip this one.
-                // TODO: I think this needs to be aware of AirSim ClockSpeed...but perhasp clock() has already taken that into account?
-                return;
-            }
+                last_update_time_ = now;
+                hil_sensor_count_++;
+                advanceTime();
 
-            last_hil_sensor_time_ = now;
+                //send sensor updates
+                const auto& imu_output = getImuData("");
+                const auto& mag_output = getMagnetometerData("");
+                const auto& baro_output = getBarometerData("");
 
-            update_count_++;
-            if (last_fps_time_ == 0 || last_fps_time_ + 1000000 < now) {
-                // compute update rate once per second.
-                update_rate_ = update_count_;
-                update_count_ = 0;
-                last_fps_time_ = now;
-                sendTelemetry(1);
-            }
+                sendHILSensor(imu_output.linear_acceleration,
+                    imu_output.angular_velocity,
+                    mag_output.magnetic_field_body,
+                    baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
 
-            advanceTime();
+                sendSystemTime();
 
-            //send sensor updates
-            const auto& imu_output = getImuData("");
-            const auto& mag_output = getMagnetometerData("");
-            const auto& baro_output = getBarometerData("");
+                const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
+                if (count_distance_sensors != 0) {
+                    const auto& distance_output = getDistanceSensorData("");
 
-            sendHILSensor(imu_output.linear_acceleration,
-                imu_output.angular_velocity,
-                mag_output.magnetic_field_body,
-                baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
-            
-            sendSystemTime();
-
-            const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
-            if (count_distance_sensors != 0) {
-                const auto& distance_output = getDistanceSensorData("");
-
-                sendDistanceSensor(distance_output.min_distance * 100, //m -> cm
-                    distance_output.max_distance * 100, //m -> cm
-                    distance_output.distance * 100, //m-> cm
-                    0, //sensor type: //TODO: allow changing in settings?
-                    77, //sensor id, //TODO: should this be something real?
-                    distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
-            }
-
-            const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
-            if (count_gps_sensors != 0) {
-                const auto& gps_output = getGpsData("");
-
-                //send GPS
-                if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
-                    last_gps_time_ = gps_output.gnss.time_utc;
-                    Vector3r gps_velocity = gps_output.gnss.velocity;
-                    Vector3r gps_velocity_xy = gps_velocity;
-                    gps_velocity_xy.z() = 0;
-                    float gps_cog;
-                    if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
-                        gps_cog = 0;
-                    else
-                        gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
-                    if (gps_cog < 0)
-                        gps_cog += 360;
-
-                    sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
-                        gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
+                    sendDistanceSensor(distance_output.min_distance * 100, //m -> cm
+                        distance_output.max_distance * 100, //m -> cm
+                        distance_output.distance * 100, //m-> cm
+                        0, //sensor type: //TODO: allow changing in settings?
+                        77, //sensor id, //TODO: should this be something real?
+                        distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
                 }
-            }            
 
-        }
-        catch (std::exception& e) {
-            addStatusMessage("Exception sending messages to vehicle");
-            addStatusMessage(e.what());
-            disconnect();
-            connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
-        }
+                const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
+                if (count_gps_sensors != 0) {
+                    const auto& gps_output = getGpsData("");
+
+                    //send GPS
+                    if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
+                        last_gps_time_ = gps_output.gnss.time_utc;
+                        Vector3r gps_velocity = gps_output.gnss.velocity;
+                        Vector3r gps_velocity_xy = gps_velocity;
+                        gps_velocity_xy.z() = 0;
+                        float gps_cog;
+                        if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
+                            gps_cog = 0;
+                        else
+                            gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
+                        if (gps_cog < 0)
+                            gps_cog += 360;
+
+                        sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
+                            gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
+                    }
+                }
 
-        //must be done at the end
-        if (was_reset_)
-            was_reset_ = false;
-    }
+            }
+            catch (std::exception& e) {
+                addStatusMessage("Exception sending messages to vehicle");
+                addStatusMessage(e.what());
+                disconnect();
+                connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
+            }
 
-    virtual bool isReady(std::string& message) const override
-    {
-        if (!is_ready_ && is_ready_message_.size() > 0) {
-            message = is_ready_message_;
+            //must be done at the end
+            if (was_reset_)
+                was_reset_ = false;
         }
-        return is_ready_;
-    }
 
-    virtual bool canArm() const override
-    {
-        return is_ready_ && has_gps_lock_;
-    }
+        virtual bool isReady(std::string& message) const override
+        {
+            if (!is_ready_ && is_ready_message_.size() > 0) {
+                message = is_ready_message_;
+            }
+            return is_ready_;
+        }
 
-    //TODO: this method can't be const yet because it clears previous messages
-    virtual void getStatusMessages(std::vector<std::string>& messages) override
-    {
-        updateState();
+        virtual bool canArm() const override
+        {
+            return is_ready_ && has_gps_lock_;
+        }
 
-        //clear param
-        messages.clear();
+        //TODO: this method can't be const yet because it clears previous messages
+        virtual void getStatusMessages(std::vector<std::string>& messages) override
+        {
+            updateState();
+
+            //clear param
+            messages.clear();
 
-        //move messages from private vars to param
-        std::lock_guard<std::mutex> guard(status_text_mutex_);
-        while (!status_messages_.empty()) {
-            messages.push_back(status_messages_.front());
-            status_messages_.pop();
+            //move messages from private vars to param
+            std::lock_guard<std::mutex> guard(status_text_mutex_);
+            while (!status_messages_.empty()) {
+                messages.push_back(status_messages_.front());
+                status_messages_.pop();
+            }
         }
-    }
 
-    virtual Kinematics::State getKinematicsEstimated() const override
-    {
-        updateState();
-        Kinematics::State state;
-        //TODO: reduce code duplication in getPosition() etc methods?
-        state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
-        state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
-        state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
-        state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate);
-        state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z);
-        //TODO: how do we get angular acceleration?
-        return state;
-    }
+        virtual Kinematics::State getKinematicsEstimated() const override
+        {
+            updateState();
+            Kinematics::State state;
+            //TODO: reduce code duplication in getPosition() etc methods?
+            state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
+            state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
+            state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
+            state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate);
+            state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z);
+            //TODO: how do we get angular acceleration?
+            return state;
+        }
+
+        virtual bool isApiControlEnabled() const override
+        {
+            return is_api_control_enabled_;
+        }
 
-    virtual bool isApiControlEnabled() const override
-    {
-        return is_api_control_enabled_;
-    }
+        virtual void enableApiControl(bool is_enabled) override
+        {
+            checkValidVehicle();
+            if (is_enabled) {
+                mav_vehicle_->requestControl();
+                is_api_control_enabled_ = true;
+            }
+            else {
+                mav_vehicle_->releaseControl();
+                is_api_control_enabled_ = false;
+            }
+        }
 
-    virtual void enableApiControl(bool is_enabled) override
-    {
-        checkValidVehicle();
-        if (is_enabled) {
-            mav_vehicle_->requestControl();
-            is_api_control_enabled_ = true;
+        virtual Vector3r getPosition() const override
+        {
+            updateState();
+            return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
         }
-        else {
-            mav_vehicle_->releaseControl();
-            is_api_control_enabled_ = false;
+        virtual Vector3r getVelocity() const override
+        {
+            updateState();
+            return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
         }
-    }
 
-    virtual Vector3r getPosition() const override
-    {
-        updateState();
-        return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
-    }
-    virtual Vector3r getVelocity() const override
-    {
-        updateState();
-        return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
-    }
+        virtual Quaternionr getOrientation() const override
+        {
+            updateState();
+            return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
+        }
 
-    virtual Quaternionr getOrientation() const override
-    {
-        updateState();
-        return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
-    }
+        virtual LandedState getLandedState() const override
+        {
+            updateState();
+            return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
+        }
 
-    virtual LandedState getLandedState() const override
-    {
-        updateState();
-        return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
-    }
+        virtual real_T getActuation(unsigned int rotor_index) const override
+        {
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to read motor controls while not in simulation mode");
 
-    virtual real_T getActuation(unsigned int rotor_index) const override
-    {
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to read motor controls while not in simulation mode");
+            std::lock_guard<std::mutex> guard(hil_controls_mutex_);
+            return rotor_controls_[rotor_index];
+        }
+        virtual size_t getActuatorCount() const override
+        {
+            return RotorControlsCount;
+        }
 
-        std::lock_guard<std::mutex> guard(hil_controls_mutex_);
-        return rotor_controls_[rotor_index];
-    }
-    virtual size_t getActuatorCount() const override
-    {
-        return RotorControlsCount;
-    }
+        virtual bool armDisarm(bool arm) override
+        {
+            SingleCall lock(this);
 
-    virtual bool armDisarm(bool arm) override
-    {
-        SingleCall lock(this);
+            checkValidVehicle();
+            bool rc = false;
+            if (arm) {
+                float timeout_sec = 10;
+                waitForHomeLocation(timeout_sec);
+                waitForStableGroundPosition(timeout_sec);
+            }
 
-        checkValidVehicle();
-        bool rc = false;
-        if (arm) {
-            float timeout_sec = 10;
-            waitForHomeLocation(timeout_sec);
-            waitForStableGroundPosition(timeout_sec);
+            mav_vehicle_->armDisarm(arm).wait(10000, &rc);
+            return rc;
         }
 
-        mav_vehicle_->armDisarm(arm).wait(10000, &rc);
-        return rc;
-    }
-
-    void onArmed() {
-        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
-            auto con = mav_vehicle_->getConnection();
-            if (con != nullptr) {
-                if (log_ != nullptr) {
-                    log_->close();
-                }
+        void onArmed() {
+            if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+                auto con = mav_vehicle_->getConnection();
+                if (con != nullptr) {
+                    if (log_ != nullptr) {
+                        log_->close();
+                        log_ = nullptr;
+                    }
 
-                std::time_t t = std::time(0);   // get time now
-                std::tm* now = std::localtime(&t);
-                auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
-                auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder);
-                auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec);
-                auto fullpath = common_utils::FileSystem::combine(path, filename);
-                addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str()));
-                log_ = std::make_shared<mavlinkcom::MavLinkFileLog>();
-                log_->openForWriting(fullpath, false);
-                con->startLoggingSendMessage(log_);
-                con->startLoggingReceiveMessage(log_);
-                if (con != connection_) {
-                    // log the SITL channel also
-                    connection_->startLoggingSendMessage(log_);
-                    connection_->startLoggingReceiveMessage(log_);
+                    try
+                    {
+                        std::time_t t = std::time(0);   // get time now
+                        std::tm* now = std::localtime(&t);
+                        auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
+                        auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder);
+                        auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec);
+                        auto fullpath = common_utils::FileSystem::combine(path, filename);
+                        addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str()));
+                        log_file_name_ = fullpath;
+                        log_ = std::make_shared<mavlinkcom::MavLinkFileLog>();
+                        log_->openForWriting(fullpath, false);
+                        con->startLoggingSendMessage(log_);
+                        con->startLoggingReceiveMessage(log_);
+                        if (con != connection_) {
+                            // log the SITL channel also
+                            connection_->startLoggingSendMessage(log_);
+                            connection_->startLoggingReceiveMessage(log_);
+                        }
+                        start_telemtry_thread();
+                    }
+                    catch (std::exception& ex) {
+                        addStatusMessage(std::string("Opening log file failed: ") + ex.what());
+                    }
                 }
             }
         }
-    }
 
-    void onDisarmed() {
-        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
-            auto con = mav_vehicle_->getConnection();
-            if (con != nullptr) {
-                con->stopLoggingSendMessage();
-                addStatusMessage("Closing log file.");
+        void onDisarmed() {
+            if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+                auto con = mav_vehicle_->getConnection();
+                if (con != nullptr) {
+                    con->stopLoggingSendMessage();
+                    addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str()));
+                }
+                connection_->stopLoggingSendMessage();
             }
-            connection_->stopLoggingSendMessage();
             if (log_ != nullptr) {
                 log_->close();
+                log_ = nullptr;
             }
         }
-    }
 
-    void waitForHomeLocation(float timeout_sec)
-    {
-        if (!current_state_.home.is_set) {
-            addStatusMessage("Waiting for valid GPS home location...");
-            if (!waitForFunction([&]() {
-                return current_state_.home.is_set;
-                }, timeout_sec).isComplete()) {
-                throw VehicleMoveException("Vehicle does not have a valid GPS home location");
+        void waitForHomeLocation(float timeout_sec)
+        {
+            if (!current_state_.home.is_set) {
+                addStatusMessage("Waiting for valid GPS home location...");
+                if (!waitForFunction([&]() {
+                    return current_state_.home.is_set;
+                    }, timeout_sec).isComplete()) {
+                    throw VehicleMoveException("Vehicle does not have a valid GPS home location");
+                }
             }
         }
-    }
 
-    void waitForStableGroundPosition(float timeout_sec)
-    {
-        // wait for ground stabilization
-        if (ground_variance_ > GroundTolerance) {
-            addStatusMessage("Waiting for z-position to stabilize...");
-            if (!waitForFunction([&]() {
-                return ground_variance_ <= GroundTolerance;
-                }, timeout_sec).isComplete())
-            {
-                auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
-                throw VehicleMoveException(msg);
+        void waitForStableGroundPosition(float timeout_sec)
+        {
+            // wait for ground stabilization
+            if (ground_variance_ > GroundTolerance) {
+                addStatusMessage("Waiting for z-position to stabilize...");
+                if (!waitForFunction([&]() {
+                    return ground_variance_ <= GroundTolerance;
+                    }, timeout_sec).isComplete())
+                {
+                    auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
+                    throw VehicleMoveException(msg);
+                }
             }
         }
-    }
-
-    virtual bool takeoff(float timeout_sec) override
-    {
-        SingleCall lock(this);
 
-        checkValidVehicle();
-
-        waitForHomeLocation(timeout_sec);
-        waitForStableGroundPosition(timeout_sec);
-
-        bool rc = false;
-        auto vec = getPosition();
-        auto yaw = current_state_.attitude.yaw;
-        float z = vec.z() + getTakeoffZ();
-        if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
-            throw VehicleMoveException("TakeOff command - timeout waiting for response");
-        }
-        if (!rc) {
-            throw VehicleMoveException("TakeOff command rejected by drone");
-        }
-        if (timeout_sec <= 0)
-            return true; // client doesn't want to wait.
+        virtual bool takeoff(float timeout_sec) override
+        {
+            SingleCall lock(this);
 
-        return waitForZ(timeout_sec, z, getDistanceAccuracy());
-    }
+            checkValidVehicle();
 
-    virtual bool land(float timeout_sec) override
-    {
-        SingleCall lock(this);
+            waitForHomeLocation(timeout_sec);
+            waitForStableGroundPosition(timeout_sec);
 
-        //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
-        //we assume the ground is relatively flat an we are landing roughly at the home altitude.
-        updateState();
-        checkValidVehicle();
-        if (current_state_.home.is_set) {
             bool rc = false;
-            if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc))
-            {
-                throw VehicleMoveException("Landing command - timeout waiting for response from drone");
+            auto vec = getPosition();
+            auto yaw = current_state_.attitude.yaw;
+            float z = vec.z() + getTakeoffZ();
+            if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
+                throw VehicleMoveException("TakeOff command - timeout waiting for response");
             }
-            else if (!rc) {
-                throw VehicleMoveException("Landing command rejected by drone");
+            if (!rc) {
+                throw VehicleMoveException("TakeOff command rejected by drone");
             }
-        }
-        else {
-            throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude.  Could fix this if we hook up a distance to ground sensor...");
-        }
+            if (timeout_sec <= 0)
+                return true; // client doesn't want to wait.
 
-        const auto& waiter = waitForFunction([&]() {
-            updateState();
-            return current_state_.controls.landed;
-            }, timeout_sec);
+            return waitForZ(timeout_sec, z, getDistanceAccuracy());
+        }
 
-        // Wait for landed state (or user cancellation)
-        if (!waiter.isComplete())
+        virtual bool land(float timeout_sec) override
         {
-            throw VehicleMoveException("Drone hasn't reported a landing state");
-        }
-        return waiter.isComplete();
-    }
+            SingleCall lock(this);
 
-    virtual bool goHome(float timeout_sec) override
-    {
-        SingleCall lock(this);
+            //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
+            //we assume the ground is relatively flat an we are landing roughly at the home altitude.
+            updateState();
+            checkValidVehicle();
+            if (current_state_.home.is_set) {
+                bool rc = false;
+                if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc))
+                {
+                    throw VehicleMoveException("Landing command - timeout waiting for response from drone");
+                }
+                else if (!rc) {
+                    throw VehicleMoveException("Landing command rejected by drone");
+                }
+            }
+            else {
+                throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude.  Could fix this if we hook up a distance to ground sensor...");
+            }
 
-        checkValidVehicle();
-        bool rc = false;
-        if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
-            static_cast<int>(timeout_sec) * 1000, &rc)) {
-            throw VehicleMoveException("goHome - timeout waiting for response from drone");
+            const auto& waiter = waitForFunction([&]() {
+                updateState();
+                return current_state_.controls.landed;
+                }, timeout_sec);
+
+            // Wait for landed state (or user cancellation)
+            if (!waiter.isComplete())
+            {
+                throw VehicleMoveException("Drone hasn't reported a landing state");
+            }
+            return waiter.isComplete();
         }
-        return rc;
-    }
 
-    virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
-        const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
-    {
-        SingleTaskCall lock(this);
-
-        unused(adaptive_lookahead);
-        unused(lookahead);
-        unused(drivetrain);
-
-        // save current manual, cruise, and max velocity parameters
-        bool result = false;
-        mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
-        result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
-        result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
-        result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
-
-        if (result) {
-            // set max velocity parameter
-            mavlinkcom::MavLinkParameter p;
-            p.name = "MPC_XY_VEL_MAX";
-            p.value = velocity;
-            mav_vehicle_->setParameter(p).wait(1000, &result);
+        virtual bool goHome(float timeout_sec) override
+        {
+            SingleCall lock(this);
 
-            if (result) {
-                const Vector3r& goal_pos = Vector3r(x, y, z);
-                Vector3r goal_dist_vect;
-                float goal_dist;
+            checkValidVehicle();
+            bool rc = false;
+            if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
+                static_cast<int>(timeout_sec) * 1000, &rc)) {
+                throw VehicleMoveException("goHome - timeout waiting for response from drone");
+            }
+            return rc;
+        }
 
-                Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
+        virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
+            const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
+        {
+            SingleTaskCall lock(this);
 
-                while (!waiter.isComplete()) {
-                    goal_dist_vect = getPosition() - goal_pos;
-                    const Vector3r& goal_normalized = goal_dist_vect.normalized();
-                    goal_dist = goal_dist_vect.dot(goal_normalized);
+            unused(adaptive_lookahead);
+            unused(lookahead);
+            unused(drivetrain);
 
-                    if (goal_dist > getDistanceAccuracy()) {
-                        moveToPositionInternal(goal_pos, yaw_mode);
+            // save current manual, cruise, and max velocity parameters
+            bool result = false;
+            mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
+            result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
+            result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
+            result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
 
-                        //sleep for rest of the cycle
-                        if (!waiter.sleep())
-                            return false;
-                    }
-                    else {
-                        waiter.complete();
+            if (result) {
+                // set max velocity parameter
+                mavlinkcom::MavLinkParameter p;
+                p.name = "MPC_XY_VEL_MAX";
+                p.value = velocity;
+                mav_vehicle_->setParameter(p).wait(1000, &result);
+
+                if (result) {
+                    const Vector3r& goal_pos = Vector3r(x, y, z);
+                    Vector3r goal_dist_vect;
+                    float goal_dist;
+
+                    Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
+
+                    while (!waiter.isComplete()) {
+                        goal_dist_vect = getPosition() - goal_pos;
+                        const Vector3r& goal_normalized = goal_dist_vect.normalized();
+                        goal_dist = goal_dist_vect.dot(goal_normalized);
+
+                        if (goal_dist > getDistanceAccuracy()) {
+                            moveToPositionInternal(goal_pos, yaw_mode);
+
+                            //sleep for rest of the cycle
+                            if (!waiter.sleep())
+                                return false;
+                        }
+                        else {
+                            waiter.complete();
+                        }
                     }
-                }
 
-                // reset manual, cruise, and max velocity parameters
-                bool result_temp = false;
-                mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
-                mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
-                result = result && result_temp;
-                mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
-                result = result && result_temp;
+                    // reset manual, cruise, and max velocity parameters
+                    bool result_temp = false;
+                    mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
+                    mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
+                    result = result && result_temp;
+                    mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
+                    result = result && result_temp;
 
-                return result && waiter.isComplete();
+                    return result && waiter.isComplete();
+                }
             }
-        }
-
-        return result;
-    }
 
-    virtual bool hover() override
-    {
-        SingleCall lock(this);
+            return result;
+        }
 
-        bool rc = false;
-        checkValidVehicle();
-        mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
-        //auto start_time = std::chrono::system_clock::now();
-        while (!getCancelToken().isCancelled())
+        virtual bool hover() override
         {
-            if (result.wait(100, &rc))
+            SingleCall lock(this);
+
+            bool rc = false;
+            checkValidVehicle();
+            mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
+            //auto start_time = std::chrono::system_clock::now();
+            while (!getCancelToken().isCancelled())
             {
-                break;
+                if (result.wait(100, &rc))
+                {
+                    break;
+                }
             }
+            return rc;
         }
-        return rc;
-    }
 
-    virtual GeoPoint getHomeGeoPoint() const override
-    {
-        updateState();
-        if (current_state_.home.is_set)
-            return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
-        else
-            return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
-    }
-
-    virtual GeoPoint getGpsLocation() const override
-    {
-        updateState();
-        return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
-    }
+        virtual GeoPoint getHomeGeoPoint() const override
+        {
+            updateState();
+            if (current_state_.home.is_set)
+                return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
+            else
+                return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
+        }
 
-    virtual void sendTelemetry(float last_interval = -1) override
-    {
-        if ((logviewer_proxy_ == nullptr && log_ == nullptr) || connection_ == nullptr || mav_vehicle_ == nullptr) {
-            return;
+        virtual GeoPoint getGpsLocation() const override
+        {
+            updateState();
+            return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
         }
-        mavlinkcom::MavLinkTelemetry data;
-        connection_->getTelemetry(data);
-        if (data.messagesReceived == 0) {
-            if (!hil_message_timer_.started()) {
-                hil_message_timer_.start();
+
+        virtual void sendTelemetry(float last_interval = -1) override
+        {
+            if (connection_ == nullptr || mav_vehicle_ == nullptr) {
+                return;
             }
-            else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
-                addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
+            mavlinkcom::MavLinkTelemetry data;
+            connection_->getTelemetry(data);
+            if (data.messages_received == 0) {
+                if (!hil_message_timer_.started()) {
+                    hil_message_timer_.start();
+                }
+                else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
+                    addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
+                }
+            }
+            else {
+                hil_message_timer_.stop();
             }
-        }
-        else {
-            hil_message_timer_.stop();
-        }
 
-        // listen to the other mavlink connection also
-        auto mavcon = mav_vehicle_->getConnection();
-        if (mavcon != connection_) {
-            mavlinkcom::MavLinkTelemetry gcs;
-            mavcon->getTelemetry(gcs);
+            if ((logviewer_proxy_ == nullptr && log_ == nullptr)) {
+                return;
+            }
 
-            data.handlerMicroseconds += gcs.handlerMicroseconds;
-            data.messagesHandled += gcs.messagesHandled;
-            data.messagesReceived += gcs.messagesReceived;
-            data.messagesSent += gcs.messagesSent;
+            // listen to the other mavlink connection also
+            auto mavcon = mav_vehicle_->getConnection();
+            if (mavcon != connection_) {
+                mavlinkcom::MavLinkTelemetry gcs;
+                mavcon->getTelemetry(gcs);
 
-            if (gcs.messagesReceived == 0)
-            {
-                if (!gcs_message_timer_.started()) {
-                    gcs_message_timer_.start();
+                data.handler_microseconds += gcs.handler_microseconds;
+                data.messages_handled += gcs.messages_handled;
+                data.messages_received += gcs.messages_received;
+                data.messages_sent += gcs.messages_sent;
+
+                if (gcs.messages_received == 0)
+                {
+                    if (!gcs_message_timer_.started()) {
+                        gcs_message_timer_.start();
+                    }
+                    else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
+                        addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
+                    }
                 }
-                else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
-                    addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
+                else {
+                    gcs_message_timer_.stop();
                 }
             }
-            else {
-                gcs_message_timer_.stop();
+
+            data.render_time = static_cast<int64_t>(last_interval * 1000000);// microseconds
+            data.udpate_rate = update_count_;
+            data.sensor_rate = hil_sensor_count_;
+            data.actuation_delay = actuator_delay_;
+            data.lock_step_resets = lock_step_resets_;
+
+            // reset the counters we just captured.
+            update_count_ = 0;
+            hil_sensor_count_ = 0;
+            actuator_delay_ = 0;
+
+            if (logviewer_proxy_ != nullptr) {
+                logviewer_proxy_->sendMessage(data);
+            }
+
+            if (log_ != nullptr) {
+                mavlinkcom::MavLinkMessage msg;
+                msg.magic = MAVLINK_STX_MAVLINK1;
+                data.encode(msg);
+                msg.update_checksum();
+                // disk I/O is unpredictable, so we have to get it out of the update loop
+                // which is why this thread exists.
+                log_->write(msg);
             }
         }
 
-        data.renderTime = static_cast<int64_t>(last_interval * 1000000);// microseconds
-        data.udpateRateHz = update_rate_;
+    void start_telemtry_thread() {
 
-        if (logviewer_proxy_ != nullptr) {
-            logviewer_proxy_->sendMessage(data);
+        if (this->telemetry_thread_.joinable())
+        {
+            this->telemetry_thread_.join();
         }
 
-        if (log_ != nullptr) {
-            mavlinkcom::MavLinkMessage msg;
-            msg.magic = MAVLINK_STX_MAVLINK1;
-            data.encode(msg);
-            msg.update_checksum();
-            log_->write(msg);
+        // reset the counters we use for telemetry.
+        update_count_ = 0;
+        hil_sensor_count_ = 0;
+        actuator_delay_ = 0;
+
+        this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this);
+    }
+
+    void telemtry_thread() {
+        while (log_ != nullptr) {
+
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+
+            Utils::log(Utils::stringf("sending telemetry, update_count_ is %d...", update_count_));
+            sendTelemetry(1);
         }
     }
 
@@ -782,6 +816,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         addStatusMessage("Disconnecting mavlink vehicle");
         connected_ = false;
         connecting_ = false;
+
+        if (is_armed_) {
+            // close the telemetry log.
+            is_armed_ = false;
+            onDisarmed();
+        }
+
         if (connection_ != nullptr) {
             if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
                 setNormalMode();
@@ -1470,6 +1511,24 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
+    void handleLockStep() {
+        received_actuator_controls_ = true;
+        // if the timestamps match then it means we are in lockstep mode.
+        if (!lock_step_enabled_) {
+            // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
+            if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
+                addStatusMessage("Enabling lockstep mode");
+                lock_step_enabled_ = true;
+            }
+        }
+        else
+        {
+            auto now = clock()->nowNanos() / 1000;
+            auto delay = static_cast<uint32_t>(now - last_update_time_);
+            actuator_delay_ += delay;
+        }
+    }
+
     void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
     {
         if (msg.msgid == HeartbeatMessage.msgid) {
@@ -1523,7 +1582,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 rotor_controls_[7] = HilControlsMessage.aux4;
 
                 normalizeRotorControls();
-                received_actuator_controls_ = true;
+                handleLockStep();
             }
         }
         else if (msg.msgid == HilActuatorControlsMessage.msgid) {
@@ -1544,15 +1603,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             if (isarmed) {
                 normalizeRotorControls();
             }
-            received_actuator_controls_ = true;
-            // if the timestamps match then it means we are in lockstep mode.
-            if (!lock_step_enabled_) {
-                // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
-                if (getSimTime() == HilActuatorControlsMessage.time_usec) {
-                    addStatusMessage("Enabling lockstep mode");
-                    lock_step_enabled_ = true;
-                }
-            }
+            
+            handleLockStep();
         }
         else if (msg.msgid == MavLinkGpsRawInt.msgid) {
             MavLinkGpsRawInt.decode(msg);
@@ -1607,7 +1659,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
         
         mavlinkcom::MavLinkHilSensor hil_sensor;
-        hil_sensor.time_usec = getSimTime();
+        hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime();
 
         hil_sensor.xacc = acceleration.x();
         hil_sensor.yacc = acceleration.y();
@@ -1742,10 +1794,12 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         sim_time_us_ = 0;
         last_sys_time_ = 0;
         last_gps_time_ = 0;
+        last_update_time_ = 0;
         last_hil_sensor_time_ = 0;
-        update_rate_ = 0;
         update_count_ = 0;
-        last_fps_time_ = 0;
+        hil_sensor_count_ = 0;
+        lock_step_resets_ = 0;
+        actuator_delay_ = 0;
         is_api_control_enabled_ = false;
         thrust_controller_ = PidController();
         Utils::setValue(rotor_controls_, 0.0f);
@@ -1842,15 +1896,18 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     const double GroundTolerance = 0.1;
 
     // variables for throttling HIL_SENSOR and SYSTEM_TIME messages
-    const int DEFAULT_SIM_RATE = 250; // Hz.
-    const unsigned long ticks_per_update_ = 1000000 / DEFAULT_SIM_RATE; // microseconds
     uint32_t last_sys_time_ = 0;
     unsigned long long sim_time_us_ = 0;
     uint64_t last_gps_time_ = 0;
+    uint64_t last_update_time_ = 0;
     uint64_t last_hil_sensor_time_ = 0;
-    uint64_t last_fps_time_ = 0;
+
+    // variables accumulated for MavLinkTelemetry messages.
     uint32_t update_count_ = 0;
-    uint32_t update_rate_ = 0;
+    uint32_t hil_sensor_count_ = 0;
+    uint32_t lock_step_resets_ = 0;
+    uint32_t actuator_delay_ = 0;
+    std::thread telemetry_thread_;
 
     //additional variables required for MultirotorApiBase implementation
     //this is optional for methods that might not use vehicle commands
@@ -1861,6 +1918,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     common_utils::Timer hil_message_timer_;
     common_utils::Timer gcs_message_timer_;
     std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
+    std::string log_file_name_;
 
     //every time we return status update, we need to check if we have new data
     //this is why below two variables are marked as mutable
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index 3f55ddd35e..734594ae9d 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -135,16 +135,19 @@ namespace mavlinkcom
     class MavLinkTelemetry : public MavLinkMessageBase {
     public:
         const static uint8_t kMessageId = 204; // in the user range 180-229.
-        const static int MessageLength = 8 * 4;
+        const static int MessageLength = 11 * 4;
         MavLinkTelemetry() { msgid = kMessageId; }
-        uint32_t messagesSent = 0;		  // number of messages sent since the last telemetry message
-        uint32_t messagesReceived = 0;	  // number of messages received since the last telemetry message
-        uint32_t messagesHandled = 0;	  // number of messages handled since the last telemetry message
-        uint32_t crcErrors = 0;			  // # crc errors detected in mavlink stream since the last telemetry message
-        uint32_t handlerMicroseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
-        uint32_t renderTime = 0;          // total time spent rendering frames since the last telemetry message
-        int32_t wifiRssi = 0;             // if this device is communicating over wifi this is the signal strength.
-        uint32_t udpateRateHz = 0;        // HIL_SENSOR update rate in hertz
+        uint32_t messages_sent = 0;		   // number of messages sent since the last telemetry message
+        uint32_t messages_received = 0;	   // number of messages received since the last telemetry message
+        uint32_t messages_handled = 0;	   // number of messages handled since the last telemetry message
+        uint32_t crc_errors = 0;		   // # crc errors detected in mavlink stream since the last telemetry message
+        uint32_t handler_microseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
+        uint32_t render_time = 0;          // total time spent rendering frames since the last telemetry message
+        int32_t wifi_rssi = 0;             // if this device is communicating over wifi this is the signal strength.
+        uint32_t udpate_rate = 0;          // rate at which update() is being called on MavLinkMultiRotorApi
+        uint32_t actuation_delay = 0;      // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response
+        uint32_t sensor_rate = 0;          // rate we are sending HIL_SENSOR messages
+        uint32_t lock_step_resets = 0;     // total number of lock_step resets
  
         // not serialized
         const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on.
@@ -152,14 +155,17 @@ namespace mavlinkcom
 
             std::ostringstream result;
             result << "\"MavLinkTelemetry\"" << " : { ";
-            result << "\"messagesSent\":" << this->messagesSent << ",";
-            result << "\"messagesReceived\":" << this->messagesReceived << ",";
-            result << "\"messagesHandled\":" << this->messagesHandled << ",";
-            result << "\"crcErrors\":" << this->crcErrors << ",";
-            result << "\"handlerMicroseconds\":" << this->handlerMicroseconds << ",";
-            result << "\"renderTime\":" << this->renderTime;
-            result << "\"wifiRssi\":" << this->wifiRssi;
-            result << "\"udpateRateHz\":" << this->udpateRateHz;
+            result << "\"messages_sent\":" << this->messages_sent << ",";
+            result << "\"messages_received\":" << this->messages_received << ",";
+            result << "\"messages_handled\":" << this->messages_handled << ",";
+            result << "\"crc_errors\":" << this->crc_errors << ",";
+            result << "\"handler_microseconds\":" << this->handler_microseconds << ",";
+            result << "\"render_time\":" << this->render_time;
+            result << "\"wifi_rssi\":" << this->wifi_rssi;
+            result << "\"udpate_rate\":" << this->udpate_rate;
+            result << "\"actuation_delay\":" << this->actuation_delay;
+            result << "\"sensor_rate\":" << this->sensor_rate;
+            result << "\"lock_step_resets\":" << this->lock_step_resets;
             result << "}";
             return result.str();
         }
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 89df650fcc..6a76ed9521 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -305,26 +305,32 @@ void MavLinkMessageBase::unpack_float_array(int len, const char* buffer, float*
 }
 
 int MavLinkTelemetry::pack(char* buffer) const {
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesSent), 0);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesReceived), 4);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messagesHandled), 8);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->crcErrors), 12);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->handlerMicroseconds), 16);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->renderTime), 20);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->wifiRssi), 24);    
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->udpateRateHz), 28);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messages_sent), 0);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messages_received), 4);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->messages_handled), 8);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->crc_errors), 12);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->handler_microseconds), 16);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->render_time), 20);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->wifi_rssi), 24);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->udpate_rate), 28);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->actuation_delay), 32);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->sensor_rate), 36);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->lock_step_resets), 40);
     return MavLinkTelemetry::MessageLength;
 }
 
 int MavLinkTelemetry::unpack(const char* buffer) {
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesSent), 0);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesReceived), 4);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messagesHandled), 8);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->crcErrors), 12);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->handlerMicroseconds), 16);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->renderTime), 20);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->wifiRssi), 24);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->udpateRateHz), 28);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messages_sent), 0);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messages_received), 4);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->messages_handled), 8);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->crc_errors), 12);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->handler_microseconds), 16);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->render_time), 20);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->wifi_rssi), 24);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->udpate_rate), 28);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->actuation_delay), 32);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->sensor_rate), 36);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->lock_step_resets), 40);
     return MavLinkTelemetry::MessageLength;
 }
 
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
index ffa94d6879..b2c2496871 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
@@ -16,12 +16,11 @@ using namespace mavlinkcom_impl;
 MavLinkConnectionImpl::MavLinkConnectionImpl()
 {
     // add our custom telemetry message length.
-    telemetry_.crcErrors = 0;
-    telemetry_.handlerMicroseconds = 0;
-    telemetry_.messagesHandled = 0;
-    telemetry_.messagesReceived = 0;
-    telemetry_.messagesSent = 0;
-    telemetry_.renderTime = 0;
+    telemetry_.crc_errors = 0;
+    telemetry_.handler_microseconds = 0;
+    telemetry_.messages_handled = 0;
+    telemetry_.messages_received = 0;
+    telemetry_.messages_sent = 0;
     closed = true;
     ::memset(&mavlink_intermediate_status_, 0, sizeof(mavlink_status_t));
     ::memset(&mavlink_status_, 0, sizeof(mavlink_status_t));
@@ -246,7 +245,7 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m)
     }
     {
         std::lock_guard<std::mutex> guard(telemetry_mutex_);
-        telemetry_.messagesSent++;
+        telemetry_.messages_sent++;
     }
 
 }
@@ -441,7 +440,7 @@ void MavLinkConnectionImpl::readPackets()
             }
             else if (frame_state == MAVLINK_FRAMING_BAD_CRC) {
                 std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                telemetry_.crcErrors++;
+                telemetry_.crc_errors++;
             }
             else if (frame_state == MAVLINK_FRAMING_OK)
             {
@@ -463,7 +462,7 @@ void MavLinkConnectionImpl::readPackets()
                 {
                     {
                         std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                        telemetry_.messagesReceived++;
+                        telemetry_.messages_received++;
                     }
                     // queue event for publishing.
                     {
@@ -490,7 +489,7 @@ void MavLinkConnectionImpl::readPackets()
             }
             else {
                 std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                telemetry_.crcErrors++;
+                telemetry_.crc_errors++;
             }
         }	
 
@@ -565,8 +564,8 @@ void MavLinkConnectionImpl::drainQueue()
             auto diff = endTime - startTime;
             long microseconds = static_cast<long>(std::chrono::duration_cast<std::chrono::microseconds>(diff).count());
             std::lock_guard<std::mutex> guard(telemetry_mutex_);
-            telemetry_.messagesHandled++;
-            telemetry_.handlerMicroseconds += microseconds;
+            telemetry_.messages_handled++;
+            telemetry_.handler_microseconds += microseconds;
         }
     }
 }
@@ -596,14 +595,12 @@ void MavLinkConnectionImpl::getTelemetry(MavLinkTelemetry& result)
     std::lock_guard<std::mutex> guard(telemetry_mutex_);
     result = telemetry_;
     // reset counters 
-    telemetry_.crcErrors = 0;
-    telemetry_.handlerMicroseconds = 0;
-    telemetry_.messagesHandled = 0;
-    telemetry_.messagesReceived = 0;
-    telemetry_.messagesSent = 0;
-    telemetry_.renderTime = 0;
-    telemetry_.udpateRateHz = 0;
+    telemetry_.crc_errors = 0;
+    telemetry_.handler_microseconds = 0;
+    telemetry_.messages_handled = 0;
+    telemetry_.messages_received = 0;
+    telemetry_.messages_sent = 0;
     if (telemetry_.wifiInterfaceName != nullptr) {
-        telemetry_.wifiRssi = port->getRssi(telemetry_.wifiInterfaceName);
+        telemetry_.wifi_rssi = port->getRssi(telemetry_.wifiInterfaceName);
     }
 }

From 62048e5c8b563695d973a77ea3701044eae03136 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sat, 17 Apr 2021 21:52:34 -0700
Subject: [PATCH 11/37] guard telemetry variables with a mutex.

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 69 ++++++++++++-------
 1 file changed, 46 insertions(+), 23 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 29a77e8f88..f18336a6fe 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -117,25 +117,34 @@ namespace msr { namespace airlib {
                 if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
                     return;
 
-                update_count_++;
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    update_count_++;
+                }
 
                 auto now = clock()->nowNanos() / 1000;
                 if (lock_step_enabled_) {
                     if (last_update_time_ + 100000 < now) {
                         // if 100 ms passes then something is terribly wrong, reset lockstep mode
                         lock_step_enabled_ = false;
-                        lock_step_resets_++;
+                        {
+                            std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                            lock_step_resets_++;
+                        }
                         addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
                     }
-
-                    if (!received_actuator_controls_) {
+                    else if (!received_actuator_controls_) {
                         // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
                         return;
                     }
                 }
 
                 last_update_time_ = now;
-                hil_sensor_count_++;
+
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    hil_sensor_count_++;
+                }
                 advanceTime();
 
                 //send sensor updates
@@ -563,6 +572,8 @@ namespace msr { namespace airlib {
             if (connection_ == nullptr || mav_vehicle_ == nullptr) {
                 return;
             }
+
+            // This method is called at high frequence from MultirotorPawnSimApi::updateRendering.
             mavlinkcom::MavLinkTelemetry data;
             connection_->getTelemetry(data);
             if (data.messages_received == 0) {
@@ -576,11 +587,20 @@ namespace msr { namespace airlib {
             else {
                 hil_message_timer_.stop();
             }
+        }
+
+        void writeTelemetry(float last_interval = -1)
+        {
+            auto proxy = logviewer_proxy_;
+            auto log = log_;
 
             if ((logviewer_proxy_ == nullptr && log_ == nullptr)) {
                 return;
             }
 
+            mavlinkcom::MavLinkTelemetry data;
+            connection_->getTelemetry(data);
+
             // listen to the other mavlink connection also
             auto mavcon = mav_vehicle_->getConnection();
             if (mavcon != connection_) {
@@ -607,32 +627,35 @@ namespace msr { namespace airlib {
             }
 
             data.render_time = static_cast<int64_t>(last_interval * 1000000);// microseconds
-            data.udpate_rate = update_count_;
-            data.sensor_rate = hil_sensor_count_;
-            data.actuation_delay = actuator_delay_;
-            data.lock_step_resets = lock_step_resets_;
 
-            // reset the counters we just captured.
-            update_count_ = 0;
-            hil_sensor_count_ = 0;
-            actuator_delay_ = 0;
+            {
+                std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                data.udpate_rate = update_count_;
+                data.sensor_rate = hil_sensor_count_;
+                data.actuation_delay = actuator_delay_;
+                data.lock_step_resets = lock_step_resets_;
+                // reset the counters we just captured.
+                update_count_ = 0;
+                hil_sensor_count_ = 0;
+                actuator_delay_ = 0;
+            }
 
-            if (logviewer_proxy_ != nullptr) {
-                logviewer_proxy_->sendMessage(data);
+            if (proxy != nullptr) {
+                proxy->sendMessage(data);
             }
 
-            if (log_ != nullptr) {
+            if (log != nullptr) {
                 mavlinkcom::MavLinkMessage msg;
                 msg.magic = MAVLINK_STX_MAVLINK1;
                 data.encode(msg);
                 msg.update_checksum();
                 // disk I/O is unpredictable, so we have to get it out of the update loop
                 // which is why this thread exists.
-                log_->write(msg);
+                log->write(msg);
             }
         }
 
-    void start_telemtry_thread() {
+        void start_telemtry_thread() {
 
         if (this->telemetry_thread_.joinable())
         {
@@ -651,9 +674,7 @@ namespace msr { namespace airlib {
         while (log_ != nullptr) {
 
             std::this_thread::sleep_for(std::chrono::seconds(1));
-
-            Utils::log(Utils::stringf("sending telemetry, update_count_ is %d...", update_count_));
-            sendTelemetry(1);
+            writeTelemetry(1);
         }
     }
 
@@ -1426,7 +1447,7 @@ namespace msr { namespace airlib {
 
     mavlinkcom::MavLinkHilSensor getLastSensorMessage()
     {
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
+        std::lock_guard<std::mutex> guard(last_message_mutex_);       
         return last_sensor_message_;
     }
 
@@ -1525,6 +1546,8 @@ namespace msr { namespace airlib {
         {
             auto now = clock()->nowNanos() / 1000;
             auto delay = static_cast<uint32_t>(now - last_update_time_);
+
+            std::lock_guard<std::mutex> guard(telemetry_mutex_);
             actuator_delay_ += delay;
         }
     }
@@ -1871,7 +1894,7 @@ namespace msr { namespace airlib {
     mavlinkcom::MavLinkDistanceSensor last_distance_message_;
     mavlinkcom::MavLinkHilGps last_gps_message_;
 
-    std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_;
+    std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_;
 
     //variables required for VehicleApiBase implementation
     bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false;

From 938046f8f08ccfe4112aa0286fcbb148c2d2b368 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sat, 17 Apr 2021 22:08:02 -0700
Subject: [PATCH 12/37] make actuator delay an average

---
 .../multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp       | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index f18336a6fe..ae39c32fc5 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -632,7 +632,7 @@ namespace msr { namespace airlib {
                 std::lock_guard<std::mutex> guard(telemetry_mutex_);
                 data.udpate_rate = update_count_;
                 data.sensor_rate = hil_sensor_count_;
-                data.actuation_delay = actuator_delay_;
+                data.actuation_delay = hil_sensor_count_ == 0 ? actuator_delay_ : actuator_delay_  / hil_sensor_count_;
                 data.lock_step_resets = lock_step_resets_;
                 // reset the counters we just captured.
                 update_count_ = 0;

From 9608bb4a1bf910283756e818995d7262016bcde1 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sun, 18 Apr 2021 10:32:53 -0700
Subject: [PATCH 13/37] add missing script files

---
 PythonClient/multirotor/Error.wav         | Bin 0 -> 331108 bytes
 PythonClient/multirotor/speaker.py        |  37 +++++
 PythonClient/multirotor/stability_test.py |   2 +-
 PythonClient/multirotor/wav_reader.py     | 158 ++++++++++++++++++++++
 4 files changed, 196 insertions(+), 1 deletion(-)
 create mode 100644 PythonClient/multirotor/Error.wav
 create mode 100644 PythonClient/multirotor/speaker.py
 create mode 100644 PythonClient/multirotor/wav_reader.py

diff --git a/PythonClient/multirotor/Error.wav b/PythonClient/multirotor/Error.wav
new file mode 100644
index 0000000000000000000000000000000000000000..c4a01cb2b4b7af6445d018a7407d2d55d2378b44
GIT binary patch
diff --git a/PythonClient/multirotor/speaker.py b/PythonClient/multirotor/speaker.py
new file mode 100644
index 0000000000..c3beccefb0
--- /dev/null
+++ b/PythonClient/multirotor/speaker.py
@@ -0,0 +1,37 @@
+###################################################################################################
+#
+#  Project:  Embedded Learning Library (ELL)
+#  File:     speaker.py
+#  Authors:  Chris Lovett
+#
+#  Requires: Python 3.x
+#
+###################################################################################################
+
+import pyaudio
+
+
+class Speaker:
+    def __init__(self):
+        self.output_stream = None
+        self.audio = pyaudio.PyAudio()
+
+    def open(self, audio_format, num_channels, rate):
+
+        # open speakers so we can hear what it is processing...
+        self.output_stream = self.audio.open(format=audio_format,
+                                             channels=num_channels,
+                                             rate=rate,
+                                             output=True)
+
+    def write(self, data):
+        if self.output_stream:
+            self.output_stream.write(data)
+
+    def close(self):
+        if self.output_stream:
+            self.output_stream.close()
+            self.output_stream = None
+
+    def is_closed(self):
+        return self.output_stream is None
diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py
index 9612892dff..ad1da803f9 100644
--- a/PythonClient/multirotor/stability_test.py
+++ b/PythonClient/multirotor/stability_test.py
@@ -55,7 +55,7 @@ def is_unstable(self, amount):
 
     # fly for 2 minutes
     start = time.time()
-    while time.time() < start + 120:
+    while time.time() < start + 300:
         state = client.getMultirotorState()
         x_val = state.kinematics_estimated.position.x_val
         y_val = state.kinematics_estimated.position.y_val
diff --git a/PythonClient/multirotor/wav_reader.py b/PythonClient/multirotor/wav_reader.py
new file mode 100644
index 0000000000..74e3bce185
--- /dev/null
+++ b/PythonClient/multirotor/wav_reader.py
@@ -0,0 +1,158 @@
+###################################################################################################
+#
+#  Project:  Embedded Learning Library (ELL)
+#  File:     wav_reader.py
+#  Authors:  Chris Lovett
+#
+#  Requires: Python 3.x
+#
+###################################################################################################
+import audioop
+import math
+import wave
+
+import numpy as np
+import pyaudio
+
+
+class WavReader:
+    def __init__(self, sample_rate=16000, channels=1, auto_scale=True):
+        """ Initialize the wav reader with the type of audio you want returned.
+        sample_rate  Rate you want audio converted to (default 16 kHz)
+        channels     Number of channels you want output (default 1)
+        auto_scale   Whether to scale numbers to the range -1 to 1.
+        """
+        self.input_stream = None
+        self.audio = pyaudio.PyAudio()
+        self.wav_file = None
+        self.requested_channels = int(channels)
+        self.requested_rate = int(sample_rate)
+        self.buffer_size = 0
+        self.sample_width = 0
+        self.read_size = None
+        self.dtype = None
+        self.auto_scale = auto_scale
+        self.audio_scale_factor = 1
+        self.tail = None
+
+    def open(self, filename, buffer_size, speaker=None):
+        """ open a wav file for reading
+        buffersize   Number of audio samples to return on each read() call
+        speaker      Optional output speaker to send converted audio to so you can hear it.
+        """
+        self.speaker = speaker
+        # open a stream on the audio input file.
+        self.wav_file = wave.open(filename, "rb")
+        self.cvstate = None
+        self.read_size = int(buffer_size)
+        self.actual_channels = self.wav_file.getnchannels()
+        self.actual_rate = self.wav_file.getframerate()
+        self.sample_width = self.wav_file.getsampwidth()
+        # assumes signed integer used in raw audio, so for example, the max for 16bit is 2^15 (32768)
+        if self.auto_scale:
+            self.audio_scale_factor = 1 / pow(2, (8 * self.sample_width) - 1)
+
+        if self.requested_rate == 0:
+            raise Exception("Requested rate cannot be zero")
+        self.buffer_size = int(math.ceil((self.read_size * self.actual_rate) / self.requested_rate))
+
+        # convert int16 data to scaled floats
+        if self.sample_width == 1:
+            self.dtype = np.int8
+        elif self.sample_width == 2:
+            self.dtype = np.int16
+        elif self.sample_width == 4:
+            self.dtype = np.int32
+        else:
+            msg = "Unexpected sample width {}, can only handle 1, 2 or 4 byte audio"
+            raise Exception(msg.format(self.sample_width))
+
+        if speaker:
+            # configure output stream to match what we are resampling to...
+            audio_format = self.audio.get_format_from_width(self.sample_width)
+            speaker.open(audio_format, self.requested_channels, self.requested_rate)
+
+    def read_raw(self):
+        """ Reads the next chunk of audio (returns buffer_size provided to open)
+        It returns the raw data buffers converted to the target rate without any scaling.
+        """
+        if self.wav_file is None:
+            return None
+
+        data = self.wav_file.readframes(self.buffer_size)
+        if len(data) == 0:
+            return None
+
+        if self.actual_rate != self.requested_rate:
+            # convert the audio to the desired recording rate
+            data, self.cvstate = audioop.ratecv(data, self.sample_width, self.actual_channels, self.actual_rate,
+                                                self.requested_rate, self.cvstate)
+
+        return self.get_requested_channels(data)
+
+    def get_requested_channels(self, data):
+        if self.requested_channels > self.actual_channels:
+            raise Exception("Cannot add channels, actual is {}, requested is {}".format(
+                self.actual_channels, self.requested_channels))
+
+        if self.requested_channels < self.actual_channels:
+            data = np.frombuffer(data, dtype=np.int16)
+            channels = []
+            # split into separate channels
+            for i in range(self.actual_channels):
+                channels += [data[i::self.actual_channels]]
+            # drop the channels we don't want
+            channels = channels[0:self.requested_channels]
+            # zip the resulting channels back up.
+            data = np.array(list(zip(*channels))).flatten()
+            # convert back to packed bytes in PCM 16 format
+            data = bytes(np.array(data, dtype=np.int16))
+
+        return data
+
+    def read(self):
+        """ Reads the next chunk of audio (returns buffer_size provided to open)
+        It returns the data converted to floating point numbers between -1 and 1, scaled by the range of
+        values possible for the given audio format.
+        """
+
+        # deal with any accumulation of tails, if the tail grows to a full
+        # buffer then return it!
+        if self.tail is not None and len(self.tail) >= self.read_size:
+            data = self.tail[0:self.read_size]
+            self.tail = self.tail[self.read_size:]
+            return data
+
+        data = self.read_raw()
+        if data is None:
+            return None
+
+        if self.speaker:
+            self.speaker.write(data)
+
+        data = np.frombuffer(data, dtype=self.dtype).astype(float)
+        if self.tail is not None:
+            # we have a tail from previous frame, so prepend it
+            data = np.concatenate((self.tail, data))
+
+        # now the caller needs us to stick to our sample_size contract, but when
+        # rate conversion happens we can't be sure that 'data' is exactly that size.
+        if len(data) > self.read_size:
+            # usually one byte extra so add this to our accumulating tail
+            self.tail = data[self.read_size:]
+            data = data[0:self.read_size]
+
+        if len(data) < self.read_size:
+            # might have reached the end of a file, so pad with zeros.
+            zeros = np.zeros(self.read_size - len(data))
+            data = np.concatenate((data, zeros))
+
+        return data * self.audio_scale_factor
+
+    def close(self):
+        if self.wav_file:
+            self.wav_file.close()
+            self.wav_file = None
+
+    def is_closed(self):
+        return self.wav_file is None

From cb46217122b98892dd769e990993bc3fab0470a4 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sun, 18 Apr 2021 10:51:56 -0700
Subject: [PATCH 14/37] fix pwsh support

---
 build.cmd | 27 ++++++++++++++++++++-------
 1 file changed, 20 insertions(+), 7 deletions(-)

diff --git a/build.cmd b/build.cmd
index 61f798ce7f..2476ebbb02 100644
--- a/build.cmd
+++ b/build.cmd
@@ -42,6 +42,7 @@ echo found Powershell && goto start
 set powershell=pwsh
 where pwsh > nul 2>&1
 if ERRORLEVEL 1 goto :nopwsh
+set PWSHV7=1
 echo found pwsh && goto start
 :nopwsh
 echo Powershell or pwsh not found, please install it.
@@ -72,13 +73,17 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1 (
 	ECHO Downloading rpclib
 	ECHO *****************************************************************************************
 	@echo on
-	%powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }"
+    if "%PWSHV7%" == "" (
+        %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }"
+    ) else (
+	    %powershell% -command "iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip"
+    )
 	@echo off
 	
 	REM //remove any previous versions
 	rmdir external\rpclib /q /s
 
-	%powershell% -command "& { Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib }"
+	%powershell% -command "Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib"
 	del external\rpclib.zip /q
 	
 	REM //Don't fail the build if the high-poly car is unable to be downloaded
@@ -137,10 +142,14 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
         @echo on
         REM %powershell% -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }"
         REM %powershell% -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }"
-        %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }"
+        if "%PWSHV7%" == "" (
+            %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }"
+        ) else (
+            %powershell% -command "iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip"
+        )
         @echo off
 		rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV
-        %powershell% -command "& { Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv }"
+        %powershell% -command "Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv"
         rmdir suv_download_tmp /q /s
         
         REM //Don't fail the build if the high-poly car is unable to be downloaded
@@ -154,9 +163,13 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
 REM //---------- get Eigen library ----------
 IF NOT EXIST AirLib\deps mkdir AirLib\deps
 IF NOT EXIST AirLib\deps\eigen3 (
-    %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }"
-    %powershell% -command "& { Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps }"
-    %powershell% -command "& { Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen }"
+    if "%PWSHV7%" == "" (
+        %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip }"
+    ) else (
+        %powershell% -command "iwr https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip -OutFile eigen3.zip"
+    )
+    %powershell% -command "Expand-Archive -Path eigen3.zip -DestinationPath AirLib\deps"
+    %powershell% -command "Move-Item -Path AirLib\deps\eigen* -Destination AirLib\deps\del_eigen"
     REM move AirLib\deps\eigen* AirLib\deps\del_eigen
     mkdir AirLib\deps\eigen3
     move AirLib\deps\del_eigen\Eigen AirLib\deps\eigen3\Eigen

From cfc7eec7cc8ec1c73e967a4b18fb4ffcac37a352 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sun, 18 Apr 2021 12:30:56 -0700
Subject: [PATCH 15/37] add update_time to mavlinktelemetry

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 20 +++++++++++++++++--
 MavLinkCom/include/MavLinkMessageBase.hpp     |  4 +++-
 MavLinkCom/src/MavLinkMessageBase.cpp         |  3 ++-
 3 files changed, 23 insertions(+), 4 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index ae39c32fc5..d2be5f7183 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -112,6 +112,7 @@ namespace msr { namespace airlib {
         virtual void update() override
         {
             try {
+                auto now = clock()->nowNanos() / 1000;
                 MultirotorApiBase::update();
 
                 if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
@@ -122,7 +123,6 @@ namespace msr { namespace airlib {
                     update_count_++;
                 }
 
-                auto now = clock()->nowNanos() / 1000;
                 if (lock_step_enabled_) {
                     if (last_update_time_ + 100000 < now) {
                         // if 100 ms passes then something is terribly wrong, reset lockstep mode
@@ -194,6 +194,11 @@ namespace msr { namespace airlib {
                     }
                 }
 
+                auto end = clock()->nowNanos() / 1000;
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    update_time_ += (end - now);
+                }
             }
             catch (std::exception& e) {
                 addStatusMessage("Exception sending messages to vehicle");
@@ -202,6 +207,7 @@ namespace msr { namespace airlib {
                 connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
             }
 
+
             //must be done at the end
             if (was_reset_)
                 was_reset_ = false;
@@ -630,14 +636,23 @@ namespace msr { namespace airlib {
 
             {
                 std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                uint32_t average_delay = 0;
+                uint32_t average_update = 0;
+                if (hil_sensor_count_ != 0) {
+                    average_delay = actuator_delay_ / hil_sensor_count_;
+                    average_update = static_cast<uint32_t>(update_time_ / hil_sensor_count_);
+                }
+
                 data.udpate_rate = update_count_;
                 data.sensor_rate = hil_sensor_count_;
-                data.actuation_delay = hil_sensor_count_ == 0 ? actuator_delay_ : actuator_delay_  / hil_sensor_count_;
+                data.actuation_delay = average_delay;
                 data.lock_step_resets = lock_step_resets_;
+                data.update_time = average_update;
                 // reset the counters we just captured.
                 update_count_ = 0;
                 hil_sensor_count_ = 0;
                 actuator_delay_ = 0;
+                update_time_ = 0;
             }
 
             if (proxy != nullptr) {
@@ -1926,6 +1941,7 @@ namespace msr { namespace airlib {
     uint64_t last_hil_sensor_time_ = 0;
 
     // variables accumulated for MavLinkTelemetry messages.
+    uint64_t update_time_ = 0;
     uint32_t update_count_ = 0;
     uint32_t hil_sensor_count_ = 0;
     uint32_t lock_step_resets_ = 0;
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index 734594ae9d..570e0b654f 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -135,7 +135,7 @@ namespace mavlinkcom
     class MavLinkTelemetry : public MavLinkMessageBase {
     public:
         const static uint8_t kMessageId = 204; // in the user range 180-229.
-        const static int MessageLength = 11 * 4;
+        const static int MessageLength = 12 * 4;
         MavLinkTelemetry() { msgid = kMessageId; }
         uint32_t messages_sent = 0;		   // number of messages sent since the last telemetry message
         uint32_t messages_received = 0;	   // number of messages received since the last telemetry message
@@ -148,6 +148,7 @@ namespace mavlinkcom
         uint32_t actuation_delay = 0;      // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response
         uint32_t sensor_rate = 0;          // rate we are sending HIL_SENSOR messages
         uint32_t lock_step_resets = 0;     // total number of lock_step resets
+        uint32_t update_time = 0;          // time inside MavLinkMultiRotorApi::update() method
  
         // not serialized
         const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on.
@@ -166,6 +167,7 @@ namespace mavlinkcom
             result << "\"actuation_delay\":" << this->actuation_delay;
             result << "\"sensor_rate\":" << this->sensor_rate;
             result << "\"lock_step_resets\":" << this->lock_step_resets;
+            result << "\"udpate_time\":" << this->update_time;
             result << "}";
             return result.str();
         }
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 6a76ed9521..2e1d3a4dfe 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -316,6 +316,7 @@ int MavLinkTelemetry::pack(char* buffer) const {
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->actuation_delay), 32);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->sensor_rate), 36);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->lock_step_resets), 40);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->update_time), 44);
     return MavLinkTelemetry::MessageLength;
 }
 
@@ -331,10 +332,10 @@ int MavLinkTelemetry::unpack(const char* buffer) {
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->actuation_delay), 32);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->sensor_rate), 36);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->lock_step_resets), 40);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->update_time), 44);
     return MavLinkTelemetry::MessageLength;
 }
 
-
 std::string MavLinkMessageBase::char_array_tostring(int len, const char* field)
 {
     int i = 0;

From f94377061c36013bd725a09d11a673e19bcddd5b Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 20 Apr 2021 11:14:33 -0700
Subject: [PATCH 16/37] add requirements.txt for pythonclient.

---
 PythonClient/requirements.txt | 3 +++
 1 file changed, 3 insertions(+)
 create mode 100644 PythonClient/requirements.txt

diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt
new file mode 100644
index 0000000000..e329ca18d3
--- /dev/null
+++ b/PythonClient/requirements.txt
@@ -0,0 +1,3 @@
+msgpack-rpc-python
+numpy
+pyaudio

From 92f3f3f67f5bfcedc936a93f43128789d553d7c0 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 20 Apr 2021 12:00:28 -0700
Subject: [PATCH 17/37] add info on how to use mavlink logs and px4 log viewer
 to debug a bad flight.

---
 PythonClient/multirotor/stability_test.py |   9 ++--
 docs/images/px4_debugging.png             | Bin 0 -> 246137 bytes
 docs/images/px4_nice.png                  | Bin 0 -> 110696 bytes
 docs/log_viewer.md                        |   2 +-
 docs/px4_logging.md                       |  52 ++++++++++++++++------
 5 files changed, 46 insertions(+), 17 deletions(-)
 create mode 100644 docs/images/px4_debugging.png
 create mode 100644 docs/images/px4_nice.png

diff --git a/PythonClient/multirotor/stability_test.py b/PythonClient/multirotor/stability_test.py
index ad1da803f9..98d80a0e26 100644
--- a/PythonClient/multirotor/stability_test.py
+++ b/PythonClient/multirotor/stability_test.py
@@ -38,8 +38,11 @@ def is_unstable(self, amount):
         print("{}: min={}, max={}, mean={}, stddev={}".format(self.name, minimum, maximum, mean, stddev))
         return (maximum - minimum) > amount
 
+print("### TEST STARTED ###")
+print("This test takes 20 minutes.")
+
 iteration = 0
-while True:
+while iteration < 10:
     iteration  += 1
     x = Numbers("x")
     y = Numbers("y")
@@ -55,7 +58,7 @@ def is_unstable(self, amount):
 
     # fly for 2 minutes
     start = time.time()
-    while time.time() < start + 300:
+    while time.time() < start + 120:
         state = client.getMultirotorState()
         x_val = state.kinematics_estimated.position.x_val
         y_val = state.kinematics_estimated.position.y_val
@@ -84,4 +87,4 @@ def is_unstable(self, amount):
 
     time.sleep(5)
 
-    
+print("### Test Passed ###")
diff --git a/docs/images/px4_debugging.png b/docs/images/px4_debugging.png
new file mode 100644
index 0000000000000000000000000000000000000000..d4309ce5c6d863112083c9a180042efc2f908395
GIT binary patch
diff --git a/docs/images/px4_nice.png b/docs/images/px4_nice.png
new file mode 100644
index 0000000000000000000000000000000000000000..91a1bae612457f01dfc3c2e4e1ff56c61c4e568c
GIT binary patch
diff --git a/docs/log_viewer.md b/docs/log_viewer.md
index f815e503ee..ef29de0c9c 100644
--- a/docs/log_viewer.md
+++ b/docs/log_viewer.md
@@ -19,7 +19,7 @@ estimation catches up with reality, this is more visible after a bad crash.
 
 ### Installation
 
-If you can't build the LogViewer.sln, there is also a [click once installer](http://www.lovettsoftware.com/LovettSoftware/Downloads/Px4LogViewer/Px4LogViewer.application).
+If you can't build the LogViewer.sln, there is also a [click once installer](http://www.lovettsoftware.com/Downloads/Px4LogViewer/Px4LogViewer.application).
 
 ### Configuration
 
diff --git a/docs/px4_logging.md b/docs/px4_logging.md
index 9962380306..e23b710ad8 100644
--- a/docs/px4_logging.md
+++ b/docs/px4_logging.md
@@ -21,19 +21,6 @@ AirSim can capture mavlink log files if you add the following to the PX4 section
 
 AirSim will create a timestamped log file in this folder for each "armed/disarmed" flight session.
 
-You can also create log files using the `MavLinkTest` app to the Simulator and enable logging
-of all mavlink commands to and from the PX4.
-```
-MavLinkTest -server:127.0.0.1:14550 -logdir:d:\temp
-```
-
-Sometimes you will also want to log the "output" from the Simulator that is being sent to the PX4.
-Specifically this will capture the "hilgps" and "hilsensor" messages that are generated by the 
-Simulator.  To do that run this as well:
-```
-MavLinkTest -server:127.0.0.1:14389 -logdir:d:\temp\output
-```
-
 You will then see log files organized by date in d:\temp\logs, specifically *input.mavlink and *output.mavlink files.
 
 ## MavLink LogViewer
@@ -50,3 +37,42 @@ INFO  [logger] Opened log file: rootfs/fs/microsd/log/2017-03-27/20_02_49.ulg
 
 If you are using Pixhawk hardware in HIL mode, then set parameter `SYS_LOGGER=1`
 using QGroundControl. PX4 will write log file on device which you can download at later date using QGroundControl.
+
+## Debugging a bad flight
+
+You can use these *.mavlink log files to debug a bad flight using the [LogViewer](log_viewer.md).
+For example, AirSim/PX4 flight may misbehave if you run it on an under powered computer.
+The following shows what might happen in that situation.
+
+![screenshot](images/px4_debugging.png)
+
+In this flight we ran a simple `commander takeoff` test as performed
+by `PythonClient/multirotor/stability_test.py` and the flight started off fine, but then went crazy at the end and the drone crashed.  So why is that?  What can the log file show?
+
+Here we've plotted the following 5 metrics:
+- `hil_gps.alt` - the simulated altitude sent from AirSim to PX4
+- `telemetry.update_rate` - the rate AirSim is performing the critical drone update loop in updates per second.
+- `telemetry.update_time` - the average time taken inside AirSim performing the critical drone update loop.
+- `telemetry.actuation_delay` - this is a very interesting metric measuring how long it takes PX4 to send back updated actuator controls message (motor power)
+- `actuator_controls.0` - the actuator controls signal from PX4 for the first rotor.
+
+What we see then with these metrics is that things started off nicely, with nice flat altitude, high update rate in the 275 to 300 fps range, and a nice low update time inside AirSim around 113 microseconds, and a nice low actuation delay in the round trip from PX4.  The actuator controls also stabilize quickly to a nice flat line.
+
+But then the update_time starts to climb, at the same time the actuation_delay climbs and we see a little tip in actuator_controls.
+This dip should not happen, the PX4 is panicking over loss of update rate but it recovers.
+
+But then we see actuator controls go crazy, a huge spike in actuation delay, and around this time we see a message from AirSim saying `lockstep disabled`.  A delay over 100 millisecond triggers AirSim into jumping out of lockstep mode and the PX4 goes nuts and the drone crashes.
+
+The button line is that if a simple `takeoff` cannot maintain steady smooth flight and you see these kinds of spikes and uneven update rates
+then it means you are running AirSim on a computer that does not have enough horsepower.
+
+This is what a simple takeoff and hover and land should look like:
+
+![nice](images/px4_nice.png)
+
+Here you see the `update_rate` sticking the target of 333 updates per second.
+You also see the `update_time` a nice flat 39 microseconds and the `actuator_delay`
+somewhere between 1.1 and 1.7 milliseconds, and the resulting `actuator_controls`
+a lovely flat line.
+
+

From cbdff5bf0231978e213220056ff1d4aba435568d Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 20 Apr 2021 13:52:19 -0700
Subject: [PATCH 18/37] fix install link

---
 docs/log_viewer.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/docs/log_viewer.md b/docs/log_viewer.md
index ef29de0c9c..90726273af 100644
--- a/docs/log_viewer.md
+++ b/docs/log_viewer.md
@@ -19,7 +19,7 @@ estimation catches up with reality, this is more visible after a bad crash.
 
 ### Installation
 
-If you can't build the LogViewer.sln, there is also a [click once installer](http://www.lovettsoftware.com/Downloads/Px4LogViewer/Px4LogViewer.application).
+If you can't build the LogViewer.sln, there is also a [click once installer](https://lovettsoftwarestorage.blob.core.windows.net/downloads/Px4LogViewer/Px4LogViewer.application).
 
 ### Configuration
 

From d48f5c0368729332489362b0c98db8ad28493b86 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 20 Apr 2021 13:53:51 -0700
Subject: [PATCH 19/37] add link to logging.

---
 docs/log_viewer.md | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/docs/log_viewer.md b/docs/log_viewer.md
index 90726273af..fae713f39d 100644
--- a/docs/log_viewer.md
+++ b/docs/log_viewer.md
@@ -25,3 +25,6 @@ If you can't build the LogViewer.sln, there is also a [click once installer](htt
 
 The magic port number 14388 can be configured in the simulator by editing the [settings.json file](settings.md).
 
+### Debugging
+
+See [PX4 Logging](px4_logging.md) for more information on how to use the LogViewer to debug situations you are setting.

From f7d2fea248524208d25e2ac9c3874b73a3d225c1 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Sat, 24 Apr 2021 12:28:12 -0700
Subject: [PATCH 20/37] fix drone server wsl2 connection.

---
 DroneServer/main.cpp | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/DroneServer/main.cpp b/DroneServer/main.cpp
index 1806ec60c4..79da40fd8d 100644
--- a/DroneServer/main.cpp
+++ b/DroneServer/main.cpp
@@ -38,7 +38,14 @@ int main(int argc, const char* argv[])
     Settings& settings = Settings::singleton().loadJSonFile(settings_full_filepath);
     Settings child;
     if (settings.isLoadSuccess()) {
-        settings.getChild("PX4", child);
+        Settings vehicles;
+        if (settings.hasKey("Vehicles")) {
+            settings.getChild("Vehicles", vehicles);
+            vehicles.getChild("PX4", child);
+        }
+        else {
+            settings.getChild("PX4", child);
+        }
 
         // allow json overrides on a per-vehicle basis.
         connection_info.sim_sysid = static_cast<uint8_t>(child.getInt("SimSysID", connection_info.sim_sysid));
@@ -71,6 +78,8 @@ int main(int argc, const char* argv[])
         connection_info.tcp_port = child.getInt("TcpPort", connection_info.tcp_port);
         connection_info.serial_port = child.getString("SerialPort", connection_info.serial_port);
         connection_info.baud_rate = child.getInt("SerialBaudRate", connection_info.baud_rate);
+        connection_info.model = child.getString("Model", connection_info.model);
+        connection_info.logs = child.getString("Logs", connection_info.logs);
 
     }
     else {

From ad5534a5f4bf444c2c7d20abb761ded4e8838b66 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Mon, 26 Apr 2021 17:58:17 -0700
Subject: [PATCH 21/37] CR feedback.

---
 AirLib/include/common/AirSimSettings.hpp      | 43 -------------------
 .../include/sensors/imu/ImuSimpleParams.hpp   |  4 +-
 .../sensors/lidar/LidarSimpleParams.hpp       | 41 ++++++++++--------
 3 files changed, 25 insertions(+), 63 deletions(-)

diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index b0b3245d18..37ee6374c5 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -213,24 +213,6 @@ struct AirSimSettings {
     };
 
     struct LidarSetting : SensorSetting {
-        // TODO: move this to use the "Settings settings" instead.
-
-        // shared defaults
-        uint number_of_channels = 16;
-        real_T range = 10000.0f / 100;                    // meters
-        uint points_per_second = 100000;
-        uint horizontal_rotation_frequency = 10;          // rotations/sec
-        float horizontal_FOV_start = 0;                   // degrees
-        float horizontal_FOV_end = 359;                   // degrees
-
-        // defaults specific to a mode
-        float vertical_FOV_upper = Utils::nan<float>();   // drones -15, car +10
-        float vertical_FOV_lower = Utils::nan<float>();   // drones -45, car -10
-        Vector3r position = VectorMath::nanVector();
-        Rotation rotation = Rotation::nanRotation();
-
-        bool draw_debug_points = false;
-        std::string data_frame = AirSimSettings::kVehicleInertialFrame;
     };
 
     struct VehicleSetting {
@@ -1202,24 +1184,6 @@ struct AirSimSettings {
         clock_speed = settings_json.getFloat("ClockSpeed", 1.0f);
     }
 
-    static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json)
-    {
-        lidar_setting.number_of_channels = settings_json.getInt("NumberOfChannels", lidar_setting.number_of_channels);
-        lidar_setting.range = settings_json.getFloat("Range", lidar_setting.range);
-        lidar_setting.points_per_second = settings_json.getInt("PointsPerSecond", lidar_setting.points_per_second);
-        lidar_setting.horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", lidar_setting.horizontal_rotation_frequency);
-        lidar_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", lidar_setting.draw_debug_points);
-        lidar_setting.data_frame = settings_json.getString("DataFrame", lidar_setting.data_frame);
-
-        lidar_setting.vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", lidar_setting.vertical_FOV_upper);
-        lidar_setting.vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", lidar_setting.vertical_FOV_lower);
-        lidar_setting.horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", lidar_setting.horizontal_FOV_start);
-        lidar_setting.horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", lidar_setting.horizontal_FOV_end);
-
-        lidar_setting.position = createVectorSetting(settings_json, lidar_setting.position);
-        lidar_setting.rotation = createRotationSetting(settings_json, lidar_setting.rotation);
-    }
-
     static std::shared_ptr<SensorSetting> createSensorSetting(
         SensorBase::SensorType sensor_type, const std::string& sensor_name,
         bool enabled)
@@ -1264,13 +1228,6 @@ struct AirSimSettings {
         // extracted there.  This way default values can be kept in one place.  For example, see the 
         // BarometerSimpleParams::initializeFromSettings method.
         sensor_setting->settings = settings_json;
-
-        // TODO: delete this when we move initializeLidarSetting to LidarSimpleParams.hpp 
-        switch (sensor_setting->sensor_type) {
-        case SensorBase::SensorType::Lidar:
-            initializeLidarSetting(*static_cast<LidarSetting*>(sensor_setting), settings_json);
-            break;
-        }
     }
 
     // creates and intializes sensor settings from json
diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
index 8dfb7529a8..78f8de4dc0 100644
--- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp
+++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
@@ -38,7 +38,7 @@ struct ImuSimpleParams {
         real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 
         //Bias Stability (tau = 800s)
         real_T tau = 800;
-        real_T bias_stability = 36.0f * 1E-6f * 9.80665f; //ug converted to m/s^2
+        real_T bias_stability = 36.0f * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
         Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
     } accel;
 
@@ -63,7 +63,7 @@ struct ImuSimpleParams {
         accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau);
         bias_stability = json.getFloat("AccelBiasStability", Utils::nan<float>());
         if (!std::isnan(bias_stability)) {
-            accel.bias_stability = bias_stability * 1E-6f * 9.80665f; //ug converted to m/s^2
+            accel.bias_stability = bias_stability * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
         }
     }
 };
diff --git a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
index c42bb59c7b..282f56a0ad 100644
--- a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
+++ b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
@@ -41,17 +41,18 @@ struct LidarSimpleParams {
     {
         std::string simmode_name = AirSimSettings::singleton().simmode_name;
 
-        number_of_channels = settings.number_of_channels;
-        range = settings.range;
-        points_per_second = settings.points_per_second;
-        horizontal_rotation_frequency = settings.horizontal_rotation_frequency;
+        const auto& settings_json = settings.settings;
+        number_of_channels = settings_json.getInt("NumberOfChannels", number_of_channels);
+        range = settings_json.getFloat("Range", range);
+        points_per_second = settings_json.getInt("PointsPerSecond", points_per_second);
+        horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", horizontal_rotation_frequency);
+        draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
+        data_frame = settings_json.getString("DataFrame", data_frame);
 
-        horizontal_FOV_start = settings.horizontal_FOV_start;
-        horizontal_FOV_end = settings.horizontal_FOV_end;
+        vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", Utils::nan<float>());
 
         // By default, for multirotors the lidars FOV point downwards;
         // for cars, the lidars FOV is more forward facing.
-        vertical_FOV_upper = settings.vertical_FOV_upper;
         if (std::isnan(vertical_FOV_upper)) {
             if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
                 vertical_FOV_upper = -15;
@@ -59,7 +60,8 @@ struct LidarSimpleParams {
                 vertical_FOV_upper = +10;
         }
 
-        vertical_FOV_lower = settings.vertical_FOV_lower;
+
+        vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", Utils::nan<float>());
         if (std::isnan(vertical_FOV_lower)) {
             if (simmode_name == AirSimSettings::kSimModeTypeMultirotor)
                 vertical_FOV_lower = -45;
@@ -67,7 +69,13 @@ struct LidarSimpleParams {
                 vertical_FOV_lower = -10;
         }
 
-        relative_pose.position = settings.position;
+
+        horizontal_FOV_start = settings_json.getFloat("HorizontalFOVStart", horizontal_FOV_start);
+        horizontal_FOV_end = settings_json.getFloat("HorizontalFOVEnd", horizontal_FOV_end);
+
+        relative_pose.position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
+        auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
+
         if (std::isnan(relative_pose.position.x()))
             relative_pose.position.x() = 0;
         if (std::isnan(relative_pose.position.y()))
@@ -80,16 +88,13 @@ struct LidarSimpleParams {
         }
 
         float pitch, roll, yaw;
-        pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
-        roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
-        yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
+        pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
+        roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
+        yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
         relative_pose.orientation = VectorMath::toQuaternion(
-            Utils::degreesToRadians(pitch),   //pitch - rotation around Y axis
-            Utils::degreesToRadians(roll),    //roll  - rotation around X axis
-            Utils::degreesToRadians(yaw));    //yaw   - rotation around Z axis
-           
-        draw_debug_points = settings.draw_debug_points;
-        data_frame = settings.data_frame;
+            Utils::degreesToRadians(pitch),   // pitch - rotation around Y axis
+            Utils::degreesToRadians(roll),    // roll  - rotation around X axis
+            Utils::degreesToRadians(yaw));    // yaw   - rotation around Z axis           
     }
 };
 

From 3d8f8b2644f9a96e4445d01ea2240f0e819caeda Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Mon, 26 Apr 2021 20:23:00 -0700
Subject: [PATCH 22/37] fix ros wrapper compile.

---
 .../include/airsim_ros_wrapper.h              |  43 ++--
 .../src/airsim_ros_wrapper.cpp                | 230 ++++++++----------
 2 files changed, 126 insertions(+), 147 deletions(-)

diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
index 9d51cae4b1..58aa4c2573 100644
--- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
+++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h
@@ -9,6 +9,7 @@ STRICT_MODE_ON
 #include "airsim_settings_parser.h"
 #include "common/AirSimSettings.hpp"
 #include "common/common_utils/FileSystem.hpp"
+#include "sensors/lidar/LidarSimpleParams.hpp"
 #include "ros/ros.h"
 #include "sensors/imu/ImuBase.hpp"
 #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
@@ -93,18 +94,18 @@ struct VelCmd
     msr::airlib::YawMode yaw_mode;
     std::string vehicle_name;
 
-    // VelCmd() : 
-    //     x(0), y(0), z(0), 
+    // VelCmd() :
+    //     x(0), y(0), z(0),
     //     vehicle_name("") {drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom;
     //             yaw_mode = msr::airlib::YawMode();};
 
-    // VelCmd(const double& x, const double& y, const double& z, 
-    //         msr::airlib::DrivetrainType drivetrain, 
+    // VelCmd(const double& x, const double& y, const double& z,
+    //         msr::airlib::DrivetrainType drivetrain,
     //         const msr::airlib::YawMode& yaw_mode,
-    //         const std::string& vehicle_name) : 
-    //     x(x), y(y), z(z), 
-    //     drivetrain(drivetrain), 
-    //     yaw_mode(yaw_mode), 
+    //         const std::string& vehicle_name) :
+    //     x(x), y(y), z(z),
+    //     drivetrain(drivetrain),
+    //     yaw_mode(yaw_mode),
     //     vehicle_name(vehicle_name) {};
 };
 
@@ -116,9 +117,9 @@ struct GimbalCmd
 
     // GimbalCmd() : vehicle_name(vehicle_name), camera_name(camera_name), target_quat(msr::airlib::Quaternionr(1,0,0,0)) {}
 
-    // GimbalCmd(const std::string& vehicle_name, 
-    //         const std::string& camera_name, 
-    //         const msr::airlib::Quaternionr& target_quat) : 
+    // GimbalCmd(const std::string& vehicle_name,
+    //         const std::string& camera_name,
+    //         const msr::airlib::Quaternionr& target_quat) :
     //         vehicle_name(vehicle_name), camera_name(camera_name), target_quat(target_quat) {};
 };
 
@@ -132,7 +133,7 @@ class AirsimROSWrapper
     };
 
     AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string & host_ip);
-    ~AirsimROSWrapper() {}; 
+    ~AirsimROSWrapper() {};
 
     void initialize_airsim();
     void initialize_ros();
@@ -163,17 +164,17 @@ class AirsimROSWrapper
         ros::Publisher global_gps_pub;
         ros::Publisher env_pub;
         airsim_ros_pkgs::Environment env_msg;
-        std::vector<SensorPublisher> sensor_pubs; 
+        std::vector<SensorPublisher> sensor_pubs;
         // handle lidar seperately for max performance as data is collected on its own thread/callback
         std::vector<SensorPublisher> lidar_pubs;
-        
+
         nav_msgs::Odometry curr_odom;
         sensor_msgs::NavSatFix gps_sensor_msg;
 
         std::vector<geometry_msgs::TransformStamped> static_tf_msg_vec;
 
         ros::Time stamp;
-        
+
 
         std::string odom_frame_id;
         /// Status
@@ -261,13 +262,13 @@ class AirsimROSWrapper
 
     sensor_msgs::ImagePtr get_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id);
     sensor_msgs::ImagePtr get_depth_img_msg_from_response(const ImageResponse& img_response, const ros::Time curr_ros_time, const std::string frame_id);
-    
+
     void process_and_publish_img_response(const std::vector<ImageResponse>& img_response_vec, const int img_response_idx, const std::string& vehicle_name);
 
     // methods which parse setting json ang generate ros pubsubsrv
     void create_ros_pubs_from_settings_json();
     void append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting);
-    void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting);
+    void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting);
     void append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting);
     void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const;
     void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const;
@@ -316,7 +317,7 @@ class AirsimROSWrapper
 
     ros::ServiceServer reset_srvr_;
     ros::Publisher origin_geo_point_pub_; // home geo coord of drones
-    msr::airlib::GeoPoint origin_geo_point_;// gps coord of unreal origin 
+    msr::airlib::GeoPoint origin_geo_point_;// gps coord of unreal origin
     airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate
 
     AirSimSettingsParser airsim_settings_parser_;
@@ -343,7 +344,7 @@ class AirsimROSWrapper
 
     // gimbal control
     bool has_gimbal_cmd_;
-    GimbalCmd gimbal_cmd_; 
+    GimbalCmd gimbal_cmd_;
 
     /// ROS tf
     const std::string AIRSIM_FRAME_ID = "world_ned";
@@ -353,7 +354,7 @@ class AirsimROSWrapper
     std::string odom_frame_id_ = AIRSIM_ODOM_FRAME_ID;
     tf2_ros::TransformBroadcaster tf_broadcaster_;
     tf2_ros::StaticTransformBroadcaster static_tf_pub_;
-    
+
     bool isENU_ = false;
     tf2_ros::Buffer tf_buffer_;
     tf2_ros::TransformListener tf_listener_;
@@ -368,7 +369,7 @@ class AirsimROSWrapper
 
     typedef std::pair<std::vector<ImageRequest>, std::string> airsim_img_request_vehicle_name_pair;
     std::vector<airsim_img_request_vehicle_name_pair> airsim_img_request_vehicle_name_pair_vec_;
-    std::vector<image_transport::Publisher> image_pub_vec_; 
+    std::vector<image_transport::Publisher> image_pub_vec_;
     std::vector<ros::Publisher> cam_info_pub_vec_;
 
     std::vector<sensor_msgs::CameraInfo> camera_info_msg_vec_;
diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
index 855dae597e..94aca385cc 100644
--- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
+++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp
@@ -26,10 +26,10 @@ const std::unordered_map<int, std::string> AirsimROSWrapper::image_type_int_to_s
     { 7, "Infrared" }
 };
 
-AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) : 
-    nh_(nh), 
+AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) :
+    nh_(nh),
     nh_private_(nh_private),
-    img_async_spinner_(1, &img_timer_cb_queue_), // a thread for image callbacks to be 'spun' by img_async_spinner_ 
+    img_async_spinner_(1, &img_timer_cb_queue_), // a thread for image callbacks to be 'spun' by img_async_spinner_
     lidar_async_spinner_(1, &lidar_timer_cb_queue_), // same as above, but for lidar
     host_ip_(host_ip),
     airsim_client_images_(host_ip),
@@ -51,7 +51,7 @@ AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHan
         airsim_mode_ = AIRSIM_MODE::CAR;
         ROS_INFO("Setting ROS wrapper to CAR mode");
     }
-    
+
     initialize_ros();
 
     std::cout << "AirsimROSWrapper Initialized!\n";
@@ -62,7 +62,7 @@ void AirsimROSWrapper::initialize_airsim()
     // todo do not reset if already in air?
     try
     {
-        
+
         if (airsim_mode_ == AIRSIM_MODE::DRONE)
         {
             airsim_client_ = std::unique_ptr<msr::airlib::RpcLibClientBase>(new msr::airlib::MultirotorRpcLibClient(host_ip_));
@@ -82,7 +82,7 @@ void AirsimROSWrapper::initialize_airsim()
         }
 
         origin_geo_point_ = airsim_client_->getHomeGeoPoint("");
-        // todo there's only one global origin geopoint for environment. but airsim API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a geopoint being assigned in the constructor. by? 
+        // todo there's only one global origin geopoint for environment. but airsim API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a geopoint being assigned in the constructor. by?
         origin_geo_point_msg_ = get_gps_msg_from_airsim_geo_point(origin_geo_point_);
     }
     catch (rpc::rpc_error&  e)
@@ -120,7 +120,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
     // subscribe to control commands on global nodehandle
     gimbal_angle_quat_cmd_sub_ = nh_private_.subscribe("gimbal_angle_quat_cmd", 50, &AirsimROSWrapper::gimbal_angle_quat_cmd_cb, this);
     gimbal_angle_euler_cmd_sub_ = nh_private_.subscribe("gimbal_angle_euler_cmd", 50, &AirsimROSWrapper::gimbal_angle_euler_cmd_cb, this);
-    origin_geo_point_pub_ = nh_private_.advertise<airsim_ros_pkgs::GPSYaw>("origin_geo_point", 10);       
+    origin_geo_point_pub_ = nh_private_.advertise<airsim_ros_pkgs::GPSYaw>("origin_geo_point", 10);
 
     airsim_img_request_vehicle_name_pair_vec_.clear();
     image_pub_vec_.clear();
@@ -137,9 +137,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         auto& vehicle_setting = curr_vehicle_elem.second;
         auto curr_vehicle_name = curr_vehicle_elem.first;
         set_nans_to_zeros_in_pose(*vehicle_setting);
-       
+
         std::unique_ptr<VehicleROS> vehicle_ros = nullptr;
-        
+
         if (airsim_mode_ == AIRSIM_MODE::DRONE)
         {
             vehicle_ros = std::unique_ptr<MultiRotorROS>(new MultiRotorROS());
@@ -153,7 +153,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         vehicle_ros->vehicle_name = curr_vehicle_name;
 
         append_static_vehicle_tf(vehicle_ros.get(), *vehicle_setting);
-        
+
         vehicle_ros->odom_local_pub = nh_private_.advertise<nav_msgs::Odometry>(curr_vehicle_name + "/" + odom_frame_id_, 10);
 
         vehicle_ros->env_pub = nh_private_.advertise<airsim_ros_pkgs::Environment>(curr_vehicle_name + "/environment", 10);
@@ -163,17 +163,17 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
         if (airsim_mode_ == AIRSIM_MODE::DRONE)
         {
             auto drone = static_cast<MultiRotorROS*>(vehicle_ros.get());
-            
+
             // bind to a single callback. todo optimal subs queue length
-            // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument 
-            drone->vel_cmd_body_frame_sub = nh_private_.subscribe<airsim_ros_pkgs::VelCmd>(curr_vehicle_name + "/vel_cmd_body_frame", 1, 
+            // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument
+            drone->vel_cmd_body_frame_sub = nh_private_.subscribe<airsim_ros_pkgs::VelCmd>(curr_vehicle_name + "/vel_cmd_body_frame", 1,
                 boost::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name)); // todo ros::TransportHints().tcpNoDelay();
-            drone->vel_cmd_world_frame_sub = nh_private_.subscribe<airsim_ros_pkgs::VelCmd>(curr_vehicle_name + "/vel_cmd_world_frame", 1, 
+            drone->vel_cmd_world_frame_sub = nh_private_.subscribe<airsim_ros_pkgs::VelCmd>(curr_vehicle_name + "/vel_cmd_world_frame", 1,
                 boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name));
 
-            drone->takeoff_srvr = nh_private_.advertiseService<airsim_ros_pkgs::Takeoff::Request, airsim_ros_pkgs::Takeoff::Response>(curr_vehicle_name + "/takeoff", 
+            drone->takeoff_srvr = nh_private_.advertiseService<airsim_ros_pkgs::Takeoff::Request, airsim_ros_pkgs::Takeoff::Response>(curr_vehicle_name + "/takeoff",
                 boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name) );
-            drone->land_srvr = nh_private_.advertiseService<airsim_ros_pkgs::Land::Request, airsim_ros_pkgs::Land::Response>(curr_vehicle_name + "/land", 
+            drone->land_srvr = nh_private_.advertiseService<airsim_ros_pkgs::Land::Request, airsim_ros_pkgs::Land::Response>(curr_vehicle_name + "/land",
                 boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name) );
             // vehicle_ros.reset_srvr = nh_private_.advertiseService(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this);
         }
@@ -223,7 +223,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
                     camera_info_msg_vec_.push_back(generate_cam_info(curr_camera_name, camera_setting, capture_setting));
                 }
             }
-            // push back pair (vector of image captures, current vehicle name) 
+            // push back pair (vector of image captures, current vehicle name)
             airsim_img_request_vehicle_name_pair_vec_.push_back(std::make_pair(current_image_request_vec, curr_vehicle_name));
 
         }
@@ -236,7 +236,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
             auto& sensor_setting = curr_sensor_map.second;
 
             if (sensor_setting->enabled)
-            {             
+            {
                 SensorPublisher sensor_publisher;
                 sensor_publisher.sensor_name = sensor_setting->sensor_name;
                 sensor_publisher.sensor_type = sensor_setting->sensor_type;
@@ -244,7 +244,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
                 {
                     case SensorBase::SensorType::Barometer:
                     {
-                        std::cout << "Barometer" << std::endl; 
+                        std::cout << "Barometer" << std::endl;
                         sensor_publisher.publisher = nh_private_.advertise<airsim_ros_pkgs::Altimeter>(curr_vehicle_name + "/altimeter/" + sensor_name, 10);
                         break;
                     }
@@ -256,19 +256,19 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
                     }
                     case SensorBase::SensorType::Gps:
                     {
-                        std::cout << "Gps" << std::endl; 
+                        std::cout << "Gps" << std::endl;
                         sensor_publisher.publisher = nh_private_.advertise<sensor_msgs::NavSatFix>(curr_vehicle_name + "/gps/" + sensor_name, 10);
                         break;
                     }
                     case SensorBase::SensorType::Magnetometer:
                     {
-                        std::cout << "Magnetometer" << std::endl; 
+                        std::cout << "Magnetometer" << std::endl;
                         sensor_publisher.publisher = nh_private_.advertise<sensor_msgs::MagneticField>(curr_vehicle_name + "/magnetometer/" + sensor_name, 10);
                         break;
                     }
                     case SensorBase::SensorType::Distance:
                     {
-                        std::cout << "Distance" << std::endl; 
+                        std::cout << "Distance" << std::endl;
                         sensor_publisher.publisher = nh_private_.advertise<sensor_msgs::Range>(curr_vehicle_name + "/distance/" + sensor_name, 10);
                         break;
                     }
@@ -276,8 +276,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
                     {
                         std::cout << "Lidar" << std::endl;
                         auto lidar_setting = *static_cast<LidarSetting*>(sensor_setting.get());
-                        set_nans_to_zeros_in_pose(*vehicle_setting, lidar_setting);
-                        append_static_lidar_tf(vehicle_ros.get(), sensor_name, lidar_setting);
+                        msr::airlib::LidarSimpleParams params;
+                        params.initializeFromSettings(lidar_setting);
+                        append_static_lidar_tf(vehicle_ros.get(), sensor_name, params);
                         sensor_publisher.publisher = nh_private_.advertise<sensor_msgs::PointCloud2>(curr_vehicle_name + "/lidar/" + sensor_name, 10);
                         break;
                     }
@@ -338,34 +339,34 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json()
 
         ros::TimerOptions timer_options(ros::Duration(update_airsim_img_response_every_n_sec), boost::bind(&AirsimROSWrapper::img_response_timer_cb, this, _1), &img_timer_cb_queue_);
         airsim_img_response_timer_ = nh_private_.createTimer(timer_options);
-        is_used_img_timer_cb_queue_ = true;        
+        is_used_img_timer_cb_queue_ = true;
     }
 
     // lidars update on their own callback/thread at a given rate
     if (lidar_cnt > 0)
-    {    
+    {
         double update_lidar_every_n_sec;
         nh_private_.getParam("update_lidar_every_n_sec", update_lidar_every_n_sec);
         // nh_private_.setCallbackQueue(&lidar_timer_cb_queue_);
         ros::TimerOptions timer_options(ros::Duration(update_lidar_every_n_sec), boost::bind(&AirsimROSWrapper::lidar_timer_cb, this, _1), &lidar_timer_cb_queue_);
-        airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options);            
+        airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options);
         is_used_lidar_timer_cb_queue_ = true;
     }
 
     initialize_airsim();
 }
 
-// todo: error check. if state is not landed, return error. 
+// todo: error check. if state is not landed, return error.
 bool AirsimROSWrapper::takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name)
 {
     std::lock_guard<std::mutex> guard(drone_control_mutex_);
 
     if (request.waitOnLastTask)
-        static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? 
-        // response.success = 
+        static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec?
+        // response.success =
     else
         static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name);
-        // response.success = 
+        // response.success =
 
     return true;
 }
@@ -376,13 +377,13 @@ bool AirsimROSWrapper::takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Reque
 
     if (request.waitOnLastTask)
         for(const auto& vehicle_name : request.vehicle_names)
-            static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? 
-        // response.success = 
+            static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec?
+        // response.success =
     else
         for(const auto& vehicle_name : request.vehicle_names)
             static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name);
-        // response.success = 
-    
+        // response.success =
+
     return true;
 }
 
@@ -392,12 +393,12 @@ bool AirsimROSWrapper::takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& req
 
     if (request.waitOnLastTask)
         for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_)
-            static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? 
-        // response.success = 
+            static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec?
+        // response.success =
     else
         for(const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_)
             static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first);
-        // response.success = 
+        // response.success =
 
     return true;
 }
@@ -459,12 +460,12 @@ tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& a
 
 msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const geometry_msgs::Quaternion& geometry_msgs_quat) const
 {
-    return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z); 
+    return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z);
 }
 
 msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion& tf2_quat) const
 {
-    return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); 
+    return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z());
 }
 
 void AirsimROSWrapper::car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& msg, const std::string& vehicle_name)
@@ -494,7 +495,7 @@ void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::Cons
     std::lock_guard<std::mutex> guard(drone_control_mutex_);
 
     auto drone = static_cast<MultiRotorROS*>(vehicle_name_ptr_map_[vehicle_name].get());
-    
+
     double roll, pitch, yaw;
     tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw
 
@@ -519,7 +520,7 @@ void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmd
 
         double roll, pitch, yaw;
         tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw
-        
+
         // todo do actual body frame?
         drone->vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll
         drone->vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame
@@ -572,7 +573,7 @@ void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::Con
     drone->has_vel_cmd = true;
 }
 
-// this is kinda unnecessary but maybe it makes life easier for the end user. 
+// this is kinda unnecessary but maybe it makes life easier for the end user.
 void AirsimROSWrapper::vel_cmd_group_world_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg)
 {
     std::lock_guard<std::mutex> guard(drone_control_mutex_);
@@ -621,7 +622,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng
         gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); // airsim uses wxyz
         gimbal_cmd_.camera_name = gimbal_angle_quat_cmd_msg.camera_name;
         gimbal_cmd_.vehicle_name = gimbal_angle_quat_cmd_msg.vehicle_name;
-        has_gimbal_cmd_ = true; 
+        has_gimbal_cmd_ = true;
     }
     catch (tf2::TransformException& ex)
     {
@@ -632,7 +633,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng
 // todo support multiple gimbal commands
 // 1. find quaternion of default gimbal pose
 // 2. forward multiply with quaternion equivalent to desired euler commands (in degrees)
-// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo 
+// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo
 void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg)
 {
     try
@@ -643,7 +644,7 @@ void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAn
         gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd);
         gimbal_cmd_.camera_name = gimbal_angle_euler_cmd_msg.camera_name;
         gimbal_cmd_.vehicle_name = gimbal_angle_euler_cmd_msg.vehicle_name;
-        has_gimbal_cmd_ = true; 
+        has_gimbal_cmd_ = true;
     }
     catch (tf2::TransformException& ex)
     {
@@ -691,7 +692,7 @@ nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airl
     {
         std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);
         odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z;
-        std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y);        
+        std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y);
         odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z;
         std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y);
         odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z;
@@ -725,7 +726,7 @@ nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const ms
     {
         std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y);
         odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z;
-        std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y);        
+        std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y);
         odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z;
         std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y);
         odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z;
@@ -751,8 +752,8 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::
         lidar_msg.width = lidar_data.point_cloud.size() / 3;
 
         lidar_msg.fields.resize(3);
-        lidar_msg.fields[0].name = "x"; 
-        lidar_msg.fields[1].name = "y"; 
+        lidar_msg.fields[0].name = "x";
+        lidar_msg.fields[1].name = "y";
         lidar_msg.fields[2].name = "z";
 
         int offset = 0;
@@ -828,8 +829,8 @@ sensor_msgs::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr::
     mag_msg.magnetic_field.x = mag_data.magnetic_field_body.x();
     mag_msg.magnetic_field.y = mag_data.magnetic_field_body.y();
     mag_msg.magnetic_field.z = mag_data.magnetic_field_body.z();
-    std::copy(std::begin(mag_data.magnetic_field_covariance), 
-        std::end(mag_data.magnetic_field_covariance), 
+    std::copy(std::begin(mag_data.magnetic_field_covariance),
+        std::end(mag_data.magnetic_field_covariance),
         std::begin(mag_msg.magnetic_field_covariance));
      mag_msg.header.stamp = airsim_timestamp_to_ros(mag_data.time_stamp);
 
@@ -842,12 +843,12 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airl
     sensor_msgs::NavSatFix gps_msg;
     gps_msg.header.stamp = airsim_timestamp_to_ros(gps_data.time_stamp);
     gps_msg.latitude = gps_data.gnss.geo_point.latitude;
-    gps_msg.longitude = gps_data.gnss.geo_point.longitude; 
+    gps_msg.longitude = gps_data.gnss.geo_point.longitude;
     gps_msg.altitude = gps_data.gnss.geo_point.altitude;
     gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GLONASS;
     gps_msg.status.status = gps_data.gnss.fix_type;
-    // gps_msg.position_covariance_type = 
-    // gps_msg.position_covariance = 
+    // gps_msg.position_covariance_type =
+    // gps_msg.position_covariance =
 
     return gps_msg;
 }
@@ -890,7 +891,7 @@ sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::Im
     imu_msg.angular_velocity.y = imu_data.angular_velocity.y();
     imu_msg.angular_velocity.z = imu_data.angular_velocity.z();
 
-    // meters/s2^m 
+    // meters/s2^m
     imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x();
     imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y();
     imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z();
@@ -906,7 +907,7 @@ void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_msg)
 {
     geometry_msgs::TransformStamped odom_tf;
     odom_tf.header = odom_msg.header;
-    odom_tf.child_frame_id = odom_msg.child_frame_id; 
+    odom_tf.child_frame_id = odom_msg.child_frame_id;
     odom_tf.transform.translation.x = odom_msg.pose.pose.position.x;
     odom_tf.transform.translation.y = odom_msg.pose.pose.position.y;
     odom_tf.transform.translation.z = odom_msg.pose.pose.position.z;
@@ -921,7 +922,7 @@ airsim_ros_pkgs::GPSYaw AirsimROSWrapper::get_gps_msg_from_airsim_geo_point(cons
 {
     airsim_ros_pkgs::GPSYaw gps_msg;
     gps_msg.latitude = geo_point.latitude;
-    gps_msg.longitude = geo_point.longitude; 
+    gps_msg.longitude = geo_point.longitude;
     gps_msg.altitude = geo_point.altitude;
     return gps_msg;
 }
@@ -930,7 +931,7 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_poin
 {
     sensor_msgs::NavSatFix gps_msg;
     gps_msg.latitude = geo_point.latitude;
-    gps_msg.longitude = geo_point.longitude; 
+    gps_msg.longitude = geo_point.longitude;
     gps_msg.altitude = geo_point.altitude;
     return gps_msg;
 }
@@ -955,7 +956,7 @@ ros::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoin
 void AirsimROSWrapper::drone_state_timer_cb(const ros::TimerEvent& event)
 {
     try
-    { 
+    {
         // todo this is global origin
         origin_geo_point_pub_.publish(origin_geo_point_msg_);
 
@@ -1013,7 +1014,7 @@ ros::Time AirsimROSWrapper::update_state()
     for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_)
     {
         ros::Time vehicle_time;
-        // get drone state from airsim 
+        // get drone state from airsim
         auto& vehicle_ros = vehicle_name_ptr_pair.second;
 
         // vehicle environment, we can get ambient temperature here and other truths
@@ -1054,20 +1055,20 @@ ros::Time AirsimROSWrapper::update_state()
             vehicle_ros->gps_sensor_msg.header.stamp = vehicle_time;
 
             vehicle_ros->curr_odom = get_odom_msg_from_car_state(car->curr_car_state);
-            
+
             airsim_ros_pkgs::CarState state_msg = get_roscarstate_msg_from_car_state(car->curr_car_state);
             state_msg.header.frame_id = vehicle_ros->vehicle_name;
             car->car_state_msg = state_msg;
         }
 
         vehicle_ros->stamp = vehicle_time;
-        
+
         airsim_ros_pkgs::Environment env_msg = get_environment_msg_from_airsim(env_data);
         env_msg.header.frame_id = vehicle_ros->vehicle_name;
         env_msg.header.stamp = vehicle_time;
         vehicle_ros->env_msg = env_msg;
 
-        // convert airsim drone state to ROS msgs            
+        // convert airsim drone state to ROS msgs
         vehicle_ros->curr_odom.header.frame_id = vehicle_ros->vehicle_name;
         vehicle_ros->curr_odom.child_frame_id = vehicle_ros->odom_frame_id;
         vehicle_ros->curr_odom.header.stamp = vehicle_time;
@@ -1108,7 +1109,7 @@ void AirsimROSWrapper::publish_vehicle_state()
                     auto baro_data = airsim_client_->getBarometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name);
                     airsim_ros_pkgs::Altimeter alt_msg = get_altimeter_msg_from_airsim(baro_data);
                     alt_msg.header.frame_id = vehicle_ros->vehicle_name;
-                    sensor_publisher.publisher.publish(alt_msg);                        
+                    sensor_publisher.publisher.publish(alt_msg);
                     break;
                 }
                 case SensorBase::SensorType::Imu:
@@ -1169,7 +1170,7 @@ void AirsimROSWrapper::update_commands()
             if (drone->has_vel_cmd)
             {
                 std::lock_guard<std::mutex> guard(drone_control_mutex_);
-                static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd.x, drone->vel_cmd.y, drone->vel_cmd.z, vel_cmd_duration_, 
+                static_cast<msr::airlib::MultirotorRpcLibClient*>(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd.x, drone->vel_cmd.y, drone->vel_cmd.z, vel_cmd_duration_,
                     msr::airlib::DrivetrainType::MaxDegreeOfFreedom, drone->vel_cmd.yaw_mode, drone->vehicle_name);
             }
             drone->has_vel_cmd = false;
@@ -1187,7 +1188,7 @@ void AirsimROSWrapper::update_commands()
         }
     }
 
-    // Only camera rotation, no translation movement of camera 
+    // Only camera rotation, no translation movement of camera
     if (has_gimbal_cmd_)
     {
         std::lock_guard<std::mutex> guard(drone_control_mutex_);
@@ -1197,7 +1198,7 @@ void AirsimROSWrapper::update_commands()
     has_gimbal_cmd_ = false;
 }
 
-// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS 
+// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS
 void AirsimROSWrapper::set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const
 {
     if (std::isnan(vehicle_setting.position.x()))
@@ -1242,27 +1243,6 @@ void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_s
 
 }
 
-void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const
-{
-    if (std::isnan(lidar_setting.position.x()))
-        lidar_setting.position.x() = vehicle_setting.position.x();
-
-    if (std::isnan(lidar_setting.position.y()))
-        lidar_setting.position.y() = vehicle_setting.position.y();
-
-    if (std::isnan(lidar_setting.position.z()))
-        lidar_setting.position.z() = vehicle_setting.position.z();
-
-    if (std::isnan(lidar_setting.rotation.yaw))
-        lidar_setting.rotation.yaw = vehicle_setting.rotation.yaw;
-
-    if (std::isnan(lidar_setting.rotation.pitch))
-        lidar_setting.rotation.pitch = vehicle_setting.rotation.pitch;
-
-    if (std::isnan(lidar_setting.rotation.roll))
-        lidar_setting.rotation.roll = vehicle_setting.rotation.roll;
-}
-
 void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting)
 {
     geometry_msgs::TransformStamped vehicle_tf_msg;
@@ -1271,7 +1251,7 @@ void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const V
     vehicle_tf_msg.child_frame_id = vehicle_ros->vehicle_name;
     vehicle_tf_msg.transform.translation.x = vehicle_setting.position.x();
     vehicle_tf_msg.transform.translation.y = vehicle_setting.position.y();
-    vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z();    
+    vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z();
     tf2::Quaternion quat;
     quat.setRPY(vehicle_setting.rotation.roll, vehicle_setting.rotation.pitch, vehicle_setting.rotation.yaw);
     vehicle_tf_msg.transform.rotation.x = quat.x();
@@ -1290,20 +1270,18 @@ void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const V
     vehicle_ros->static_tf_msg_vec.emplace_back(vehicle_tf_msg);
 }
 
-void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting)
+void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting)
 {
     geometry_msgs::TransformStamped lidar_tf_msg;
     lidar_tf_msg.header.frame_id = vehicle_ros->vehicle_name + "/" + odom_frame_id_;
     lidar_tf_msg.child_frame_id = vehicle_ros->vehicle_name + "/" + lidar_name;
-    lidar_tf_msg.transform.translation.x = lidar_setting.position.x();
-    lidar_tf_msg.transform.translation.y = lidar_setting.position.y();
-    lidar_tf_msg.transform.translation.z = lidar_setting.position.z();
-    tf2::Quaternion quat;
-    quat.setRPY(lidar_setting.rotation.roll, lidar_setting.rotation.pitch, lidar_setting.rotation.yaw);
-    lidar_tf_msg.transform.rotation.x = quat.x();
-    lidar_tf_msg.transform.rotation.y = quat.y();
-    lidar_tf_msg.transform.rotation.z = quat.z();
-    lidar_tf_msg.transform.rotation.w = quat.w();
+    lidar_tf_msg.transform.translation.x = lidar_setting.relative_pose.position.x();
+    lidar_tf_msg.transform.translation.y = lidar_setting.relative_pose.position.y();
+    lidar_tf_msg.transform.translation.z = lidar_setting.relative_pose.position.z();
+    lidar_tf_msg.transform.rotation.x = lidar_setting.relative_pose.orientation.x();
+    lidar_tf_msg.transform.rotation.y = lidar_setting.relative_pose.orientation.y();
+    lidar_tf_msg.transform.rotation.z = lidar_setting.relative_pose.orientation.z();
+    lidar_tf_msg.transform.rotation.w = lidar_setting.relative_pose.orientation.w();
 
     if (isENU_)
     {
@@ -1345,11 +1323,11 @@ void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const st
     tf2::Quaternion quat_cam_body;
     tf2::Quaternion quat_cam_optical;
     tf2::convert(static_cam_tf_body_msg.transform.rotation, quat_cam_body);
-    tf2::Matrix3x3 mat_cam_body(quat_cam_body); 
+    tf2::Matrix3x3 mat_cam_body(quat_cam_body);
     tf2::Matrix3x3 mat_cam_optical;
-    mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), 
+    mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(),
                              mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(),
-                             mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); 
+                             mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ());
     mat_cam_optical.getRotation(quat_cam_optical);
     quat_cam_optical.normalize();
     tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation);
@@ -1359,7 +1337,7 @@ void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const st
 }
 
 void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event)
-{    
+{
     try
     {
         int image_response_idx = 0;
@@ -1367,11 +1345,11 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event)
         {
             const std::vector<ImageResponse>& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second);
 
-            if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) 
+            if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size())
             {
                 process_and_publish_img_response(img_response, image_response_idx, airsim_img_request_vehicle_name_pair.second);
                 image_response_idx += img_response.size();
-            }            
+            }
         }
     }
 
@@ -1384,7 +1362,7 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event)
 }
 
 void AirsimROSWrapper::lidar_timer_cb(const ros::TimerEvent& event)
-{    
+{
     try
     {
         for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_)
@@ -1420,7 +1398,7 @@ cv::Mat AirsimROSWrapper::manual_decode_depth(const ImageResponse& img_response)
 }
 
 sensor_msgs::ImagePtr AirsimROSWrapper::get_img_msg_from_response(const ImageResponse& img_response,
-                                                                const ros::Time curr_ros_time, 
+                                                                const ros::Time curr_ros_time,
                                                                 const std::string frame_id)
 {
     sensor_msgs::ImagePtr img_msg_ptr = boost::make_shared<sensor_msgs::Image>();
@@ -1441,7 +1419,7 @@ sensor_msgs::ImagePtr AirsimROSWrapper::get_depth_img_msg_from_response(const Im
                                                                         const ros::Time curr_ros_time,
                                                                         const std::string frame_id)
 {
-    // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error, 
+    // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error,
     // hence the dependency on opencv and cv_bridge. however, this is an extremely fast op, so no big deal.
     cv::Mat depth_img = manual_decode_depth(img_response);
     sensor_msgs::ImagePtr depth_img_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg();
@@ -1462,28 +1440,28 @@ sensor_msgs::CameraInfo AirsimROSWrapper::generate_cam_info(const std::string& c
     float f_x = (capture_setting.width / 2.0) / tan(math_common::deg2rad(capture_setting.fov_degrees / 2.0));
     // todo focal length in Y direction should be same as X it seems. this can change in future a scene capture component which exactly correponds to a cine camera
     // float f_y = (capture_setting.height / 2.0) / tan(math_common::deg2rad(fov_degrees / 2.0));
-    cam_info_msg.K = {f_x, 0.0, capture_setting.width / 2.0, 
-                        0.0, f_x, capture_setting.height / 2.0, 
+    cam_info_msg.K = {f_x, 0.0, capture_setting.width / 2.0,
+                        0.0, f_x, capture_setting.height / 2.0,
                         0.0, 0.0, 1.0};
     cam_info_msg.P = {f_x, 0.0, capture_setting.width / 2.0, 0.0,
-                        0.0, f_x, capture_setting.height / 2.0, 0.0, 
+                        0.0, f_x, capture_setting.height / 2.0, 0.0,
                         0.0, 0.0, 1.0, 0.0};
     return cam_info_msg;
 }
 
 void AirsimROSWrapper::process_and_publish_img_response(const std::vector<ImageResponse>& img_response_vec, const int img_response_idx, const std::string& vehicle_name)
-{    
+{
     // todo add option to use airsim time (image_response.TTimePoint) like Gazebo /use_sim_time param
-    ros::Time curr_ros_time = ros::Time::now(); 
+    ros::Time curr_ros_time = ros::Time::now();
     int img_response_idx_internal = img_response_idx;
 
     for (const auto& curr_img_response : img_response_vec)
     {
-        // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope. 
-        // Ideally, we should loop over cameras and then captures, and publish only one tf.  
+        // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope.
+        // Ideally, we should loop over cameras and then captures, and publish only one tf.
         publish_camera_tf(curr_img_response, curr_ros_time, vehicle_name, curr_img_response.camera_name);
 
-        // todo simGetCameraInfo is wrong + also it's only for image type -1.  
+        // todo simGetCameraInfo is wrong + also it's only for image type -1.
         // msr::airlib::CameraInfo camera_info = airsim_client_.simGetCameraInfo(curr_img_response.camera_name);
 
         // update timestamp of saved cam info msgs
@@ -1493,15 +1471,15 @@ void AirsimROSWrapper::process_and_publish_img_response(const std::vector<ImageR
         // DepthPlanar / DepthPerspective / DepthVis / DisparityNormalized
         if (curr_img_response.pixels_as_float)
         {
-            image_pub_vec_[img_response_idx_internal].publish(get_depth_img_msg_from_response(curr_img_response, 
-                                                    curr_ros_time, 
+            image_pub_vec_[img_response_idx_internal].publish(get_depth_img_msg_from_response(curr_img_response,
+                                                    curr_ros_time,
                                                     curr_img_response.camera_name + "_optical"));
         }
         // Scene / Segmentation / SurfaceNormals / Infrared
         else
         {
-            image_pub_vec_[img_response_idx_internal].publish(get_img_msg_from_response(curr_img_response, 
-                                                    curr_ros_time, 
+            image_pub_vec_[img_response_idx_internal].publish(get_img_msg_from_response(curr_img_response,
+                                                    curr_ros_time,
                                                     curr_img_response.camera_name + "_optical"));
         }
         img_response_idx_internal++;
@@ -1510,8 +1488,8 @@ void AirsimROSWrapper::process_and_publish_img_response(const std::vector<ImageR
 }
 
 // publish camera transforms
-// camera poses are obtained from airsim's client API which are in (local) NED frame. 
-// We first do a change of basis to camera optical frame (Z forward, X right, Y down) 
+// camera poses are obtained from airsim's client API which are in (local) NED frame.
+// We first do a change of basis to camera optical frame (Z forward, X right, Y down)
 void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, const ros::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id)
 {
     geometry_msgs::TransformStamped cam_tf_body_msg;
@@ -1545,13 +1523,13 @@ void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, cons
     tf2::Quaternion quat_cam_body;
     tf2::Quaternion quat_cam_optical;
     tf2::convert(cam_tf_body_msg.transform.rotation, quat_cam_body);
-    tf2::Matrix3x3 mat_cam_body(quat_cam_body); 
+    tf2::Matrix3x3 mat_cam_body(quat_cam_body);
     // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body * matrix_cam_body_to_optical_inverse_;
     // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body;
     tf2::Matrix3x3 mat_cam_optical;
-    mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), 
+    mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(),
                              mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(),
-                             mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); 
+                             mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ());
     mat_cam_optical.getRotation(quat_cam_optical);
     quat_cam_optical.normalize();
     tf2::convert(quat_cam_optical, cam_tf_optical_msg.transform.rotation);

From e894d3a21521d4df2a247f8bae20e108147a3493 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 27 Apr 2021 18:35:30 -0700
Subject: [PATCH 23/37] script to test flying up high

---
 PythonClient/multirotor/high.py | 52 +++++++++++++++++++++++++++++++++
 1 file changed, 52 insertions(+)
 create mode 100644 PythonClient/multirotor/high.py

diff --git a/PythonClient/multirotor/high.py b/PythonClient/multirotor/high.py
new file mode 100644
index 0000000000..afdcadff91
--- /dev/null
+++ b/PythonClient/multirotor/high.py
@@ -0,0 +1,52 @@
+import setup_path
+import airsim
+
+import sys
+import time
+
+print("""This script is designed to fly on the streets of the Neighborhood environment
+and assumes the unreal position of the drone is [160, -1500, 120].""")
+
+client = airsim.MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+
+print("arming the drone...")
+client.armDisarm(True)
+
+state = client.getMultirotorState()
+if state.landed_state == airsim.LandedState.Landed:
+    print("taking off...")
+    client.takeoffAsync().join()
+else:
+    client.hoverAsync().join()
+
+time.sleep(1)
+
+state = client.getMultirotorState()
+if state.landed_state == airsim.LandedState.Landed:
+    print("take off failed...")
+    sys.exit(1)
+
+# AirSim uses NED coordinates so negative axis is up.
+# z of -50 is 50 meters above the original launch point.
+z = -50
+print("make sure we are hovering at 50 meters...")
+client.moveToZAsync(z, 1).join()
+
+# see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo
+
+# this method is async and we are not waiting for the result since we are passing timeout_sec=0.
+
+time.sleep(1)
+
+# come down quickly
+z = -7
+client.moveToZAsync(z, 5).join()
+
+print("landing...")
+client.landAsync().join()
+print("disarming...")
+client.armDisarm(False)
+client.enableApiControl(False)
+print("done.")

From 0b81265bee0e028dec4ae1218cd4033376780c93 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Apr 2021 09:42:01 -0700
Subject: [PATCH 24/37] fix missing stopLoggingReceiveMessage

---
 .../multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index d2be5f7183..09eda7ad4c 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -368,11 +368,15 @@ namespace msr { namespace airlib {
                 auto con = mav_vehicle_->getConnection();
                 if (con != nullptr) {
                     con->stopLoggingSendMessage();
-                    addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str()));
+                    con->stopLoggingReceiveMessage();
+                }
+                if (connection_ != nullptr) {
+                    connection_->stopLoggingSendMessage();
+                    connection_->stopLoggingReceiveMessage();
                 }
-                connection_->stopLoggingSendMessage();
             }
             if (log_ != nullptr) {
+                addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str()));
                 log_->close();
                 log_ = nullptr;
             }

From 62b08da606f101b4c08972d96c4410aa1a5d514e Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Apr 2021 11:32:08 -0700
Subject: [PATCH 25/37] consistency with logviewer logging and add docs.

---
 .../mavlink/MavLinkMultirotorApi.hpp          | 34 ++++++++++++++++---
 docs/log_viewer.md                            | 10 ++++--
 docs/px4_logging.md                           |  4 ++-
 3 files changed, 39 insertions(+), 9 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 63385391fe..49b7105b6c 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -832,15 +832,30 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy) {
             proxy_ = proxy;
         }
+        ~MavLinkLogViewerLog() {
+            proxy_ = nullptr;
+        }
         void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override {
-            unused(timestamp);
-            mavlinkcom::MavLinkMessage copy;
-            ::memcpy(&copy, &msg, sizeof(mavlinkcom::MavLinkMessage));
-            proxy_->sendMessage(copy);
+            if (proxy_ != nullptr) {
+                unused(timestamp);
+                mavlinkcom::MavLinkMessage copy;
+                ::memcpy(&copy, &msg, sizeof(mavlinkcom::MavLinkMessage));
+                try {
+                    proxy_->sendMessage(copy);
+                }
+                catch (std::exception&) {
+                    failures++;
+                    if (failures == 100) {
+                        // hmmm, doesn't like the proxy, bad socket perhaps, so stop trying...
+                        proxy_ = nullptr;
+                    }
+                }
+            }
         }
 
     private:
         std::shared_ptr<mavlinkcom::MavLinkNode> proxy_;
+        int failures = 0;
     };
 
 
@@ -873,6 +888,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 setNormalMode();
             }
 
+            connection_->stopLoggingSendMessage();
+            connection_->stopLoggingReceiveMessage();
             connection_->close();
         }
 
@@ -884,6 +901,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             auto c = mav_vehicle_->getConnection();
             if (c != nullptr) {
                 c->stopLoggingSendMessage();
+                c->stopLoggingReceiveMessage();
             }
             mav_vehicle_->close();
             mav_vehicle_ = nullptr;
@@ -1139,7 +1157,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 logviewer_out_proxy_ = nullptr;
             }
             else if (mav_vehicle_ != nullptr) {
-                mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_));
+                auto proxylog = std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_);
+                mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog);
+                mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog);
+                if (connection_ != nullptr) {
+                    connection_->startLoggingSendMessage(proxylog);
+                    connection_->startLoggingReceiveMessage(proxylog);
+                }
             }
         }
         return logviewer_proxy_ != nullptr;
diff --git a/docs/log_viewer.md b/docs/log_viewer.md
index 61e17106f3..2f97249481 100644
--- a/docs/log_viewer.md
+++ b/docs/log_viewer.md
@@ -20,7 +20,7 @@ log files so you can compare the data from each.
 
 ### Realtime
 
-You can also get a realtime view if you connect the LogViewer `before` you run the simulation.  
+You can also get a realtime view if you connect the LogViewer `before` you run the simulation.
 
 ![connect](images/log_viewer_connect.png)
 
@@ -38,9 +38,13 @@ For this to work you need to configure the `settings.json` with the following se
     }
 }
 ```
+
+Note: do not use the "Logs" setting when you want realtime LogViewer logging.  Logging to
+a file using "Logs" is mutually exclusive. with LogViewer logging.
+
 Simply press the blue connector button on the top right corner of the window, select the Socket
-`tab`, enter the port number `14388`, and your `localhost` network.  If you are using WSL 2 on 
-Windows then select `vEthernet (WSL)`.  
+`tab`, enter the port number `14388`, and your `localhost` network.  If you are using WSL 2 on
+Windows then select `vEthernet (WSL)`.
 
 If you do choose `vEthernet (WSL)` then make sure you also set `LocalHostIp` and
 `LogViewerHostIp` to the matching WSL ethernet address, something like `172.31.64.1`.
diff --git a/docs/px4_logging.md b/docs/px4_logging.md
index e23b710ad8..68a81e7c3e 100644
--- a/docs/px4_logging.md
+++ b/docs/px4_logging.md
@@ -25,10 +25,12 @@ You will then see log files organized by date in d:\temp\logs, specifically *inp
 
 ## MavLink LogViewer
 For MavLink enabled drones, you can also use our [Log Viewer](log_viewer.md) to visualize the streams of data.
+If you enable this form of realtime logging you should not use the "Logs" setting above, these two forms of logging
+are mutually exclusive.
 
 ## PX4 Log in SITL Mode
 
-In SITL mode, please a log file is produced when drone is armed. The SITL terminal will contain the path to the log file, it should look something like this 
+In SITL mode, please a log file is produced when drone is armed. The SITL terminal will contain the path to the log file, it should look something like this
 ```
 INFO  [logger] Opened log file: rootfs/fs/microsd/log/2017-03-27/20_02_49.ulg
 ```

From 6db1f215eef16c6d8cbbcdc4fe8797850bbb44aa Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Apr 2021 14:26:49 -0700
Subject: [PATCH 26/37] Pause physics update loop while waiting for actuator
 controls during lockstep enabled mavlink multirotor update loop.

---
 AirLib/include/common/UpdatableContainer.hpp  | 19 +++++++++++---
 AirLib/include/common/UpdatableObject.hpp     | 19 +++++++++++++-
 AirLib/include/physics/FastPhysicsEngine.hpp  |  3 ++-
 AirLib/include/physics/PhysicsBody.hpp        |  2 ++
 AirLib/include/physics/PhysicsEngineBase.hpp  | 26 +------------------
 AirLib/include/physics/PhysicsWorld.hpp       |  5 +++-
 AirLib/include/physics/World.hpp              |  8 +++++-
 .../multirotor/MultiRotorPhysicsBody.hpp      |  2 ++
 .../mavlink/MavLinkMultirotorApi.hpp          | 17 ++++++++++++
 9 files changed, 68 insertions(+), 33 deletions(-)

diff --git a/AirLib/include/common/UpdatableContainer.hpp b/AirLib/include/common/UpdatableContainer.hpp
index b841d8f44b..e55168f530 100644
--- a/AirLib/include/common/UpdatableContainer.hpp
+++ b/AirLib/include/common/UpdatableContainer.hpp
@@ -24,10 +24,21 @@ class UpdatableContainer : public UpdatableObject {
     const TUpdatableObjectPtr& at(uint index) const { members_.at(index);  }
     TUpdatableObjectPtr& at(uint index) { return members_.at(index);  }
     //allow to override membership modifications
-    virtual void clear() { members_.clear(); }
-    virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member);  }
-    virtual void erase_remove(TUpdatableObjectPtr obj) { 
-        members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
+    virtual void clear() {
+        for (auto m : members_) {
+            m->setParent(nullptr);
+        }
+        members_.clear();
+    }
+    virtual void insert(TUpdatableObjectPtr member) {
+        member->setParent(this);
+        members_.push_back(member);
+    }
+    virtual void erase_remove(TUpdatableObjectPtr member) {
+        member->setParent(nullptr);
+        members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end());
+    }
+
 
 public:
     //*** Start: UpdatableState implementation ***//
diff --git a/AirLib/include/common/UpdatableObject.hpp b/AirLib/include/common/UpdatableObject.hpp
index 579f2350fb..620f54e7d1 100644
--- a/AirLib/include/common/UpdatableObject.hpp
+++ b/AirLib/include/common/UpdatableObject.hpp
@@ -24,7 +24,6 @@ Do not call reset() from constructor or initialization because that will produce
 init->reset calls for base-derived class that would be incorrect.
 */
 
-
 class UpdatableObject {
 public:
     void reset()
@@ -73,6 +72,22 @@ class UpdatableObject {
         return ClockFactory::get();
     }
 
+    UpdatableObject* getParent() {
+        return parent_;
+    }
+
+    void setParent(UpdatableObject* container) {
+        parent_ = container;
+    }
+
+    std::string getName(){
+        return name_;
+    }
+
+    void setName(const std::string& name) {
+        this->name_ = name;
+    }
+
 protected:
     virtual void resetImplementation() = 0;
     virtual void failResetUpdateOrdering(std::string err)
@@ -84,6 +99,8 @@ class UpdatableObject {
     bool reset_called = false;
     bool update_called = false;
     bool reset_in_progress = false;
+    UpdatableObject* parent_ = nullptr;
+    std::string name_;
 };
 
 }} //namespace
diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp
index ad962a00ab..035f89d0fc 100644
--- a/AirLib/include/physics/FastPhysicsEngine.hpp
+++ b/AirLib/include/physics/FastPhysicsEngine.hpp
@@ -20,7 +20,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 public:
     FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
         : enable_ground_lock_(enable_ground_lock), wind_(wind)
-    { 
+    {
+        setName("FastPhysicsEngine");
     }
 
     //*** Start: UpdatableState implementation ***//
diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp
index d2749259b3..a20daffc7c 100644
--- a/AirLib/include/physics/PhysicsBody.hpp
+++ b/AirLib/include/physics/PhysicsBody.hpp
@@ -84,7 +84,9 @@ class PhysicsBody : public UpdatableObject {
         inertia_ = inertia;
         inertia_inv_ = inertia_.inverse();
         environment_ = environment;
+        environment_->setParent(this);
         kinematics_ = kinematics;
+        kinematics_->setParent(this);
     }
 
     //enable physics body detection
diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp
index a8b09f28ba..dd06c3f829 100644
--- a/AirLib/include/physics/PhysicsEngineBase.hpp
+++ b/AirLib/include/physics/PhysicsEngineBase.hpp
@@ -10,7 +10,7 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsEngineBase : public UpdatableObject {
+class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*> {
 public:
     virtual void update() override
     {
@@ -23,31 +23,7 @@ class PhysicsEngineBase : public UpdatableObject {
         //default nothing to report for physics engine
     }
 
-    //TODO: reduce copy-past from UpdatableContainer which has same code
-    /********************** Container interface **********************/
-    typedef PhysicsBody* TUpdatableObjectPtr;
-    typedef vector<TUpdatableObjectPtr> MembersContainer;
-    typedef typename MembersContainer::iterator iterator;
-    typedef typename MembersContainer::const_iterator const_iterator;
-    typedef typename MembersContainer::value_type value_type;
-
-    iterator begin() { return members_.begin(); }
-    iterator end() { return members_.end(); }
-    const_iterator begin() const { return members_.begin(); }
-    const_iterator end() const { return members_.end(); }
-    uint size() const { return static_cast<uint>(members_.size()); }
-    const TUpdatableObjectPtr &at(uint index) const { return members_.at(index);  }
-    TUpdatableObjectPtr &at(uint index) { return members_.at(index);  }
-    //allow to override membership modifications
-    virtual void clear() { members_.clear(); }
-    virtual void insert(TUpdatableObjectPtr member) { members_.push_back(member);  }
-    virtual void erase_remove(TUpdatableObjectPtr obj) { 
-        members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
-
     virtual void setWind(const Vector3r& wind) { unused(wind); };
-
-private:
-    MembersContainer members_;
 };
 
 
diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp
index dbf0ec8302..b56fc8eb92 100644
--- a/AirLib/include/physics/PhysicsWorld.hpp
+++ b/AirLib/include/physics/PhysicsWorld.hpp
@@ -13,7 +13,7 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsWorld {
+class PhysicsWorld : UpdatableObject {
 public:
     PhysicsWorld(std::unique_ptr<PhysicsEngineBase> physics_engine, const std::vector<UpdatableObject*>& bodies,
             uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
@@ -21,6 +21,7 @@ class PhysicsWorld {
         )
         : world_(std::move(physics_engine))
     {
+        setName("PhysicsWorld");
         enableStateReport(state_reporter_enabled);
         update_period_nanos_ = update_period_nanos;
         initializeWorld(bodies, start_async_updator);
@@ -108,6 +109,8 @@ class PhysicsWorld {
         world_.setFrameNumber(frameNumber);
     }
 
+    void resetImplementation() override {}
+
 private:
     void initializeWorld(const std::vector<UpdatableObject*>& bodies, bool start_async_updator)
     {
diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp
index bf88c188b6..b8c0cc2fd3 100644
--- a/AirLib/include/physics/World.hpp
+++ b/AirLib/include/physics/World.hpp
@@ -20,7 +20,8 @@ class World : public UpdatableContainer<UpdatableObject*> {
         : physics_engine_(std::move(physics_engine))
     { 
         World::clear();
-
+        setName("World");
+        physics_engine_->setParent(this);
         if (physics_engine)
             physics_engine_->clear();
     }
@@ -118,6 +119,11 @@ class World : public UpdatableContainer<UpdatableObject*> {
         return executor_.isPaused();
     }
 
+    void pauseForTime(double seconds)
+    {
+        executor_.pauseForTime(seconds);
+    }
+
     void continueForTime(double seconds)
     {
         executor_.continueForTime(seconds);
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
index 81902d6cfb..f0fe7ca87e 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
@@ -22,6 +22,8 @@ class MultiRotorPhysicsBody : public PhysicsBody {
         Kinematics* kinematics, Environment* environment)
         : params_(params), vehicle_api_(vehicle_api)
     {
+        setName("MultiRotorPhysicsBody");
+        vehicle_api_->setParent(this);
         initialize(kinematics, environment);
     }
 
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 49b7105b6c..52ba5841d6 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -29,6 +29,7 @@
 #include "vehicles/multirotor/api/MultirotorApiBase.hpp"
 #include "common/PidController.hpp"
 #include "sensors/SensorCollection.hpp"
+#include "physics/World.hpp"
 
 //sensors
 #include "sensors/barometer/BarometerBase.hpp"
@@ -87,6 +88,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     {
         // note this is called AFTER "initialize" when we've connected to the drone
         // so this method cannot reset any of that connection state.
+        world_ = nullptr;
+
+        for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) {
+            if (container->getName() == "World") {
+                // cool beans!
+                // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason...
+                world_ = static_cast<World*>(container);
+            }
+        }
         MultirotorApiBase::resetImplementation();
         was_reset_ = true;
     }
@@ -1598,6 +1608,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             std::lock_guard<std::mutex> guard(telemetry_mutex_);
             actuator_delay_ += delay;
         }
+        if (world_ != nullptr) {
+            world_->pause(false);
+        }
     }
 
     void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
@@ -1762,6 +1775,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         if (hil_node_ != nullptr) {
             hil_node_->sendMessage(hil_sensor);
             received_actuator_controls_ = false;
+            if (lock_step_enabled_ && world_ != nullptr) {
+                world_->pauseForTime(0.1); // 100 millisecond delay max waiting for actuator controls.
+            }
         }
 
         std::lock_guard<std::mutex> guard(last_message_mutex_);
@@ -1992,6 +2008,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     common_utils::Timer gcs_message_timer_;
     std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
     std::string log_file_name_;
+    World* world_;
 
     //every time we return status update, we need to check if we have new data
     //this is why below two variables are marked as mutable

From 6fdb9b77514f27805bcda8a8222c0dc8497fe5fc Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Apr 2021 14:35:00 -0700
Subject: [PATCH 27/37] add missing update

---
 .../common/common_utils/ScheduledExecutor.hpp       | 13 ++++++++++---
 1 file changed, 10 insertions(+), 3 deletions(-)

diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp
index d62cea1be4..68781dd661 100644
--- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp
+++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp
@@ -49,6 +49,7 @@ class ScheduledExecutor {
     void pause(bool is_paused)
     {
         paused_ = is_paused;
+        pause_period_start_ = 0; // cancel any pause period.
     }
 
     bool isPaused() const
@@ -56,6 +57,13 @@ class ScheduledExecutor {
         return paused_;
     }
 
+    void pauseForTime(double seconds)
+    {
+        pause_period_start_ = nanos();
+        pause_period_ = static_cast<TTimeDelta>(1E9 * seconds);
+        paused_ = true;
+    }
+
     void continueForTime(double seconds)
     {
         pause_period_start_ = nanos();
@@ -65,6 +73,7 @@ class ScheduledExecutor {
 
     void continueForFrames(uint32_t frames)
     {
+        pause_period_start_ = 0; // cancel any pause period.
         frame_countdown_enabled_ = true;
         targetFrameNumber_ = frames + currentFrameNumber_;
         paused_ = false;
@@ -174,9 +183,7 @@ class ScheduledExecutor {
             
             if (pause_period_start_ > 0) {
                 if (nanos() - pause_period_start_ >= pause_period_) {
-                    if (! isPaused())
-                        pause(true);
-
+                    pause(!isPaused());
                     pause_period_start_ = 0;
                 }
             }

From d28ace6b97b8d8fbbb74810bf89c5012b6e63b60 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 28 Apr 2021 14:52:23 -0700
Subject: [PATCH 28/37] cleanup unnecessary cmake messages.

---
 cmake/cmake-modules/CommonSetup.cmake | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake
index 22241de7d7..f2da040fc8 100644
--- a/cmake/cmake-modules/CommonSetup.cmake
+++ b/cmake/cmake-modules/CommonSetup.cmake
@@ -24,12 +24,8 @@ macro(SetupConsoleBuild)
 endmacro(SetupConsoleBuild)
 
 macro(CommonSetup)
-    message(STATUS "Running CommonSetup...")
-
     find_package(Threads REQUIRED)
-
-    find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../..")
-    message(STATUS "found AIRSIM_ROOT=${AIRSIM_ROOT}")
+    find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../.." REQUIRED)    
 
     #setup output paths
     set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/output/lib)

From f28f32b6907828eb631633a7c75e3487e159cc06 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 15:59:31 -0700
Subject: [PATCH 29/37] Fix code formatting

---
 AirLib/include/common/AirSimSettings.hpp      |  81 +++--
 AirLib/include/common/UpdatableContainer.hpp  |  12 +-
 AirLib/include/common/UpdatableObject.hpp     |  15 +-
 .../common/common_utils/FileSystem.hpp        |  32 +-
 .../common/common_utils/ScheduledExecutor.hpp |  10 +-
 AirLib/include/physics/FastPhysicsEngine.hpp  |  30 +-
 AirLib/include/physics/PhysicsBody.hpp        |  41 +--
 AirLib/include/physics/PhysicsEngineBase.hpp  |   3 +-
 AirLib/include/physics/PhysicsWorld.hpp       |   3 +-
 AirLib/include/physics/World.hpp              |   3 +-
 AirLib/include/sensors/SensorBase.hpp         |   3 +-
 AirLib/include/sensors/SensorFactory.hpp      |   3 +-
 .../sensors/barometer/BarometerSimple.hpp     |   3 +-
 .../barometer/BarometerSimpleParams.hpp       |   3 +-
 .../sensors/distance/DistanceSimple.hpp       |   3 +-
 .../sensors/distance/DistanceSimpleParams.hpp |   3 +-
 AirLib/include/sensors/gps/GpsSimple.hpp      |   3 +-
 .../include/sensors/gps/GpsSimpleParams.hpp   |   3 +-
 AirLib/include/sensors/imu/ImuSimple.hpp      |   3 +-
 .../include/sensors/imu/ImuSimpleParams.hpp   |   9 +-
 AirLib/include/sensors/lidar/LidarSimple.hpp  |   3 +-
 .../sensors/lidar/LidarSimpleParams.hpp       |   3 +-
 .../magnetometer/MagnetometerSimple.hpp       |   6 +-
 .../magnetometer/MagnetometerSimpleParams.hpp |   3 +-
 .../include/vehicles/car/api/CarApiBase.hpp   |   5 +-
 .../car/firmwares/ardurover/ArduRoverApi.hpp  |   3 +-
 .../vehicles/multirotor/MultiRotorParams.hpp  |   3 +-
 .../multirotor/MultiRotorPhysicsBody.hpp      |   3 +-
 .../firmwares/arducopter/ArduCopterApi.hpp    |   3 +-
 .../firmwares/mavlink/ArduCopterSoloApi.hpp   |   9 +-
 .../mavlink/MavLinkMultirotorApi.hpp          |  96 +++---
 MavLinkCom/common_utils/Utils.hpp             |  81 +++--
 MavLinkCom/include/AsyncResult.hpp            | 247 +++++++-------
 MavLinkCom/include/MavLinkConnection.hpp      | 264 +++++++--------
 MavLinkCom/include/MavLinkDebugLog.hpp        |  41 +--
 MavLinkCom/include/MavLinkLog.hpp             |  56 ++--
 MavLinkCom/include/MavLinkMessageBase.hpp     | 315 +++++++++---------
 MavLinkCom/include/MavLinkNode.hpp            | 149 +++++----
 MavLinkCom/include/MavLinkVehicle.hpp         | 101 +++---
 MavLinkCom/src/MavLinkLog.cpp                 | 309 +++++++++--------
 MavLinkCom/src/MavLinkMessageBase.cpp         |  12 +-
 MavLinkCom/src/MavLinkVehicle.cpp             |   6 +-
 MavLinkCom/src/impl/MavLinkConnectionImpl.cpp |  42 +--
 MavLinkCom/src/impl/MavLinkVehicleImpl.cpp    |  21 +-
 MavLinkCom/src/impl/MavLinkVehicleImpl.hpp    | 119 +++----
 PythonClient/multirotor/path.py               |   6 +-
 .../UnitySensors/UnitySensorFactory.cpp       |  25 +-
 .../Source/UnitySensors/UnitySensorFactory.h  |  10 +-
 docs/log_viewer.md                            |   2 +-
 docs/px4_setup.md                             |  18 +-
 docs/px4_sitl.md                              |   6 +-
 51 files changed, 1149 insertions(+), 1084 deletions(-)

diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index 37ee6374c5..4d685cbe14 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -17,7 +17,8 @@
 
 namespace msr { namespace airlib {
 
-struct AirSimSettings {
+struct AirSimSettings 
+{
 private:
     typedef common_utils::Utils Utils;
     typedef ImageCaptureBase::ImageType ImageType;
@@ -39,7 +40,8 @@ struct AirSimSettings {
     static constexpr char const * kSimModeTypeCar = "Car";
     static constexpr char const * kSimModeTypeComputerVision = "ComputerVision";
 
-    struct SubwindowSetting {
+    struct SubwindowSetting 
+    {
         int window_index;
         ImageType image_type;
         bool visible;
@@ -54,7 +56,8 @@ struct AirSimSettings {
         }
     };
 
-    struct RecordingSetting {
+    struct RecordingSetting 
+    {
         bool record_on_move = false;
         float record_interval = 0.05f;
         std::string folder = "";
@@ -73,7 +76,8 @@ struct AirSimSettings {
         }
     };
 
-    struct PawnPath {
+    struct PawnPath 
+    {
         std::string pawn_bp;
         std::string slippery_mat;
         std::string non_slippery_mat;
@@ -86,12 +90,14 @@ struct AirSimSettings {
         }
     };
 
-    struct RCSettings {
+    struct RCSettings 
+    {
         int remote_control_id = -1;
         bool allow_api_when_disconnected = false;
     };
 
-    struct Rotation {
+    struct Rotation
+    {
         float yaw = 0;
         float pitch = 0;
         float roll = 0;
@@ -113,13 +119,15 @@ struct AirSimSettings {
     };
 
 
-    struct GimbalSetting {
+    struct GimbalSetting 
+    {
         float stabilization = 0;
         //bool is_world_frame = false;
         Rotation rotation = Rotation::nanRotation();
     };
 
-    struct CaptureSetting {
+    struct CaptureSetting 
+    {
         //below settings_json are obtained by using Unreal console command (press ~):
         // ShowFlag.VisualizeHDR 1.
         //to replicate camera settings_json to SceneCapture2D
@@ -145,7 +153,8 @@ struct AirSimSettings {
         float ortho_width = Utils::nan<float>();
     };
 
-    struct NoiseSetting {
+    struct NoiseSetting 
+    {
         int ImageType = 0;
 
         bool Enabled = false;
@@ -168,7 +177,8 @@ struct AirSimSettings {
         float HorzDistortionStrength = 0.002f;
     };
 
-    struct CameraSetting {
+    struct CameraSetting 
+    {
         //nan means keep the default values set in components
         Vector3r position = VectorMath::nanVector();
         Rotation rotation = Rotation::nanRotation();
@@ -184,38 +194,47 @@ struct AirSimSettings {
         }
     };
 
-    struct CameraDirectorSetting {
+    struct CameraDirectorSetting 
+    {
         Vector3r position = VectorMath::nanVector();
         Rotation rotation = Rotation::nanRotation();
         float follow_distance = Utils::nan<float>();
     };
 
-    struct SensorSetting {
+    struct SensorSetting 
+    {
         SensorBase::SensorType sensor_type;
         std::string sensor_name;
         bool enabled = true;
         Settings settings; // imported json data that needs to be parsed by specific sensors.
     };
 
-    struct BarometerSetting : SensorSetting {
+    struct BarometerSetting : SensorSetting 
+    {
     };
 
-    struct ImuSetting : SensorSetting {
+    struct ImuSetting : SensorSetting 
+    {
     };
 
-    struct GpsSetting : SensorSetting {
+    struct GpsSetting : SensorSetting 
+    {
     };
 
-    struct MagnetometerSetting : SensorSetting {
+    struct MagnetometerSetting : SensorSetting 
+    {
     };
 
-    struct DistanceSetting : SensorSetting {
+    struct DistanceSetting : SensorSetting 
+    {
     };
 
-    struct LidarSetting : SensorSetting {
+    struct LidarSetting : SensorSetting 
+    {
     };
 
-    struct VehicleSetting {
+    struct VehicleSetting 
+    {
         //required
         std::string vehicle_name;
         std::string vehicle_type;
@@ -240,7 +259,8 @@ struct AirSimSettings {
         RCSettings rc;
     };
 
-    struct MavLinkConnectionInfo {
+    struct MavLinkConnectionInfo 
+    {
         /* Default values are requires so uninitialized instance doesn't have random values */
 
         bool use_serial = true; // false means use UDP or TCP instead
@@ -292,11 +312,13 @@ struct AirSimSettings {
         std::string logs;
     };
 
-    struct MavLinkVehicleSetting : public VehicleSetting {
+    struct MavLinkVehicleSetting : public VehicleSetting 
+    {
         MavLinkConnectionInfo connection_info;
     };
 
-    struct SegmentationSetting {
+    struct SegmentationSetting 
+    {
         enum class InitMethodType {
             None, CommonObjectsRandomIDs
         };
@@ -310,7 +332,8 @@ struct AirSimSettings {
         MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName;
     };
 
-    struct TimeOfDaySetting {
+    struct TimeOfDaySetting 
+    {
         bool enabled = false;
         std::string start_datetime = "";    //format: %Y-%m-%d %H:%M:%S
         bool is_start_datetime_dst = false;
@@ -711,13 +734,11 @@ struct AirSimSettings {
         connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote);
 
         std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address);
-        if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0)
-        {
+        if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) {
             // backwards compat
             connection_info.control_ip_address = sitlip;
         }
-        if (settings_json.hasKey("SitlPort"))
-        {
+        if (settings_json.hasKey("SitlPort")) {
             // backwards compat
             connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local);
         }
@@ -738,8 +759,7 @@ struct AirSimSettings {
         if (settings_json.getChild("Parameters", params)) {
             std::vector<std::string> keys;
             params.getChildNames(keys);
-            for (auto key: keys)
-            {
+            for (auto key: keys) {
                 connection_info.params[key] = params.getFloat(key, 0);
             }
         }
@@ -1170,8 +1190,7 @@ struct AirSimSettings {
                 //for multirotors we select steppable fixed interval clock unless we have
                 //PX4 enabled vehicle
                 clock_type = "SteppableClock";
-                for (auto const& vehicle : vehicles)
-                {
+                for (auto const& vehicle : vehicles) {
                     if (vehicle.second->auto_create &&
                         vehicle.second->vehicle_type == kVehicleTypePX4) {
                         clock_type = "ScalableClock";
diff --git a/AirLib/include/common/UpdatableContainer.hpp b/AirLib/include/common/UpdatableContainer.hpp
index e55168f530..2b9d476930 100644
--- a/AirLib/include/common/UpdatableContainer.hpp
+++ b/AirLib/include/common/UpdatableContainer.hpp
@@ -10,7 +10,8 @@
 namespace msr { namespace airlib {
 
 template<typename TUpdatableObjectPtr>
-class UpdatableContainer : public UpdatableObject {
+class UpdatableContainer : public UpdatableObject 
+{
 public: //limited container interface
     typedef vector<TUpdatableObjectPtr> MembersContainer;
     typedef typename MembersContainer::iterator iterator;
@@ -24,17 +25,20 @@ class UpdatableContainer : public UpdatableObject {
     const TUpdatableObjectPtr& at(uint index) const { members_.at(index);  }
     TUpdatableObjectPtr& at(uint index) { return members_.at(index);  }
     //allow to override membership modifications
-    virtual void clear() {
+    virtual void clear()
+    {
         for (auto m : members_) {
             m->setParent(nullptr);
         }
         members_.clear();
     }
-    virtual void insert(TUpdatableObjectPtr member) {
+    virtual void insert(TUpdatableObjectPtr member)
+    {
         member->setParent(this);
         members_.push_back(member);
     }
-    virtual void erase_remove(TUpdatableObjectPtr member) {
+    virtual void erase_remove(TUpdatableObjectPtr member)
+    {
         member->setParent(nullptr);
         members_.erase(std::remove(members_.begin(), members_.end(), member), members_.end());
     }
diff --git a/AirLib/include/common/UpdatableObject.hpp b/AirLib/include/common/UpdatableObject.hpp
index 620f54e7d1..9f40e3bafb 100644
--- a/AirLib/include/common/UpdatableObject.hpp
+++ b/AirLib/include/common/UpdatableObject.hpp
@@ -24,7 +24,8 @@ Do not call reset() from constructor or initialization because that will produce
 init->reset calls for base-derived class that would be incorrect.
 */
 
-class UpdatableObject {
+class UpdatableObject
+{
 public:
     void reset()
     { 
@@ -72,19 +73,23 @@ class UpdatableObject {
         return ClockFactory::get();
     }
 
-    UpdatableObject* getParent() {
+    UpdatableObject* getParent()
+    {
         return parent_;
     }
 
-    void setParent(UpdatableObject* container) {
+    void setParent(UpdatableObject* container)
+    {
         parent_ = container;
     }
 
-    std::string getName(){
+    std::string getName()
+    {
         return name_;
     }
 
-    void setName(const std::string& name) {
+    void setName(const std::string& name)
+    {
         this->name_ = name;
     }
 
diff --git a/AirLib/include/common/common_utils/FileSystem.hpp b/AirLib/include/common/common_utils/FileSystem.hpp
index 2d3ae8f994..5b7849d627 100644
--- a/AirLib/include/common/common_utils/FileSystem.hpp
+++ b/AirLib/include/common/common_utils/FileSystem.hpp
@@ -50,23 +50,27 @@ class FileSystem
 
     static std::string getUserDocumentsFolder();
 
-	static std::string getExecutableFolder();
+    static std::string getExecutableFolder();
 
-    static std::string getAppDataFolder() {
+    static std::string getAppDataFolder()
+    {
         return ensureFolder(combine(getUserDocumentsFolder(), ProductFolderName));
     }
 
-    static std::string ensureFolder(const std::string& fullpath) {
+    static std::string ensureFolder(const std::string& fullpath)
+    {
         // make sure this directory exists.
         return createDirectory(fullpath);
     }
 
-    static std::string ensureFolder(const std::string& parentFolder, const std::string& child) {
+    static std::string ensureFolder(const std::string& parentFolder, const std::string& child)
+    {
         // make sure this directory exists.
         return createDirectory(combine(parentFolder, child));
     }
 
-    static std::string combine(const std::string& parentFolder, const std::string& child) {
+    static std::string combine(const std::string& parentFolder, const std::string& child)
+    {
         if (child.size() == 0)
             return parentFolder;
 
@@ -83,7 +87,8 @@ class FileSystem
         return parentFolder + kPathSeparator + child;
     }
 
-    static void removeLeaf(std::string& path) {
+    static void removeLeaf(std::string& path)
+    {
         size_t size = path.size();
         size_t pos = path.find_last_of('/');
         if (pos != std::string::npos) {
@@ -98,8 +103,7 @@ class FileSystem
         int len = static_cast<int>(str.size());
         const char* ptr = str.c_str();
         int i = 0;
-        for (i = len - 1; i >= 0; i--)
-        {
+        for (i = len - 1; i >= 0; i--) {
             if (ptr[i] == '.')
                 break;
         }
@@ -138,8 +142,8 @@ class FileSystem
         //return filename_ss.str();
     }
 
-    static void openTextFile(const std::string& filepath, std::ifstream& file){
-        
+    static void openTextFile(const std::string& filepath, std::ifstream& file)
+    {        
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
         std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
@@ -150,8 +154,8 @@ class FileSystem
 #endif
     }
     
-    static void createBinaryFile(const std::string& filepath, std::ofstream& file){
-        
+    static void createBinaryFile(const std::string& filepath, std::ofstream& file)
+    {        
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
         std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
@@ -162,8 +166,8 @@ class FileSystem
 #endif
     }
     
-    static void createTextFile(const std::string& filepath, std::ofstream& file){
-        
+    static void createTextFile(const std::string& filepath, std::ofstream& file)
+    {        
 #ifdef _WIN32
         // WIN32 will create the wrong file names if we don't first convert them to UTF-16.
         std::wstring_convert<std::codecvt_utf8<wchar_t>, wchar_t> converter;
diff --git a/AirLib/include/common/common_utils/ScheduledExecutor.hpp b/AirLib/include/common/common_utils/ScheduledExecutor.hpp
index 68781dd661..8ab23b9e1f 100644
--- a/AirLib/include/common/common_utils/ScheduledExecutor.hpp
+++ b/AirLib/include/common/common_utils/ScheduledExecutor.hpp
@@ -14,18 +14,22 @@
 
 namespace common_utils {
 
-class ScheduledExecutor {
+class ScheduledExecutor 
+{
 public:
     ScheduledExecutor()
     {}
+
     ScheduledExecutor(const std::function<bool(uint64_t)>& callback, uint64_t period_nanos)
     {
         initialize(callback, period_nanos);
     }
+
     ~ScheduledExecutor()
     {
         stop();
     }
+
     void initialize(const std::function<bool(uint64_t)>& callback, uint64_t period_nanos)
     {
         callback_ = callback;
@@ -95,8 +99,8 @@ class ScheduledExecutor {
                     th_.join();
                 }
             }
-            catch(const std::system_error& /* e */)
-            { }
+            catch(const std::system_error& /* e */) {
+            }
         }
     }
 
diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp
index 035f89d0fc..ff024462bb 100644
--- a/AirLib/include/physics/FastPhysicsEngine.hpp
+++ b/AirLib/include/physics/FastPhysicsEngine.hpp
@@ -16,7 +16,8 @@
 
 namespace msr { namespace airlib {
 
-class FastPhysicsEngine : public PhysicsEngineBase {
+class FastPhysicsEngine : public PhysicsEngineBase 
+{
 public:
     FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
         : enable_ground_lock_(enable_ground_lock), wind_(wind)
@@ -51,8 +52,8 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     {
         for (PhysicsBody* body_ptr : *this) {
             reporter.writeValue("Phys", debug_string_.str());
-			reporter.writeValue("Is Grounded", body_ptr->isGrounded());
-			reporter.writeValue("Force (world)", body_ptr->getWrench().force);
+            reporter.writeValue("Is Grounded", body_ptr->isGrounded());
+            reporter.writeValue("Force (world)", body_ptr->getWrench().force);
             reporter.writeValue("Torque (body)", body_ptr->getWrench().torque);
         }
         //call base
@@ -106,12 +107,12 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         body.unlock();
 
 
-		//TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
+        //TODO: this is now being done in PawnSimApi::update. We need to re-think this sequence
         //with below commented out - Arducopter GPS may not work.
-		//body.getEnvironment().setPosition(next.pose.position);
-		//body.getEnvironment().update();
-		
-	}
+        //body.getEnvironment().setPosition(next.pose.position);
+        //body.getEnvironment().update();
+        
+    }
 
     static void updateCollisionResponseInfo(const CollisionInfo& collision_info, const Kinematics::State& next, 
         bool is_collision_response, CollisionResponse& collision_response)
@@ -232,7 +233,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
             // also eliminate any linear velocity due to twist - since we are sitting on the ground there shouldn't be any.
             next.twist.linear = Vector3r::Zero();
             next.pose.position = collision_info.position;
-			body.setGrounded(true);
+            body.setGrounded(true);
 
             // but we do want to "feel" the ground when we hit it (we should see a small z-acc bump)
             // equal and opposite our downward velocity.
@@ -240,8 +241,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
             //throttledLogOutput("*** Triggering ground lock", 0.1);
         }
-        else
-        {
+        else {
             //else keep the orientation
             next.pose.position = collision_info.position + (collision_info.normal * collision_info.penetration_depth) + next.twist.linear * (dt_real * kCollisionResponseCycles);
         }
@@ -256,8 +256,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     {
         TTimeDelta dt = clock()->elapsedSince(last_message_time);
         const real_T dt_real = static_cast<real_T>(dt);
-        if (dt_real > seconds)
-        {
+        if (dt_real > seconds) {
             Utils::log(msg);
             last_message_time = clock()->nowNanos();
         }
@@ -341,10 +340,9 @@ class FastPhysicsEngine : public PhysicsEngineBase {
             float external_force_magnitude = body_wrench.force.squaredNorm();
             Vector3r weight = body.getMass() * body.getEnvironment().getState().gravity;
             float weight_magnitude = weight.squaredNorm();
-            if (external_force_magnitude >= weight_magnitude)
-            {
+            if (external_force_magnitude >= weight_magnitude) {
                 //throttledLogOutput("*** Losing ground lock due to body_wrench " + VectorMath::toString(body_wrench.force), 0.1);
-				body.setGrounded(false);
+                body.setGrounded(false);
             }
             next_wrench.force = Vector3r::Zero();
             next_wrench.torque = Vector3r::Zero();
diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp
index a20daffc7c..8e98155196 100644
--- a/AirLib/include/physics/PhysicsBody.hpp
+++ b/AirLib/include/physics/PhysicsBody.hpp
@@ -15,7 +15,8 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsBody : public UpdatableObject {
+class PhysicsBody : public UpdatableObject 
+{
 public: //interface
     virtual real_T getRestitution() const = 0;
     virtual real_T getFriction() const = 0;
@@ -104,7 +105,7 @@ class PhysicsBody : public UpdatableObject {
         wrench_ = Wrench::zero();
         collision_info_ = CollisionInfo();
         collision_response_ = CollisionResponse();
-		grounded_ = false;
+        grounded_ = false;
 
         //update individual vertices
         for (uint vertex_index = 0; vertex_index < wrenchVertexCount(); ++vertex_index) {
@@ -218,24 +219,24 @@ class PhysicsBody : public UpdatableObject {
         return collision_response_;
     }
 
-	bool isGrounded() const
-	{
-		return grounded_;
-	}
-	void setGrounded(bool grounded)
-	{
-		grounded_ = grounded;
-	}
+    bool isGrounded() const
+    {
+        return grounded_;
+    }
+    void setGrounded(bool grounded)
+    {
+        grounded_ = grounded;
+    }
 
-	void lock()
-	{
-		mutex_.lock();
-	}
+    void lock()
+    {
+        mutex_.lock();
+    }
 
-	void unlock()
-	{
-		mutex_.unlock();
-	}
+    void unlock()
+    {
+        mutex_.unlock();
+    }
 
 public:
     //for use in physics engine: //TODO: use getter/setter or friend method?
@@ -254,8 +255,8 @@ class PhysicsBody : public UpdatableObject {
     CollisionInfo collision_info_;
     CollisionResponse collision_response_;
 
-	bool grounded_ = false;
-	std::mutex mutex_;
+    bool grounded_ = false;
+    std::mutex mutex_;
 };
 
 }} //namespace
diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp
index dd06c3f829..0d41967f49 100644
--- a/AirLib/include/physics/PhysicsEngineBase.hpp
+++ b/AirLib/include/physics/PhysicsEngineBase.hpp
@@ -10,7 +10,8 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*> {
+class PhysicsEngineBase : public UpdatableContainer<PhysicsBody*>
+{
 public:
     virtual void update() override
     {
diff --git a/AirLib/include/physics/PhysicsWorld.hpp b/AirLib/include/physics/PhysicsWorld.hpp
index b56fc8eb92..7dbcc8ae26 100644
--- a/AirLib/include/physics/PhysicsWorld.hpp
+++ b/AirLib/include/physics/PhysicsWorld.hpp
@@ -13,7 +13,8 @@
 
 namespace msr { namespace airlib {
 
-class PhysicsWorld : UpdatableObject {
+class PhysicsWorld : UpdatableObject 
+{
 public:
     PhysicsWorld(std::unique_ptr<PhysicsEngineBase> physics_engine, const std::vector<UpdatableObject*>& bodies,
             uint64_t update_period_nanos = 3000000LL, bool state_reporter_enabled = false,
diff --git a/AirLib/include/physics/World.hpp b/AirLib/include/physics/World.hpp
index b8c0cc2fd3..94f738e84e 100644
--- a/AirLib/include/physics/World.hpp
+++ b/AirLib/include/physics/World.hpp
@@ -14,7 +14,8 @@
 
 namespace msr { namespace airlib {
 
-class World : public UpdatableContainer<UpdatableObject*> {
+class World : public UpdatableContainer<UpdatableObject*>
+{
 public:
     World(std::unique_ptr<PhysicsEngineBase> physics_engine)
         : physics_engine_(std::move(physics_engine))
diff --git a/AirLib/include/sensors/SensorBase.hpp b/AirLib/include/sensors/SensorBase.hpp
index 44bd8531aa..d56f46b3de 100644
--- a/AirLib/include/sensors/SensorBase.hpp
+++ b/AirLib/include/sensors/SensorBase.hpp
@@ -18,7 +18,8 @@ namespace msr { namespace airlib {
     After construction of the derived class an initialize(...) must be made which would
     set the sensor in good-to-use state by call to reset.
 */
-class SensorBase : public UpdatableObject  {
+class SensorBase : public UpdatableObject
+{
 public:
     enum class SensorType : uint {
         Barometer = 1,
diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp
index 0c93412ec5..1f25312a13 100644
--- a/AirLib/include/sensors/SensorFactory.hpp
+++ b/AirLib/include/sensors/SensorFactory.hpp
@@ -15,7 +15,8 @@
 namespace msr { namespace airlib {
 
 
-class SensorFactory {
+class SensorFactory
+{
 public:
 
     // creates one sensor from settings
diff --git a/AirLib/include/sensors/barometer/BarometerSimple.hpp b/AirLib/include/sensors/barometer/BarometerSimple.hpp
index 515ae21e57..eda67bf092 100644
--- a/AirLib/include/sensors/barometer/BarometerSimple.hpp
+++ b/AirLib/include/sensors/barometer/BarometerSimple.hpp
@@ -16,7 +16,8 @@
 
 namespace msr { namespace airlib {
 
-class BarometerSimple  : public BarometerBase {
+class BarometerSimple  : public BarometerBase
+{
 public:
     BarometerSimple(const AirSimSettings::BarometerSetting& setting = AirSimSettings::BarometerSetting())
         : BarometerBase(setting.sensor_name)
diff --git a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
index 45b4f6601c..ef128e78a1 100644
--- a/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
+++ b/AirLib/include/sensors/barometer/BarometerSimpleParams.hpp
@@ -11,7 +11,8 @@
 namespace msr { namespace airlib {
 
 
-struct BarometerSimpleParams {
+struct BarometerSimpleParams 
+{
     //user specified sea level pressure is specified in hPa units
     real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
 
diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp
index 63de52c15a..81f7a4858e 100644
--- a/AirLib/include/sensors/distance/DistanceSimple.hpp
+++ b/AirLib/include/sensors/distance/DistanceSimple.hpp
@@ -14,7 +14,8 @@
 
 namespace msr { namespace airlib {
 
-class DistanceSimple : public DistanceBase {
+class DistanceSimple : public DistanceBase
+{
 public:
     DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting())
         : DistanceBase(setting.sensor_name)
diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
index f223c03e85..3afadd29c1 100644
--- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
+++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp
@@ -11,7 +11,8 @@
 namespace msr { namespace airlib {
 
 
-struct DistanceSimpleParams {
+struct DistanceSimpleParams 
+{
     real_T min_distance = 20.0f / 100; //m
     real_T max_distance = 4000.0f / 100; //m
 
diff --git a/AirLib/include/sensors/gps/GpsSimple.hpp b/AirLib/include/sensors/gps/GpsSimple.hpp
index dd32cfd22b..8645a3b609 100644
--- a/AirLib/include/sensors/gps/GpsSimple.hpp
+++ b/AirLib/include/sensors/gps/GpsSimple.hpp
@@ -15,7 +15,8 @@
 
 namespace msr { namespace airlib {
 
-class GpsSimple : public GpsBase {
+class GpsSimple : public GpsBase
+{
 public: //methods
     GpsSimple(const AirSimSettings::GpsSetting& setting = AirSimSettings::GpsSetting())
         : GpsBase(setting.sensor_name)
diff --git a/AirLib/include/sensors/gps/GpsSimpleParams.hpp b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
index 18e9949dd8..b8715fd16e 100644
--- a/AirLib/include/sensors/gps/GpsSimpleParams.hpp
+++ b/AirLib/include/sensors/gps/GpsSimpleParams.hpp
@@ -9,7 +9,8 @@
 
 namespace msr { namespace airlib {
 
-struct GpsSimpleParams {
+struct GpsSimpleParams 
+{
     real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
     real_T eph_initial = 100.0f, epv_initial = 100.0f;   //initially fully diluted positions
     real_T eph_final = 0.1f, epv_final = 0.1f;   // PX4 won't converge GPS sensor fusion until we get to 10% accuracty.
diff --git a/AirLib/include/sensors/imu/ImuSimple.hpp b/AirLib/include/sensors/imu/ImuSimple.hpp
index fbf38c730c..38bac02795 100644
--- a/AirLib/include/sensors/imu/ImuSimple.hpp
+++ b/AirLib/include/sensors/imu/ImuSimple.hpp
@@ -11,7 +11,8 @@
 
 namespace msr { namespace airlib {
 
-class ImuSimple : public ImuBase {
+class ImuSimple : public ImuBase
+{
 public:
     //constructors
     ImuSimple(const AirSimSettings::ImuSetting& setting = AirSimSettings::ImuSetting())
diff --git a/AirLib/include/sensors/imu/ImuSimpleParams.hpp b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
index 78f8de4dc0..115f65294e 100644
--- a/AirLib/include/sensors/imu/ImuSimpleParams.hpp
+++ b/AirLib/include/sensors/imu/ImuSimpleParams.hpp
@@ -15,7 +15,8 @@ namespace msr { namespace airlib {
 
 // A description of the parameters:
 // https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics
-struct ImuSimpleParams {
+struct ImuSimpleParams 
+{
     /* ref: Parameter values are for MPU 6000 IMU from InvenSense 
     Design and Characterization of a Low Cost MEMS IMU Cluster for Precision Navigation
     Daniel R. Greenheck, 2009, sec 2.2, pp 17
@@ -24,7 +25,8 @@ struct ImuSimpleParams {
     https://www.invensense.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
     For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf
     */
-    struct Gyroscope {
+    struct Gyroscope 
+    {
         //angular random walk (ARW)
         real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
         //Bias Stability (tau = 500s)
@@ -33,7 +35,8 @@ struct ImuSimpleParams {
         Vector3r turn_on_bias = Vector3r::Zero(); //assume calibration is done
     } gyro;
 
-    struct Accelerometer {
+    struct Accelerometer 
+    {
         //velocity random walk (ARW)
         real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 
         //Bias Stability (tau = 800s)
diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp
index db6f4b05a7..f05f21b127 100644
--- a/AirLib/include/sensors/lidar/LidarSimple.hpp
+++ b/AirLib/include/sensors/lidar/LidarSimple.hpp
@@ -13,7 +13,8 @@
 
 namespace msr { namespace airlib {
 
-class LidarSimple : public LidarBase {
+class LidarSimple : public LidarBase
+{
 public:
     LidarSimple(const AirSimSettings::LidarSetting& setting = AirSimSettings::LidarSetting())
         : LidarBase(setting.sensor_name)
diff --git a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
index 15ea6a89fd..e53922f817 100644
--- a/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
+++ b/AirLib/include/sensors/lidar/LidarSimpleParams.hpp
@@ -9,7 +9,8 @@
 
 namespace msr { namespace airlib {
 
-struct LidarSimpleParams {
+struct LidarSimpleParams 
+{
 
     // Velodyne VLP-16 Puck config
     // https://velodynelidar.com/vlp-16.html
diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp
index b2a1556611..c77eee9829 100644
--- a/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp
+++ b/AirLib/include/sensors/magnetometer/MagnetometerSimple.hpp
@@ -15,7 +15,8 @@
 
 namespace msr { namespace airlib {
 
-class MagnetometerSimple : public MagnetometerBase {
+class MagnetometerSimple : public MagnetometerBase
+{
 public: 
     MagnetometerSimple(const AirSimSettings::MagnetometerSetting& setting = AirSimSettings::MagnetometerSetting())
         : MagnetometerBase(setting.sensor_name)
@@ -66,8 +67,7 @@ class MagnetometerSimple : public MagnetometerBase {
 private: //methods
     void updateReference(const GroundTruth& ground_truth)
     {
-        switch (params_.ref_source)
-        {
+        switch (params_.ref_source) {
         case MagnetometerSimpleParams::ReferenceSource::ReferenceSource_Constant:
             // Constant magnetic field for Seattle
             magnetic_field_true_ = Vector3r(0.34252f, 0.09805f, 0.93438f);
diff --git a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
index b0a7aab325..884cef5a1a 100644
--- a/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
+++ b/AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp
@@ -10,7 +10,8 @@
 namespace msr { namespace airlib {
 
 
-struct MagnetometerSimpleParams {
+struct MagnetometerSimpleParams 
+{
     enum ReferenceSource {
         ReferenceSource_Constant,
         ReferenceSource_DipoleModel
diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp
index 2cbbb1cfb9..7d69420d43 100644
--- a/AirLib/include/vehicles/car/api/CarApiBase.hpp
+++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp
@@ -14,7 +14,8 @@
 
 namespace msr { namespace airlib {
 
-class CarApiBase : public VehicleApiBase  {
+class CarApiBase : public VehicleApiBase
+{
 public:
     struct CarControls {
         float throttle = 0; /* 1 to -1 */
@@ -83,7 +84,7 @@ class CarApiBase : public VehicleApiBase  {
 public:
 
     // TODO: Temporary constructor for the Unity implementation which does not use the new Sensor Configuration Settings implementation.
-	//CarApiBase() {}
+    //CarApiBase() {}
 
     CarApiBase(const AirSimSettings::VehicleSetting* vehicle_setting, 
         std::shared_ptr<SensorFactory> sensor_factory, 
diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
index b1c24cbf6b..ae61993aad 100644
--- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
+++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
@@ -28,7 +28,8 @@
 
 namespace msr { namespace airlib {
 
-class ArduRoverApi : public CarApiBase {
+class ArduRoverApi : public CarApiBase
+{
 
 public:
     ArduRoverApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr<SensorFactory> sensor_factory,
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
index 75f24ea1c6..bb730c8540 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp
@@ -12,7 +12,8 @@
 
 namespace msr { namespace airlib {
 
-class MultiRotorParams {
+class MultiRotorParams
+{
 //All units are SI
 public: //types
     struct RotorPose {
diff --git a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
index f0fe7ca87e..b6ac4ec992 100644
--- a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
+++ b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp
@@ -16,7 +16,8 @@
 
 namespace msr { namespace airlib {
 
-class MultiRotorPhysicsBody : public PhysicsBody {
+class MultiRotorPhysicsBody : public PhysicsBody
+{
 public:
     MultiRotorPhysicsBody(MultiRotorParams* params, VehicleApiBase* vehicle_api, 
         Kinematics* kinematics, Environment* environment)
diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
index e6ea24f8fe..9bd5e3014d 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
@@ -31,7 +31,8 @@
 
 namespace msr { namespace airlib {
 
-class ArduCopterApi : public MultirotorApiBase {
+class ArduCopterApi : public MultirotorApiBase
+{
 
 public:
     ArduCopterApi(const MultiRotorParams* vehicle_params, const AirSimSettings::MavLinkConnectionInfo& connection_info)
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
index bc1db991f7..bbeeeb43b5 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
@@ -71,8 +71,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
 
             packet.magic = 0x4c56414f;
 
-            if (udpSocket_ != nullptr)
-            {
+            if (udpSocket_ != nullptr) {
                 std::vector<uint8_t> msg(sizeof(packet));
                 memcpy(msg.data(), &packet, sizeof(packet));
                 udpSocket_->sendMessage(msg);
@@ -170,8 +169,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
 
     void rotorPowerMessageHandler(std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg)
     {
-        if (msg.size() != sizeof(RotorControlMessage))
-        {
+        if (msg.size() != sizeof(RotorControlMessage)) {
             Utils::log("Got rotor control message of size " + std::to_string(msg.size()) + " when we were expecting size " + std::to_string(sizeof(RotorControlMessage)), Utils::kLogLevelError);
             return;
         }
@@ -201,8 +199,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
         float phiDot = p + tan(theta)*(q*sin(phi) + r * cos(phi));
 
         float thetaDot = q * cos(phi) - r * sin(phi);
-        if (fabs(cos(theta)) < 1.0e-20)
-        {
+        if (fabs(cos(theta)) < 1.0e-20) {
             theta += 1.0e-10f;
         }
 
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 52ba5841d6..e0b297a050 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -46,8 +46,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     virtual ~MavLinkMultirotorApi()
     {
         closeAllConnection();
-        if (this->connect_thread_.joinable())
-        {
+        if (this->connect_thread_.joinable()) {
             this->connect_thread_.join();
         }
         if (this->telemetry_thread_.joinable()) {
@@ -101,7 +100,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         was_reset_ = true;
     }
 
-    unsigned long long getSimTime() {
+    unsigned long long getSimTime() 
+    {
         // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
         if (lock_step_enabled_) {
             if (sim_time_us_ == 0) {
@@ -114,7 +114,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    void advanceTime() {
+    void advanceTime() 
+    {
         sim_time_us_ = clock()->nowNanos() / 1000;
     }
 
@@ -294,6 +295,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         updateState();
         return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
     }
+
     virtual Vector3r getVelocity() const override
     {
         updateState();
@@ -320,6 +322,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         std::lock_guard<std::mutex> guard(hil_controls_mutex_);
         return rotor_controls_[rotor_index];
     }
+
     virtual size_t getActuatorCount() const override
     {
         return RotorControlsCount;
@@ -341,7 +344,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         return rc;
     }
 
-    void onArmed() {
+    void onArmed() 
+    {
         if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
             auto con = mav_vehicle_->getConnection();
             if (con != nullptr) {
@@ -350,8 +354,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                     log_ = nullptr;
                 }
 
-                try
-                {
+                try {
                     std::time_t t = std::time(0);   // get time now
                     std::tm* now = std::localtime(&t);
                     auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
@@ -378,7 +381,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    void onDisarmed() {
+    void onDisarmed() 
+    {
         if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
             auto con = mav_vehicle_->getConnection();
             if (con != nullptr) {
@@ -416,8 +420,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             addStatusMessage("Waiting for z-position to stabilize...");
             if (!waitForFunction([&]() {
                 return ground_variance_ <= GroundTolerance;
-                }, timeout_sec).isComplete())
-            {
+                }, timeout_sec).isComplete()) {
                 auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
                 throw VehicleMoveException(msg);
             }
@@ -459,8 +462,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         checkValidVehicle();
         if (current_state_.home.is_set) {
             bool rc = false;
-            if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc))
-            {
+            if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) {
                 throw VehicleMoveException("Landing command - timeout waiting for response from drone");
             }
             else if (!rc) {
@@ -477,8 +479,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             }, timeout_sec);
 
         // Wait for landed state (or user cancellation)
-        if (!waiter.isComplete())
-        {
+        if (!waiter.isComplete()) {
             throw VehicleMoveException("Drone hasn't reported a landing state");
         }
         return waiter.isComplete();
@@ -567,10 +568,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         checkValidVehicle();
         mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
         //auto start_time = std::chrono::system_clock::now();
-        while (!getCancelToken().isCancelled())
-        {
-            if (result.wait(100, &rc))
-            {
+        while (!getCancelToken().isCancelled()) {
+            if (result.wait(100, &rc)) {
                 break;
             }
         }
@@ -637,8 +636,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             data.messages_received += gcs.messages_received;
             data.messages_sent += gcs.messages_sent;
 
-            if (gcs.messages_received == 0)
-            {
+            if (gcs.messages_received == 0) {
                 if (!gcs_message_timer_.started()) {
                     gcs_message_timer_.start();
                 }
@@ -689,10 +687,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    void start_telemtry_thread() {
-
-        if (this->telemetry_thread_.joinable())
-        {
+    void start_telemtry_thread()
+    {
+        if (this->telemetry_thread_.joinable()) {
             this->telemetry_thread_.join();
         }
 
@@ -704,9 +701,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this);
     }
 
-    void telemtry_thread() {
+    void telemtry_thread()
+    {
         while (log_ != nullptr) {
-
             std::this_thread::sleep_for(std::chrono::seconds(1));
             writeTelemetry(1);
         }
@@ -716,12 +713,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     {
         return 1.0f / 50; //1 period of 50hz
     }
+
     virtual float getTakeoffZ() const override
     {
         // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
         // enough to get out of the backwash turbulence.  Negative due to NED coordinate system.
         return -3.0f;
     }
+
     virtual float getDistanceAccuracy() const override
     {
         return 0.5f;    //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
@@ -771,16 +770,19 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
         mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
     }
+    
     virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
     {
         checkValidVehicle();
         mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
     }
+
     virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
     {
         checkValidVehicle();
         mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
     }
+    
     virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
     {
         if (target_height_ != -z) {
@@ -792,6 +794,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
         mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
     }
+
     virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
     {
         checkValidVehicle();
@@ -804,12 +807,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
         mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
     }
+
     virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
     {
         checkValidVehicle();
         float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
         mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
     }
+
     virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
     {
         checkValidVehicle();
@@ -829,6 +834,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     {
         startOffboardMode();
     }
+
     virtual void afterTask() override
     {
         endOffboardMode();
@@ -839,13 +845,18 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
     {
     public:
-        MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy) {
+        MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy) 
+        {
             proxy_ = proxy;
         }
-        ~MavLinkLogViewerLog() {
+
+        ~MavLinkLogViewerLog() 
+        {
             proxy_ = nullptr;
         }
-        void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override {
+
+        void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override 
+        {
             if (proxy_ != nullptr) {
                 unused(timestamp);
                 mavlinkcom::MavLinkMessage copy;
@@ -874,15 +885,15 @@ class MavLinkMultirotorApi : public MultirotorApiBase
     {
         if (!connecting_) {
             connecting_ = true;
-            if (this->connect_thread_.joinable())
-            {
+            if (this->connect_thread_.joinable()) {
                 this->connect_thread_.join();
             }
             this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this);
         }
     }
 
-    virtual void disconnect() {
+    virtual void disconnect() 
+    {
         addStatusMessage("Disconnecting mavlink vehicle");
         connected_ = false;
         connecting_ = false;
@@ -1088,7 +1099,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    void checkValidVehicle() {
+    void checkValidVehicle()
+    {
         if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) {
             throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding");
         }
@@ -1100,8 +1112,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         StatusLock lock(this);
         if (mav_vehicle_ != nullptr) {
             int version = mav_vehicle_->getVehicleStateVersion();
-            if (version != state_version_)
-            {
+            if (version != state_version_) {
                 current_state_ = mav_vehicle_->getVehicleState();
                 state_version_ = version;
             }
@@ -1127,7 +1138,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node) {
+    bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node)
+    {
         try {
             // try and send a test message.
             mavlinkcom::MavLinkHeartbeat test;
@@ -1239,8 +1251,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
 
                 std::string portName_str;
 
-                for (wchar_t ch : info.portName)
-                {
+                for (wchar_t ch : info.portName) {
                     portName_str.push_back(static_cast<char>(ch));
                 }
                 return portName_str;
@@ -1590,7 +1601,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
     }
 
-    void handleLockStep() {
+    void handleLockStep()
+    {
         received_actuator_controls_ = true;
         // if the timestamps match then it means we are in lockstep mode.
         if (!lock_step_enabled_) {
@@ -1707,8 +1719,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
         else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) {
             // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff.
-            if (current_state_.controls.landed)
-            {
+            if (current_state_.controls.landed) {
                 monitorGroundAltitude();
             }
         }
@@ -1784,7 +1795,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         last_sensor_message_ = hil_sensor;
     }
 
-    void sendSystemTime() {
+    void sendSystemTime()
+    {
         // SYSTEM TIME from host
         auto tu = getSimTime();
         uint32_t ms = (uint32_t)(tu / 1000);
diff --git a/MavLinkCom/common_utils/Utils.hpp b/MavLinkCom/common_utils/Utils.hpp
index 7c0bd91c85..40146937d5 100644
--- a/MavLinkCom/common_utils/Utils.hpp
+++ b/MavLinkCom/common_utils/Utils.hpp
@@ -86,7 +86,8 @@ void unused(T const & result) { static_cast<void>(result); }
 
 namespace mavlink_utils {
 
-class Utils {
+class Utils
+{
 private:
     typedef std::chrono::system_clock system_clock;
     typedef std::chrono::steady_clock steady_clock;
@@ -99,7 +100,8 @@ class Utils {
 
 
 public:
-    static void enableImmediateConsoleFlush() {
+    static void enableImmediateConsoleFlush()
+    {
         //disable buffering
         setbuf(stdout, NULL);
     }
@@ -113,16 +115,20 @@ class Utils {
         return gaussian_dist(random_gen) * stddev + mean;
     }
     
-    static constexpr double degreesToRadians(double degrees) {
+    static constexpr double degreesToRadians(double degrees) 
+    {
         return static_cast<double>(M_PIl * degrees / 180.0);
     }
-    static constexpr float degreesToRadians(float degrees) {
+    static constexpr float degreesToRadians(float degrees) 
+    {
         return static_cast<float>(M_PIf * degrees / 180.0f);
     }
-    static constexpr double radiansToDegrees(double radians) {
+    static constexpr double radiansToDegrees(double radians) 
+    {
         return static_cast<double>(radians * 180.0 / M_PIl);
     }
-    static constexpr float radiansToDegrees(float radians) {
+    static constexpr float radiansToDegrees(float radians) 
+    {
         return static_cast<float>(radians * 180.0f / M_PIf);
     }
 
@@ -135,13 +141,15 @@ class Utils {
     }
 
     template <typename T>
-    static int sign(T val) {
+    static int sign(T val)
+    {
         return T(0) < val ? 1 : (T(0) > val ? -1 : 0);
     }   
 
     /// Limits absolute value whole preserving sign
     template <typename T> 
-    static T limitAbsValue(T val, T min_value, T max_value) {
+    static T limitAbsValue(T val, T min_value, T max_value)
+    {
         T val_abs = std::abs(val);
         T val_limited = std::max(val_abs, min_value);
         val_limited = std::min(val_limited, max_value);
@@ -150,7 +158,8 @@ class Utils {
     
     /// Limits absolute value whole preserving sign
     template <typename T> 
-    static T clip(T val, T min_value, T max_value) {
+    static T clip(T val, T min_value, T max_value)
+    {
         return std::max(min_value, std::min(val, max_value));
     }
 
@@ -167,8 +176,7 @@ class Utils {
         stringstream ss;
         ss << prefix;
 
-        for (Iterator i = start; i != last; ++i)
-        {
+        for (Iterator i = start; i != last; ++i) {
             if (i != start)
                 ss << delim;
             ss << *i;
@@ -183,8 +191,7 @@ class Utils {
         int len = static_cast<int>(str.size());
         const char* ptr = str.c_str();
         int i = 0;
-        for (i = len - 1; i >= 0; i--)
-        {
+        for (i = len - 1; i >= 0; i--) {
             if (ptr[i] == '.')
                 break;
         }
@@ -223,14 +230,12 @@ class Utils {
         int len = static_cast<int>(str.size());
         const char* ptr = str.c_str();
         int i = 0;
-        for (i = 0; i < len; i++)
-        {
+        for (i = 0; i < len; i++) {
             if (ptr[i] != ch)
                 break;
         }
         int j = 0;
-        for (j = len - 1; j >= i; j--)
-        {
+        for (j = len - 1; j >= i; j--) {
             if (ptr[j] != ch)
                 break;
         }
@@ -241,29 +246,24 @@ class Utils {
     {
         auto start = s.begin();
         std::vector<string> result;
-        for (auto it = s.begin(); it != s.end(); it++)
-        {
+        for (auto it = s.begin(); it != s.end(); it++) {
             char ch = *it;
             bool split = false;
-            for (int i = 0; i < numSplitChars; i++)
-            {
+            for (int i = 0; i < numSplitChars; i++) {
                 if (ch == splitChars[i]) {
                     split = true;
                     break;
                 }
             }
-            if (split)
-            {
-                if (start < it)
-                {
+            if (split) {
+                if (start < it) {
                     result.push_back(string(start, it));
                 }
                 start = it;
                 start++;
             }
         }
-        if (start < s.end())
-        {
+        if (start < s.end()) {
             result.push_back(string(start, s.end()));
         }
         return result;
@@ -277,14 +277,12 @@ class Utils {
         auto start = line.begin();
         std::vector<std::string> result;
         auto end = line.end();
-        for (auto it = line.begin(); it != end; )
-        {
+        for (auto it = line.begin(); it != end; ) {
             bool split = false;
             char ch = *it;
             if (ch == '\'' || ch == '"') {
                 // skip quoted literal
-                if (start < it)
-                {
+                if (start < it) {
                     result.push_back(string(start, it));
                 }
                 it++;
@@ -303,10 +301,8 @@ class Utils {
                     }
                 }
             }
-            if (split)
-            {
-                if (start < it)
-                {
+            if (split) {
+                if (start < it) {
                     result.push_back(string(start, it));
                 }
                 start = it;
@@ -316,8 +312,7 @@ class Utils {
                 it++;
             }
         }
-        if (start < end)
-        {
+        if (start < end) {
             result.push_back(string(start, end));
         }
         return result;
@@ -333,8 +328,7 @@ class Utils {
         _strlwr_s(buf.get(), len + 1U);
 #else
         char* p = buf.get();
-        for (int i = len; i > 0; i--)
-        {
+        for (int i = len; i > 0; i--) {
             *p = tolower(*p);
             p++;
         }
@@ -368,15 +362,18 @@ class Utils {
     }
 
     template<typename T>
-    static constexpr T nan() {
+    static constexpr T nan()
+    {
         return std::numeric_limits<T>::quiet_NaN();
     }
     template<typename T>
-    static constexpr T max() {
+    static constexpr T max()
+    {
         return std::numeric_limits<T>::max();
     }
     template<typename T>
-    static constexpr T min() {
+    static constexpr T min()
+    {
         return std::numeric_limits<T>::min();
     }
 
diff --git a/MavLinkCom/include/AsyncResult.hpp b/MavLinkCom/include/AsyncResult.hpp
index 64fb1ba310..f778c5556c 100644
--- a/MavLinkCom/include/AsyncResult.hpp
+++ b/MavLinkCom/include/AsyncResult.hpp
@@ -13,147 +13,166 @@
 
 namespace mavlinkcom {
 
-    template<class T>
-    class AsyncResultState
-    {
-    public:
-        typedef std::function<void(int state)> CompletionHandler;
-        mavlink_utils::Semaphore resultReceived_;
-        T result_;
-        CompletionHandler owner_;
-        int state_ = 0;
-        bool completed_ = false;
-
-        AsyncResultState(CompletionHandler owner) {
-            owner_ = owner;
-        }
+template<class T>
+class AsyncResultState
+{
+public:
+    typedef std::function<void(int state)> CompletionHandler;
+    mavlink_utils::Semaphore resultReceived_;
+    T result_;
+    CompletionHandler owner_;
+    int state_ = 0;
+    bool completed_ = false;
+
+    AsyncResultState(CompletionHandler owner)
+    {
+        owner_ = owner;
+    }
 
-        ~AsyncResultState() {
-            complete();
-        }        
+    ~AsyncResultState()
+    {
+        complete();
+    }        
         
-        int getState() {
-            return state_;
-        }
-        void setState(int s) {
-            state_ = s;
-        }
-        T getResult() {
-            resultReceived_.wait();
-            return result_;
-        }
-        void setResult(T result) {
-            result_ = result;
-            resultReceived_.post();
+    int getState()
+    {
+        return state_;
+    }
+    void setState(int s)
+    {
+        state_ = s;
+    }
+    T getResult()
+    {
+        resultReceived_.wait();
+        return result_;
+    }
+    void setResult(T result)
+    {
+        result_ = result;
+        resultReceived_.post();
+        complete();
+    }
+    bool getResult(int millisecondTimeout, T* r)
+    {
+        if (!resultReceived_.timed_wait(millisecondTimeout)) {
+            // timeout on wait, so complete the task in this case too.
             complete();
+            return false;
         }
-        bool getResult(int millisecondTimeout, T* r) {
-            if (!resultReceived_.timed_wait(millisecondTimeout)) {
-                // timeout on wait, so complete the task in this case too.
-                complete();
-                return false;
-            }
-            *r = result_;
-            return true;
-        }
-        void complete() {
-            CompletionHandler rh = owner_;
-            owner_ = nullptr;
-
-            completed_ = true;
-            if (rh != nullptr) {
-                rh(state_);
-            }
+        *r = result_;
+        return true;
+    }
+    void complete()
+    {
+        CompletionHandler rh = owner_;
+        owner_ = nullptr;
+
+        completed_ = true;
+        if (rh != nullptr) {
+            rh(state_);
         }
-        bool isCompleted() { return completed_; }
-    };
+    }
+    bool isCompleted() { return completed_; }
+};
+
+template<class T>
+class AsyncResult 
+{
+public:
+    typedef std::function<void(T result)> ResultHandler;
+    typedef std::function<void(int state)> CompletionHandler;
 
-    template<class T>
-    class AsyncResult 
+    AsyncResult(CompletionHandler owner)
     {
-    public:
-        typedef std::function<void(T result)> ResultHandler;
-        typedef std::function<void(int state)> CompletionHandler;
+        state_ = std::make_shared<AsyncResultState<T>>(owner);
+    }
+    ~AsyncResult() 
+    {
+        state_ = nullptr;
+    }
 
-        AsyncResult(CompletionHandler owner)
-        {
-            state_ = std::make_shared<AsyncResultState<T>>(owner);
-        }
-        ~AsyncResult() 
-        {
-            state_ = nullptr;
-        }
+    static AsyncResult<T> Completed(T state)
+    {
+        AsyncResult<T> r(nullptr);
+        r.setResult(state);
+        return r;
+    }
 
-        static AsyncResult<T> Completed(T state) {
-            AsyncResult<T> r(nullptr);
-            r.setResult(state);
-            return r;
-        }
+    void then(ResultHandler handler)
+    {
+        // keep state alive while we wait for async result.
+        std::shared_ptr<AsyncResultState<T>> safe(state_);
+        T result = std::async(std::launch::async, getResult, safe).get();
+        handler(result);
+    }
 
-        void then(ResultHandler handler)
-        {
-            // keep state alive while we wait for async result.
-            std::shared_ptr<AsyncResultState<T>> safe(state_);
-            T result = std::async(std::launch::async, getResult, safe).get();
-            handler(result);
-        }
+    int getState()
+    {
+        return state_->getState();
+    }
 
-        int getState() {
-            return state_->getState();
+    void setState(int s) const
+    {
+        state_->setState(s);
+    }
+    void setResult(T result) const
+    {
+        if (state_ != nullptr) {
+            state_->setResult(result);
         }
+    }
 
-        void setState(int s) const {
-            state_->setState(s);
-        }
-        void setResult(T result) const {
-            if (state_ != nullptr) {
-                state_->setResult(result);
-            }
-        }
+    AsyncResult(const AsyncResult<T>& other)
+    {
+        this->state_ = other.state_;
+    }
 
-        AsyncResult(const AsyncResult<T>& other) {
+    AsyncResult<T>& operator=(const AsyncResult<T>& other)
+    {
+        if (this != &other) {
             this->state_ = other.state_;
         }
+        return *this;
+    }
 
-        AsyncResult<T>& operator=(const AsyncResult<T>& other) {
-            if (this != &other) {
-                this->state_ = other.state_;
-            }
-            return *this;
-        }
+    AsyncResult(AsyncResult<T>&& other)
+    {
+        this->state_ = other.state_;
+        other.state_ = nullptr;
+    }
 
-        AsyncResult(AsyncResult<T>&& other) {
+    AsyncResult<T>& operator=(AsyncResult<T>&& other)
+    {
+        if (this != &other) {
             this->state_ = other.state_;
             other.state_ = nullptr;
         }
+        return *this;
+    }
 
-        AsyncResult<T>& operator=(AsyncResult<T>&& other) {
-            if (this != &other) {
-                this->state_ = other.state_;
-                other.state_ = nullptr;
-            }
-            return *this;
-        }
-
-        bool wait(int millisecondTimeout, T* r) {
-            // keep the state alive while we wait.
-            std::shared_ptr<AsyncResultState<T>> safe(state_);
-            return state_->getResult(millisecondTimeout, r);
-        }
+    bool wait(int millisecondTimeout, T* r)
+    {
+        // keep the state alive while we wait.
+        std::shared_ptr<AsyncResultState<T>> safe(state_);
+        return state_->getResult(millisecondTimeout, r);
+    }
 
-        bool isCompleted() {
-            return state_->isCompleted();
-        }
+    bool isCompleted()
+    {
+        return state_->isCompleted();
+    }
 
-    private:
-        static T getResult(std::shared_ptr<AsyncResultState<T>> state) {
-            return state->getResult();
-        }
+private:
+    static T getResult(std::shared_ptr<AsyncResultState<T>> state)
+    {
+        return state->getResult();
+    }
 
 
 
-        std::shared_ptr<AsyncResultState<T>> state_;
-    };
+    std::shared_ptr<AsyncResultState<T>> state_;
+};
 }
 
 #endif
diff --git a/MavLinkCom/include/MavLinkConnection.hpp b/MavLinkCom/include/MavLinkConnection.hpp
index c77693b770..3760072db0 100644
--- a/MavLinkCom/include/MavLinkConnection.hpp
+++ b/MavLinkCom/include/MavLinkConnection.hpp
@@ -21,142 +21,142 @@
 class Port;
 
 namespace mavlinkcom_impl {
-    class MavLinkConnectionImpl;
-    class MavLinkTcpServerImpl;
-    class MavLinkNodeImpl;
+class MavLinkConnectionImpl;
+class MavLinkTcpServerImpl;
+class MavLinkNodeImpl;
 }
 
 namespace mavlinkcom {
 
-    class MavLinkConnection;
-    class MavLinkNode;
-
-    // This callback is invoked when a MavLink message is read from the connection.
-    typedef std::function<void(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)> MessageHandler;
-
-    // This callback is invoked when a new TCP connection is accepted via acceptTcp().
-    typedef std::function<void(std::shared_ptr<MavLinkConnection> port)> MavLinkConnectionHandler;
-
-    struct SerialPortInfo {
-        std::wstring displayName;
-        std::wstring portName;
-        int vid;
-        int pid;
-    };
-
-    // This class represents a single connection to a remote mavlink node connected either over UDP, TCP or Serial port.
-    // You can use this connection in a MavLinkNode to send a message directly to that node, and start listening to messages 
-    // from that remote node.  You can handle those messages directly using subscribe.
-    class MavLinkConnection : public std::enable_shared_from_this<MavLinkConnection>
-    {
-    public:
-        MavLinkConnection();
-
-        // Find available serial ports for the given vendor id/product id pair.  If a matching port is found it returns the
-        // SerialPortInfo of that port, the portName can then be used connectSerial.  Pass 0 for vid and pid to find all 
-        // available serial ports.
-        static std::vector<SerialPortInfo> findSerialPorts(int vid, int pid);
-
-        // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5").
-        // pass initial string to write to the port, which can be used to configure the port.
-        // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams.
-        static std::shared_ptr<MavLinkConnection>  connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = "");
-
-        // Start listening on a specific local port for packets from any remote computer.  Once a packet is received
-        // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender.
-        // This is useful if the remote sender already knows which local port you plan to listen on.
-        // The localAddr can also a specific local ip address if you need to specify which
-        // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets
-        // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet 
-        // adapter rather than 127.0.0.1.
-        static std::shared_ptr<MavLinkConnection>  connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort);
-
-        // Connect to a specific remote machine that is already listening on a specific port for messages from any computer.
-        // This will use any free local port that is available.
-        // The localAddr can also a specific local ip address if you need to specify which
-        // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets
-        // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet 
-        // adapter rather than 127.0.0.1.
-        static std::shared_ptr<MavLinkConnection>  connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort);
-
-        // This method sets up a tcp connection to the specified remote host and port.  The remote host
-        // must already be listening and accepting TCP socket connections for this to succeed. 
-        // The  localAddr can also a specific local ip address if you need to specify which
-        // NIC to use, for example, wifi versus hard wired ethernet adapter.  For localhost pass 127.0.0.1.
-        static std::shared_ptr<MavLinkConnection>  connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort);
-
-        // This method accepts one tcp connection from a remote host on a given port.
-        // You may need to open this port in your firewall.
-        // The  localAddr can also a specific local ip address if you need to specify which
-        // NIC to use, for example, wifi versus hard wired ethernet adapter.  For localhost pass 127.0.0.1.
-        // It returns the address of the remote machine that connected.
-        std::string acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort);
-
-        // instance methods
-        std::string getName();
-        int getTargetComponentId();
-        int getTargetSystemId();
-        bool isOpen();
-        void close();
-
-        // provide a callback function that will be called for every message "received" from the remote mavlink node.
-        int subscribe(MessageHandler handler);
-        void unsubscribe(int id);
-
-        // log every message that is "sent" using sendMessage.
-        void startLoggingSendMessage(std::shared_ptr<MavLinkLog> log);
-        void stopLoggingSendMessage();
-
-        // log every message that is "received" by a subscriber.
-        void startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log);
-        void stopLoggingReceiveMessage();
-
-        uint8_t getNextSequence();
-
-        // Advanced method that create a bridge between two connections.  For example, if you use connectRemoteUdp to connect to 
-        // QGroundControl port 14550, and connectSerial to connect to PX4, then you can call this method to join the two so that
-        // all messages from PX4 are sent to QGroundControl and vice versa.
-        void join(std::shared_ptr<MavLinkConnection> remote, bool subscribeToLeft = true, bool subscribeToRight = true);
-
-        // Pack and send the given message, assuming the compid and sysid have been set by the caller.
-        void sendMessage(const MavLinkMessageBase& msg);
-
-        // Send the given already encoded message, assuming the compid and sysid have been set by the caller.
-        void sendMessage(const MavLinkMessage& msg);
-
-        // get the next telemetry snapshot, then clear the internal counters and start over.  This way each snapshot
-        // gives you a picture of what happened in whatever timeslice you decide to call this method.  This is packaged
-        // in a mavlink message so you can easily send it to the LogViewer.
-        void getTelemetry(MavLinkTelemetry& result);
-
-        //add the message in to list of ignored messages. These messages will not be sent in the sendMessage() call.
-        //this does not effect reception of message, however. This is typically useful in scenario where many connections
-        //are bridged and you don't want certain connection to read ceratin messages.
-        void ignoreMessage(uint8_t message_id);
-
-        // Compute crc checksums, and pack according to mavlink1 or mavlink2 (depending on what target node supports) and do optional 
-        // message signing according to the target node we are communicating with, and return the message length.
-        int prepareForSending(MavLinkMessage& msg);
-
-        // Returns true if we are on the publishing thread.  Certain blocing operations that wait for messages from mavlin vehicle are not
-        // allowed on this thread.
-        bool isPublishThread() const;
-
-    protected:
-        void startListening(const std::string& nodeName, std::shared_ptr<Port> connectedPort);
-
-    public:
-        //needed for piml pattern
-        ~MavLinkConnection();
-        //MavLinkConnection(MavLinkConnection&&);
-        //MavLinkConnection& operator=(MavLinkConnection&&);
-    private:
-        std::unique_ptr<mavlinkcom_impl::MavLinkConnectionImpl> pImpl;
-        friend class MavLinkNode;
-        friend class mavlinkcom_impl::MavLinkNodeImpl;
-        friend class mavlinkcom_impl::MavLinkConnectionImpl;
-        friend class mavlinkcom_impl::MavLinkTcpServerImpl;
-    };
+class MavLinkConnection;
+class MavLinkNode;
+
+// This callback is invoked when a MavLink message is read from the connection.
+typedef std::function<void(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& msg)> MessageHandler;
+
+// This callback is invoked when a new TCP connection is accepted via acceptTcp().
+typedef std::function<void(std::shared_ptr<MavLinkConnection> port)> MavLinkConnectionHandler;
+
+struct SerialPortInfo {
+    std::wstring displayName;
+    std::wstring portName;
+    int vid;
+    int pid;
+};
+
+// This class represents a single connection to a remote mavlink node connected either over UDP, TCP or Serial port.
+// You can use this connection in a MavLinkNode to send a message directly to that node, and start listening to messages 
+// from that remote node.  You can handle those messages directly using subscribe.
+class MavLinkConnection : public std::enable_shared_from_this<MavLinkConnection>
+{
+public:
+    MavLinkConnection();
+
+    // Find available serial ports for the given vendor id/product id pair.  If a matching port is found it returns the
+    // SerialPortInfo of that port, the portName can then be used connectSerial.  Pass 0 for vid and pid to find all 
+    // available serial ports.
+    static std::vector<SerialPortInfo> findSerialPorts(int vid, int pid);
+
+    // create connection over serial port (e.g. /dev/ttyACM0 or on windows "com5").
+    // pass initial string to write to the port, which can be used to configure the port.
+    // For example, on PX4 you can send "sh /etc/init.d/rc.usb\n" to turn on lots of mavlink streams.
+    static std::shared_ptr<MavLinkConnection>  connectSerial(const std::string& nodeName, const std::string& portName, int baudrate = 115200, const std::string& initString = "");
+
+    // Start listening on a specific local port for packets from any remote computer.  Once a packet is received
+    // it will remember the remote address of the sender so that subsequend sendMessage calls will go back to that sender.
+    // This is useful if the remote sender already knows which local port you plan to listen on.
+    // The localAddr can also a specific local ip address if you need to specify which
+    // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets
+    // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet 
+    // adapter rather than 127.0.0.1.
+    static std::shared_ptr<MavLinkConnection>  connectLocalUdp(const std::string& nodeName, const std::string& localAddr, int localPort);
+
+    // Connect to a specific remote machine that is already listening on a specific port for messages from any computer.
+    // This will use any free local port that is available.
+    // The localAddr can also a specific local ip address if you need to specify which
+    // network interface to use, for example, a corporate wired ethernet usually does not transmit UDP packets
+    // to a wifi connected device, so in that case the localAddress needs to be the IP address of a specific wifi internet 
+    // adapter rather than 127.0.0.1.
+    static std::shared_ptr<MavLinkConnection>  connectRemoteUdp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteAddr, int remotePort);
+
+    // This method sets up a tcp connection to the specified remote host and port.  The remote host
+    // must already be listening and accepting TCP socket connections for this to succeed. 
+    // The  localAddr can also a specific local ip address if you need to specify which
+    // NIC to use, for example, wifi versus hard wired ethernet adapter.  For localhost pass 127.0.0.1.
+    static std::shared_ptr<MavLinkConnection>  connectTcp(const std::string& nodeName, const std::string& localAddr, const std::string& remoteIpAddr, int remotePort);
+
+    // This method accepts one tcp connection from a remote host on a given port.
+    // You may need to open this port in your firewall.
+    // The  localAddr can also a specific local ip address if you need to specify which
+    // NIC to use, for example, wifi versus hard wired ethernet adapter.  For localhost pass 127.0.0.1.
+    // It returns the address of the remote machine that connected.
+    std::string acceptTcp(const std::string& nodeName, const std::string& localAddr, int listeningPort);
+
+    // instance methods
+    std::string getName();
+    int getTargetComponentId();
+    int getTargetSystemId();
+    bool isOpen();
+    void close();
+
+    // provide a callback function that will be called for every message "received" from the remote mavlink node.
+    int subscribe(MessageHandler handler);
+    void unsubscribe(int id);
+
+    // log every message that is "sent" using sendMessage.
+    void startLoggingSendMessage(std::shared_ptr<MavLinkLog> log);
+    void stopLoggingSendMessage();
+
+    // log every message that is "received" by a subscriber.
+    void startLoggingReceiveMessage(std::shared_ptr<MavLinkLog> log);
+    void stopLoggingReceiveMessage();
+
+    uint8_t getNextSequence();
+
+    // Advanced method that create a bridge between two connections.  For example, if you use connectRemoteUdp to connect to 
+    // QGroundControl port 14550, and connectSerial to connect to PX4, then you can call this method to join the two so that
+    // all messages from PX4 are sent to QGroundControl and vice versa.
+    void join(std::shared_ptr<MavLinkConnection> remote, bool subscribeToLeft = true, bool subscribeToRight = true);
+
+    // Pack and send the given message, assuming the compid and sysid have been set by the caller.
+    void sendMessage(const MavLinkMessageBase& msg);
+
+    // Send the given already encoded message, assuming the compid and sysid have been set by the caller.
+    void sendMessage(const MavLinkMessage& msg);
+
+    // get the next telemetry snapshot, then clear the internal counters and start over.  This way each snapshot
+    // gives you a picture of what happened in whatever timeslice you decide to call this method.  This is packaged
+    // in a mavlink message so you can easily send it to the LogViewer.
+    void getTelemetry(MavLinkTelemetry& result);
+
+    //add the message in to list of ignored messages. These messages will not be sent in the sendMessage() call.
+    //this does not effect reception of message, however. This is typically useful in scenario where many connections
+    //are bridged and you don't want certain connection to read ceratin messages.
+    void ignoreMessage(uint8_t message_id);
+
+    // Compute crc checksums, and pack according to mavlink1 or mavlink2 (depending on what target node supports) and do optional 
+    // message signing according to the target node we are communicating with, and return the message length.
+    int prepareForSending(MavLinkMessage& msg);
+
+    // Returns true if we are on the publishing thread.  Certain blocing operations that wait for messages from mavlin vehicle are not
+    // allowed on this thread.
+    bool isPublishThread() const;
+
+protected:
+    void startListening(const std::string& nodeName, std::shared_ptr<Port> connectedPort);
+
+public:
+    //needed for piml pattern
+    ~MavLinkConnection();
+    //MavLinkConnection(MavLinkConnection&&);
+    //MavLinkConnection& operator=(MavLinkConnection&&);
+private:
+    std::unique_ptr<mavlinkcom_impl::MavLinkConnectionImpl> pImpl;
+    friend class MavLinkNode;
+    friend class mavlinkcom_impl::MavLinkNodeImpl;
+    friend class mavlinkcom_impl::MavLinkConnectionImpl;
+    friend class mavlinkcom_impl::MavLinkTcpServerImpl;
+};
 }
 
 #endif
diff --git a/MavLinkCom/include/MavLinkDebugLog.hpp b/MavLinkCom/include/MavLinkDebugLog.hpp
index 4dada3d665..14e2cceb7b 100644
--- a/MavLinkCom/include/MavLinkDebugLog.hpp
+++ b/MavLinkCom/include/MavLinkDebugLog.hpp
@@ -9,29 +9,30 @@
 
 namespace mavlinkcom
 {
-    class MavLinkDebugLog {
-    public:
-        virtual void log(int level, const std::string& message)
-        {
-            if (level >= 0)
-                std::cout << message;
-            else
-                std::cerr << message;
-        }
+class MavLinkDebugLog
+{
+public:
+    virtual void log(int level, const std::string& message)
+    {
+        if (level >= 0)
+            std::cout << message;
+        else
+            std::cerr << message;
+    }
 
-        static MavLinkDebugLog* getSetLogger(MavLinkDebugLog* logger = nullptr)
-        {
-            static MavLinkDebugLog logger_default_;
-            static MavLinkDebugLog* logger_;
+    static MavLinkDebugLog* getSetLogger(MavLinkDebugLog* logger = nullptr)
+    {
+        static MavLinkDebugLog logger_default_;
+        static MavLinkDebugLog* logger_;
 
-            if (logger != nullptr)
-                logger_ = logger;
-            else if (logger_ == nullptr)
-                logger_ = &logger_default_;
+        if (logger != nullptr)
+            logger_ = logger;
+        else if (logger_ == nullptr)
+            logger_ = &logger_default_;
 
-            return logger_;
-        }
-    };
+        return logger_;
+    }
+};
 }
 
 #endif
\ No newline at end of file
diff --git a/MavLinkCom/include/MavLinkLog.hpp b/MavLinkCom/include/MavLinkLog.hpp
index 3c22f95b84..922dfeaa47 100644
--- a/MavLinkCom/include/MavLinkLog.hpp
+++ b/MavLinkCom/include/MavLinkLog.hpp
@@ -14,34 +14,34 @@
 
 namespace mavlinkcom
 {
-    // This abstract class defines the interface for logging MavLinkMessages.
-    class MavLinkLog
-    {
-    public:
-        virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) = 0;
-        virtual ~MavLinkLog() = default;
-    };
-
-    // This implementation of MavLinkLog reads/writes MavLinkMessages to a local file.
-    class MavLinkFileLog : public MavLinkLog
-    {
-        std::string file_name_;
-        FILE* ptr_;
-        bool reading_;
-        bool writing_;
-        bool json_;
-        std::mutex log_lock_;
-    public:
-        MavLinkFileLog();
-        virtual ~MavLinkFileLog();
-        bool isOpen();
-        void openForReading(const std::string& filename);
-        void openForWriting(const std::string& filename, bool json = false);
-        void close();
-        virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override;
-        bool read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp);
-        static uint64_t getTimeStamp();
-    };
+// This abstract class defines the interface for logging MavLinkMessages.
+class MavLinkLog
+{
+public:
+    virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) = 0;
+    virtual ~MavLinkLog() = default;
+};
+
+// This implementation of MavLinkLog reads/writes MavLinkMessages to a local file.
+class MavLinkFileLog : public MavLinkLog
+{
+    std::string file_name_;
+    FILE* ptr_;
+    bool reading_;
+    bool writing_;
+    bool json_;
+    std::mutex log_lock_;
+public:
+    MavLinkFileLog();
+    virtual ~MavLinkFileLog();
+    bool isOpen();
+    void openForReading(const std::string& filename);
+    void openForWriting(const std::string& filename, bool json = false);
+    void close();
+    virtual void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override;
+    bool read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp);
+    static uint64_t getTimeStamp();
+};
 
 
 }
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index 570e0b654f..9444ee397a 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -14,167 +14,170 @@ namespace mavlinkcom_impl {
 }
 namespace mavlinkcom
 {
-    class MavLinkConnection;
+class MavLinkConnection;
 
 #define PayloadSize ((255 + 2 + 7) / 8)
 
-    // This is the raw undecoded message, use the msgid field to figure out which strongly typed
-    // MavLinkMessageBase subclass can be used to decode this message.  For example, a heartbeat:
-    // MavLinkMessage msg;
-    // if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
-    //     MavLinkHeartbeat heartbeat;
-    //     heartbeat.decode(msg);
-    //     ...
-    // }
-    class MavLinkMessage {
-    public:
-        uint16_t checksum;      ///< sent at end of packet
-        uint8_t magic;          ///< protocol magic marker
-        uint8_t len;            ///< Length of payload
-        uint8_t incompat_flags; ///< flags that must be understood
-        uint8_t compat_flags;   ///< flags that can be ignored if not understood
-        uint8_t seq;            ///< Sequence of packet
-        uint8_t sysid;          ///< ID of message sender system/aircraft
-        uint8_t compid;         ///< ID of the message sender component
-        uint32_t msgid : 24;      ///< ID of message in payload
-        uint64_t payload64[PayloadSize];
-        uint8_t ck[2];          ///< incoming checksum bytes
-        uint8_t signature[13];
-        uint8_t protocol_version;
-
-        // initialize checksum
-        int update_checksum();
-    };
-
-    // This is the base class for all the strongly typed messages define in MavLinkMessages.hpp
-    class MavLinkMessageBase
-    {
-    public:
-        uint32_t msgid = 0;
-        uint8_t sysid = 0;   ///< ID of message sender system/aircraft
-        uint8_t compid = 0;  ///< ID of the message sender component
-        uint64_t timestamp = 0;
-        uint8_t protocol_version = 0;
-
-        // unpack the given message
-        void decode(const MavLinkMessage& msg);
-        // pack this message into given message buffer 
-        void encode(MavLinkMessage& msg) const;
-
-        // find what type of message this is and decode it on the heap (call delete when you are done with it).
-        static MavLinkMessageBase* lookup(const MavLinkMessage& msg);
-        virtual std::string toJSon() = 0;
-        virtual ~MavLinkMessageBase() {}
-    protected:
-        virtual int pack(char* buffer) const = 0;
-        virtual int unpack(const char* buffer) = 0;
-
-        void pack_uint8_t(char* buffer, const uint8_t* field, int offset) const;
-        void pack_int8_t(char* buffer, const int8_t* field, int offset) const;
-        void pack_int16_t(char* buffer, const int16_t* field, int offset) const;
-        void pack_uint16_t(char* buffer, const uint16_t* field, int offset) const;
-        void pack_uint32_t(char* buffer, const uint32_t* field, int offset) const;
-        void pack_int32_t(char* buffer, const int32_t* field, int offset) const;
-        void pack_uint64_t(char* buffer, const uint64_t* field, int offset) const;
-        void pack_int64_t(char* buffer, const int64_t* field, int offset) const;
-        void pack_float(char* buffer, const float* field, int offset) const;
-        void pack_char_array(int len, char* buffer, const char* field, int offset) const;
-        void pack_uint8_t_array(int len, char* buffer, const uint8_t* field, int offset) const;
-        void pack_int8_t_array(int len, char* buffer, const int8_t* field, int offset) const;
-        void pack_uint16_t_array(int len, char* buffer, const uint16_t* field, int offset) const;
-        void pack_int16_t_array(int len, char* buffer, const int16_t* field, int offset) const;
-        void pack_float_array(int len, char* buffer, const float* field, int offset) const;
+// This is the raw undecoded message, use the msgid field to figure out which strongly typed
+// MavLinkMessageBase subclass can be used to decode this message.  For example, a heartbeat:
+// MavLinkMessage msg;
+// if (msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+//     MavLinkHeartbeat heartbeat;
+//     heartbeat.decode(msg);
+//     ...
+// }
+class MavLinkMessage
+{
+public:
+    uint16_t checksum;      ///< sent at end of packet
+    uint8_t magic;          ///< protocol magic marker
+    uint8_t len;            ///< Length of payload
+    uint8_t incompat_flags; ///< flags that must be understood
+    uint8_t compat_flags;   ///< flags that can be ignored if not understood
+    uint8_t seq;            ///< Sequence of packet
+    uint8_t sysid;          ///< ID of message sender system/aircraft
+    uint8_t compid;         ///< ID of the message sender component
+    uint32_t msgid : 24;      ///< ID of message in payload
+    uint64_t payload64[PayloadSize];
+    uint8_t ck[2];          ///< incoming checksum bytes
+    uint8_t signature[13];
+    uint8_t protocol_version;
+
+    // initialize checksum
+    int update_checksum();
+};
+
+// This is the base class for all the strongly typed messages define in MavLinkMessages.hpp
+class MavLinkMessageBase
+{
+public:
+    uint32_t msgid = 0;
+    uint8_t sysid = 0;   ///< ID of message sender system/aircraft
+    uint8_t compid = 0;  ///< ID of the message sender component
+    uint64_t timestamp = 0;
+    uint8_t protocol_version = 0;
+
+    // unpack the given message
+    void decode(const MavLinkMessage& msg);
+    // pack this message into given message buffer 
+    void encode(MavLinkMessage& msg) const;
+
+    // find what type of message this is and decode it on the heap (call delete when you are done with it).
+    static MavLinkMessageBase* lookup(const MavLinkMessage& msg);
+    virtual std::string toJSon() = 0;
+    virtual ~MavLinkMessageBase() {}
+protected:
+    virtual int pack(char* buffer) const = 0;
+    virtual int unpack(const char* buffer) = 0;
+
+    void pack_uint8_t(char* buffer, const uint8_t* field, int offset) const;
+    void pack_int8_t(char* buffer, const int8_t* field, int offset) const;
+    void pack_int16_t(char* buffer, const int16_t* field, int offset) const;
+    void pack_uint16_t(char* buffer, const uint16_t* field, int offset) const;
+    void pack_uint32_t(char* buffer, const uint32_t* field, int offset) const;
+    void pack_int32_t(char* buffer, const int32_t* field, int offset) const;
+    void pack_uint64_t(char* buffer, const uint64_t* field, int offset) const;
+    void pack_int64_t(char* buffer, const int64_t* field, int offset) const;
+    void pack_float(char* buffer, const float* field, int offset) const;
+    void pack_char_array(int len, char* buffer, const char* field, int offset) const;
+    void pack_uint8_t_array(int len, char* buffer, const uint8_t* field, int offset) const;
+    void pack_int8_t_array(int len, char* buffer, const int8_t* field, int offset) const;
+    void pack_uint16_t_array(int len, char* buffer, const uint16_t* field, int offset) const;
+    void pack_int16_t_array(int len, char* buffer, const int16_t* field, int offset) const;
+    void pack_float_array(int len, char* buffer, const float* field, int offset) const;
         
-        void unpack_uint8_t(const char* buffer, uint8_t* field, int offset);
-        void unpack_int8_t(const char* buffer, int8_t* field, int offset);
-        void unpack_int16_t(const char* buffer, int16_t* field, int offset);
-        void unpack_uint16_t(const char* buffer, uint16_t* field, int offset);
-        void unpack_uint32_t(const char* buffer, uint32_t* field, int offset);
-        void unpack_int32_t(const char* buffer, int32_t* field, int offset);
-        void unpack_uint64_t(const char* buffer, uint64_t* field, int offset);
-        void unpack_int64_t(const char* buffer, int64_t* field, int offset);
-        void unpack_float(const char* buffer, float* field, int offset);
-        void unpack_char_array(int len, const char* buffer, char* field, int offset);
-        void unpack_uint8_t_array(int len, const char* buffer, uint8_t* field, int offset);
-        void unpack_int8_t_array(int len, const char* buffer, int8_t* field, int offset);
-        void unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset);
-        void unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset);
-        void unpack_float_array(int len, const char* buffer, float* field, int offset);
-
-        std::string char_array_tostring(int len, const char* field);
-        std::string uint8_t_array_tostring(int len, const uint8_t* field);
-        std::string int8_t_array_tostring(int len, const int8_t* field);
-        std::string int16_t_array_tostring(int len, const int16_t* field);
-        std::string uint16_t_array_tostring(int len, const uint16_t* field);
-        std::string float_array_tostring(int len, const float* field);
-        std::string float_tostring(float value); 
-    };
-
-    // Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp
-    class MavLinkCommand {
-    public:
-        uint16_t command = 0;
-    protected:
-        virtual void pack() = 0;
-        virtual void unpack() = 0;
-
-        float param1 = 0;
-        float param2 = 0;
-        float param3 = 0;
-        float param4 = 0;
-        float param5 = 0;
-        float param6 = 0;
-        float param7 = 0;
-
-        friend class mavlinkcom_impl::MavLinkNodeImpl;
-        friend class mavlinkcom_impl::MavLinkConnectionImpl;
-    };
-
-
-    // The location of a landing area captured from a downward facing camera
-    class MavLinkTelemetry : public MavLinkMessageBase {
-    public:
-        const static uint8_t kMessageId = 204; // in the user range 180-229.
-        const static int MessageLength = 12 * 4;
-        MavLinkTelemetry() { msgid = kMessageId; }
-        uint32_t messages_sent = 0;		   // number of messages sent since the last telemetry message
-        uint32_t messages_received = 0;	   // number of messages received since the last telemetry message
-        uint32_t messages_handled = 0;	   // number of messages handled since the last telemetry message
-        uint32_t crc_errors = 0;		   // # crc errors detected in mavlink stream since the last telemetry message
-        uint32_t handler_microseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
-        uint32_t render_time = 0;          // total time spent rendering frames since the last telemetry message
-        int32_t wifi_rssi = 0;             // if this device is communicating over wifi this is the signal strength.
-        uint32_t udpate_rate = 0;          // rate at which update() is being called on MavLinkMultiRotorApi
-        uint32_t actuation_delay = 0;      // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response
-        uint32_t sensor_rate = 0;          // rate we are sending HIL_SENSOR messages
-        uint32_t lock_step_resets = 0;     // total number of lock_step resets
-        uint32_t update_time = 0;          // time inside MavLinkMultiRotorApi::update() method
+    void unpack_uint8_t(const char* buffer, uint8_t* field, int offset);
+    void unpack_int8_t(const char* buffer, int8_t* field, int offset);
+    void unpack_int16_t(const char* buffer, int16_t* field, int offset);
+    void unpack_uint16_t(const char* buffer, uint16_t* field, int offset);
+    void unpack_uint32_t(const char* buffer, uint32_t* field, int offset);
+    void unpack_int32_t(const char* buffer, int32_t* field, int offset);
+    void unpack_uint64_t(const char* buffer, uint64_t* field, int offset);
+    void unpack_int64_t(const char* buffer, int64_t* field, int offset);
+    void unpack_float(const char* buffer, float* field, int offset);
+    void unpack_char_array(int len, const char* buffer, char* field, int offset);
+    void unpack_uint8_t_array(int len, const char* buffer, uint8_t* field, int offset);
+    void unpack_int8_t_array(int len, const char* buffer, int8_t* field, int offset);
+    void unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset);
+    void unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset);
+    void unpack_float_array(int len, const char* buffer, float* field, int offset);
+
+    std::string char_array_tostring(int len, const char* field);
+    std::string uint8_t_array_tostring(int len, const uint8_t* field);
+    std::string int8_t_array_tostring(int len, const int8_t* field);
+    std::string int16_t_array_tostring(int len, const int16_t* field);
+    std::string uint16_t_array_tostring(int len, const uint16_t* field);
+    std::string float_array_tostring(int len, const float* field);
+    std::string float_tostring(float value); 
+};
+
+// Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp
+class MavLinkCommand
+{
+public:
+    uint16_t command = 0;
+protected:
+    virtual void pack() = 0;
+    virtual void unpack() = 0;
+
+    float param1 = 0;
+    float param2 = 0;
+    float param3 = 0;
+    float param4 = 0;
+    float param5 = 0;
+    float param6 = 0;
+    float param7 = 0;
+
+    friend class mavlinkcom_impl::MavLinkNodeImpl;
+    friend class mavlinkcom_impl::MavLinkConnectionImpl;
+};
+
+
+// The location of a landing area captured from a downward facing camera
+class MavLinkTelemetry : public MavLinkMessageBase
+{
+public:
+    const static uint8_t kMessageId = 204; // in the user range 180-229.
+    const static int MessageLength = 12 * 4;
+    MavLinkTelemetry() { msgid = kMessageId; }
+    uint32_t messages_sent = 0;		   // number of messages sent since the last telemetry message
+    uint32_t messages_received = 0;	   // number of messages received since the last telemetry message
+    uint32_t messages_handled = 0;	   // number of messages handled since the last telemetry message
+    uint32_t crc_errors = 0;		   // # crc errors detected in mavlink stream since the last telemetry message
+    uint32_t handler_microseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
+    uint32_t render_time = 0;          // total time spent rendering frames since the last telemetry message
+    int32_t wifi_rssi = 0;             // if this device is communicating over wifi this is the signal strength.
+    uint32_t udpate_rate = 0;          // rate at which update() is being called on MavLinkMultiRotorApi
+    uint32_t actuation_delay = 0;      // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response
+    uint32_t sensor_rate = 0;          // rate we are sending HIL_SENSOR messages
+    uint32_t lock_step_resets = 0;     // total number of lock_step resets
+    uint32_t update_time = 0;          // time inside MavLinkMultiRotorApi::update() method
  
-        // not serialized
-        const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on.
-        virtual std::string toJSon() {
-
-            std::ostringstream result;
-            result << "\"MavLinkTelemetry\"" << " : { ";
-            result << "\"messages_sent\":" << this->messages_sent << ",";
-            result << "\"messages_received\":" << this->messages_received << ",";
-            result << "\"messages_handled\":" << this->messages_handled << ",";
-            result << "\"crc_errors\":" << this->crc_errors << ",";
-            result << "\"handler_microseconds\":" << this->handler_microseconds << ",";
-            result << "\"render_time\":" << this->render_time;
-            result << "\"wifi_rssi\":" << this->wifi_rssi;
-            result << "\"udpate_rate\":" << this->udpate_rate;
-            result << "\"actuation_delay\":" << this->actuation_delay;
-            result << "\"sensor_rate\":" << this->sensor_rate;
-            result << "\"lock_step_resets\":" << this->lock_step_resets;
-            result << "\"udpate_time\":" << this->update_time;
-            result << "}";
-            return result.str();
-        }
-    protected:
-        virtual int pack(char* buffer) const;
-        virtual int unpack(const char* buffer);
-    };
+    // not serialized
+    const char* wifiInterfaceName = nullptr; // the name of the wifi interface we are measuring RSSI on.
+    virtual std::string toJSon() {
+
+        std::ostringstream result;
+        result << "\"MavLinkTelemetry\"" << " : { ";
+        result << "\"messages_sent\":" << this->messages_sent << ",";
+        result << "\"messages_received\":" << this->messages_received << ",";
+        result << "\"messages_handled\":" << this->messages_handled << ",";
+        result << "\"crc_errors\":" << this->crc_errors << ",";
+        result << "\"handler_microseconds\":" << this->handler_microseconds << ",";
+        result << "\"render_time\":" << this->render_time;
+        result << "\"wifi_rssi\":" << this->wifi_rssi;
+        result << "\"udpate_rate\":" << this->udpate_rate;
+        result << "\"actuation_delay\":" << this->actuation_delay;
+        result << "\"sensor_rate\":" << this->sensor_rate;
+        result << "\"lock_step_resets\":" << this->lock_step_resets;
+        result << "\"udpate_time\":" << this->update_time;
+        result << "}";
+        return result.str();
+    }
+protected:
+    virtual int pack(char* buffer) const;
+    virtual int unpack(const char* buffer);
+};
 
 }
 
diff --git a/MavLinkCom/include/MavLinkNode.hpp b/MavLinkCom/include/MavLinkNode.hpp
index be754090eb..0483dbbcc7 100644
--- a/MavLinkCom/include/MavLinkNode.hpp
+++ b/MavLinkCom/include/MavLinkNode.hpp
@@ -15,94 +15,95 @@ namespace mavlinkcom_impl {
 }
 namespace mavlinkcom {
 
-    struct MavLinkParameter {
-    public:
-        std::string name;
-        int index = -1;
-        float value = 0;
-        uint8_t type = 0;
-    };
-
-    class MavLinkCommand;
-
-    // This is the base class for all MavLink objects that represent a single
-    // mavlink node.  This class provides high level state that applies to any
-    // kind of mavlink node whether it is a vehicle, ground control, simulator
-    // or log viewer.   This base class also implements the heartbeat protocol
-    // which pings heart beats back to the remote node to ensure it knows our
-    // connection is still alive.
-    class MavLinkNode
-    {
-    public:
-        MavLinkNode(int localSystemId, int localComponentId);
-        ~MavLinkNode();
-
-        // start listening to this connection
-        void connect(std::shared_ptr<MavLinkConnection> connection);
-
-        // stop listening to the connection.
-        void close();
-
-        // Send heartbeat to drone.  You should not do this if some other node is
-        // already doing it.
-        void startHeartbeat();
-
-        // get the list of configurable parameters supported by this node.
-        std::vector<MavLinkParameter> getParamList();
+struct MavLinkParameter
+{
+public:
+    std::string name;
+    int index = -1;
+    float value = 0;
+    uint8_t type = 0;
+};
+
+class MavLinkCommand;
+
+// This is the base class for all MavLink objects that represent a single
+// mavlink node.  This class provides high level state that applies to any
+// kind of mavlink node whether it is a vehicle, ground control, simulator
+// or log viewer.   This base class also implements the heartbeat protocol
+// which pings heart beats back to the remote node to ensure it knows our
+// connection is still alive.
+class MavLinkNode
+{
+public:
+    MavLinkNode(int localSystemId, int localComponentId);
+    ~MavLinkNode();
+
+    // start listening to this connection
+    void connect(std::shared_ptr<MavLinkConnection> connection);
+
+    // stop listening to the connection.
+    void close();
+
+    // Send heartbeat to drone.  You should not do this if some other node is
+    // already doing it.
+    void startHeartbeat();
+
+    // get the list of configurable parameters supported by this node.
+    std::vector<MavLinkParameter> getParamList();
         
-        // get the parameter from last getParamList download.
-        MavLinkParameter getCachedParameter(const std::string& name);
+    // get the parameter from last getParamList download.
+    MavLinkParameter getCachedParameter(const std::string& name);
 
-        // get a single parameter by name.
-        AsyncResult<MavLinkParameter> getParameter(const std::string& name);
+    // get a single parameter by name.
+    AsyncResult<MavLinkParameter> getParameter(const std::string& name);
 
-        // set a new value on a given parameter.  
-        // it is best if you use getParameter to get the current value, see if it
-        // needs changing, change the value, then call setParameter with the same parameter object.
-        AsyncResult<bool> setParameter(MavLinkParameter p);
+    // set a new value on a given parameter.  
+    // it is best if you use getParameter to get the current value, see if it
+    // needs changing, change the value, then call setParameter with the same parameter object.
+    AsyncResult<bool> setParameter(MavLinkParameter p);
 
-        // get the connection 
-        std::shared_ptr<MavLinkConnection> getConnection();
+    // get the connection 
+    std::shared_ptr<MavLinkConnection> getConnection();
 
-        // get the capabilities of the drone
-        AsyncResult<MavLinkAutopilotVersion> getCapabilities();
+    // get the capabilities of the drone
+    AsyncResult<MavLinkAutopilotVersion> getCapabilities();
 
-        // request that target node sends this message stream at given frequency (specify messages per second that you want).
-        // where msgId is a MAVLINK_MSG_ID_* enum value.
-        void setMessageInterval(int msgId, int frequency);
+    // request that target node sends this message stream at given frequency (specify messages per second that you want).
+    // where msgId is a MAVLINK_MSG_ID_* enum value.
+    void setMessageInterval(int msgId, int frequency);
 
-        // wait a given amount of time for a heart beat, and return the decoded message.
-        AsyncResult<MavLinkHeartbeat> waitForHeartbeat();
+    // wait a given amount of time for a heart beat, and return the decoded message.
+    AsyncResult<MavLinkHeartbeat> waitForHeartbeat();
 
-        // send a single heartbeat
-        void sendOneHeartbeat();
+    // send a single heartbeat
+    void sendOneHeartbeat();
 
-        // Get the local system and component id
-        int getLocalSystemId();
-        int getLocalComponentId();
+    // Get the local system and component id
+    int getLocalSystemId();
+    int getLocalComponentId();
 
-        // Get the target system and component id on the other end of the connection.
-        int getTargetSystemId();
-        int getTargetComponentId();
+    // Get the target system and component id on the other end of the connection.
+    int getTargetSystemId();
+    int getTargetComponentId();
 
-        // Send a message to the remote node
-        void sendMessage(MavLinkMessageBase& msg);
+    // Send a message to the remote node
+    void sendMessage(MavLinkMessageBase& msg);
 
-        // Send raw undecoded messge to remote node (this is handy in proxy scenarios where you are simply
-        // passing a message along).
-        void sendMessage(MavLinkMessage& msg);
+    // Send raw undecoded messge to remote node (this is handy in proxy scenarios where you are simply
+    // passing a message along).
+    void sendMessage(MavLinkMessage& msg);
 
-        // send a command to the remote node        
-        void sendCommand(MavLinkCommand& cmd);
+    // send a command to the remote node        
+    void sendCommand(MavLinkCommand& cmd);
 
-        // send a command to the remote node and get async result that tells you whether it succeeded or not.
-        AsyncResult<bool> sendCommandAndWaitForAck(MavLinkCommand& cmd);
-    public:
-        MavLinkNode();
-        //MavLinkNode(MavLinkNode&&);
-    protected:
-        std::unique_ptr<mavlinkcom_impl::MavLinkNodeImpl> pImpl;
-    };
+    // send a command to the remote node and get async result that tells you whether it succeeded or not.
+    AsyncResult<bool> sendCommandAndWaitForAck(MavLinkCommand& cmd);
+public:
+    MavLinkNode();
+    //MavLinkNode(MavLinkNode&&);
+protected:
+    std::unique_ptr<mavlinkcom_impl::MavLinkNodeImpl> pImpl;
+};
 }
 
 #endif
diff --git a/MavLinkCom/include/MavLinkVehicle.hpp b/MavLinkCom/include/MavLinkVehicle.hpp
index 9607041bbf..c29ae4d4b2 100644
--- a/MavLinkCom/include/MavLinkVehicle.hpp
+++ b/MavLinkCom/include/MavLinkVehicle.hpp
@@ -17,64 +17,65 @@ namespace mavlinkcom_impl {
 
 namespace mavlinkcom {
 
-    // This class represents a MavLinkNode that can be controlled, hence a "vehicle" of some sort.
-    // It also keeps certain state about the vehicle position so you can query it any time.
-    // All x,y,z coordinates are in the NED coordinate system.
-    class MavLinkVehicle : public MavLinkNode {
-    public:
-        MavLinkVehicle(int localSystemId, int localComponentId);
+// This class represents a MavLinkNode that can be controlled, hence a "vehicle" of some sort.
+// It also keeps certain state about the vehicle position so you can query it any time.
+// All x,y,z coordinates are in the NED coordinate system.
+class MavLinkVehicle : public MavLinkNode
+{
+public:
+    MavLinkVehicle(int localSystemId, int localComponentId);
 
-        // Send command to arm or disarm the drone.  Drone will not fly until it is armed successfully.
-        // It returns false if the command is rejected.
-        AsyncResult<bool> armDisarm(bool arm);
-        AsyncResult<bool> takeoff(float z = -2.5, float pitch = 0, float yaw = 0);
-        AsyncResult<bool> land(float yaw, float  lat = 0, float lon = 0, float altitude = 0);
-        AsyncResult<bool> returnToHome();
-        AsyncResult<bool> loiter();
-        AsyncResult<bool> setPositionHoldMode();
-        AsyncResult<bool> setStabilizedFlightMode();
-        AsyncResult<bool> setHomePosition(float lat = 0, float lon = 0, float alt = 0);
-        AsyncResult<bool> setMissionMode();
-        AsyncResult<MavLinkHomePosition> waitForHomePosition();
-        AsyncResult<bool> allowFlightControlOverUsb();
-        AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
+    // Send command to arm or disarm the drone.  Drone will not fly until it is armed successfully.
+    // It returns false if the command is rejected.
+    AsyncResult<bool> armDisarm(bool arm);
+    AsyncResult<bool> takeoff(float z = -2.5, float pitch = 0, float yaw = 0);
+    AsyncResult<bool> land(float yaw, float  lat = 0, float lon = 0, float altitude = 0);
+    AsyncResult<bool> returnToHome();
+    AsyncResult<bool> loiter();
+    AsyncResult<bool> setPositionHoldMode();
+    AsyncResult<bool> setStabilizedFlightMode();
+    AsyncResult<bool> setHomePosition(float lat = 0, float lon = 0, float alt = 0);
+    AsyncResult<bool> setMissionMode();
+    AsyncResult<MavLinkHomePosition> waitForHomePosition();
+    AsyncResult<bool> allowFlightControlOverUsb();
+    AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
 
-        // wait for drone to reach specified local z, ensure it is not just blowing past the z location,
-        // wait for it to settle down with dz delta, and dvz delta velocity.
-        AsyncResult<bool> waitForAltitude(float z, float dz, float dvz);
+    // wait for drone to reach specified local z, ensure it is not just blowing past the z location,
+    // wait for it to settle down with dz delta, and dvz delta velocity.
+    AsyncResult<bool> waitForAltitude(float z, float dz, float dvz);
 
-        // request OFFBOARD control.  
-        void requestControl();
-        // release OFFBOARD control
-        void releaseControl();
-        // return true if we still have offboard control (can lose this if user flips the switch).
-        bool hasOffboardControl();
+    // request OFFBOARD control.  
+    void requestControl();
+    // release OFFBOARD control
+    void releaseControl();
+    // return true if we still have offboard control (can lose this if user flips the switch).
+    bool hasOffboardControl();
 
-        // offboard control methods.
-        bool isLocalControlSupported();
-        void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate);
-        void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate);
-        void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate);
-        void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate);
-        void writeMessage(MavLinkMessageBase& message, bool update_stats = true);
+    // offboard control methods.
+    bool isLocalControlSupported();
+    void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate);
+    void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate);
+    void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate);
+    void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate);
+    void writeMessage(MavLinkMessageBase& message, bool update_stats = true);
 
-        // low level control, only use this one if you really know what you are doing!!
-        bool isAttitudeControlSupported();
+    // low level control, only use this one if you really know what you are doing!!
+    bool isAttitudeControlSupported();
 
-        // Move drone by directly controlling the attitude of the drone (units are degrees).
-        // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone.
-        void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust);
+    // Move drone by directly controlling the attitude of the drone (units are degrees).
+    // If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone.
+    void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust);
          
-        uint32_t getTimeStamp();
-        int getVehicleStateVersion();
-        const VehicleState& getVehicleState();
+    uint32_t getTimeStamp();
+    int getVehicleStateVersion();
+    const VehicleState& getVehicleState();
 
-    public:
-        //needed for piml pattern
-        MavLinkVehicle();
-        ~MavLinkVehicle();
-        //MavLinkVehicle(MavLinkVehicle&&);
-    };
+public:
+    //needed for piml pattern
+    MavLinkVehicle();
+    ~MavLinkVehicle();
+    //MavLinkVehicle(MavLinkVehicle&&);
+};
 }
 
 #endif
diff --git a/MavLinkCom/src/MavLinkLog.cpp b/MavLinkCom/src/MavLinkLog.cpp
index b8824eeabd..838aa0c448 100644
--- a/MavLinkCom/src/MavLinkLog.cpp
+++ b/MavLinkCom/src/MavLinkLog.cpp
@@ -19,205 +19,204 @@ uint64_t MavLinkFileLog::getTimeStamp()
 
 MavLinkFileLog::MavLinkFileLog()
 {
-	ptr_ = nullptr;
-	reading_ = false;
-	writing_ = false;
-	json_ = false;
+    ptr_ = nullptr;
+    reading_ = false;
+    writing_ = false;
+    json_ = false;
 }
 
 MavLinkFileLog::~MavLinkFileLog()
 {
-	close();
+    close();
 }
 
 bool MavLinkFileLog::isOpen()
 {
-	return reading_ || writing_;
+    return reading_ || writing_;
 }
 void MavLinkFileLog::openForReading(const std::string& filename)
 {
-	close();
-	file_name_ = filename;
-	ptr_ = fopen(filename.c_str(), "rb");
-	if (ptr_ == nullptr) {
-		throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno));
-	}
+    close();
+    file_name_ = filename;
+    ptr_ = fopen(filename.c_str(), "rb");
+    if (ptr_ == nullptr) {
+        throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno));
+    }
 
     fseek(ptr_, 0, SEEK_SET);
-	reading_ = true;
-	writing_ = false;
+    reading_ = true;
+    writing_ = false;
 }
 void MavLinkFileLog::openForWriting(const std::string& filename, bool json)
 {
-	close();
-	json_ = json;
-	file_name_ = filename;
-	ptr_ = fopen(filename.c_str(), "wb");
-	if (ptr_ == nullptr) {
-		throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno));
-	}
-	if (json) {
-		fprintf(ptr_, "{ \"rows\": [\n");
-	}
-	reading_ = false;
-	writing_ = true;
+    close();
+    json_ = json;
+    file_name_ = filename;
+    ptr_ = fopen(filename.c_str(), "wb");
+    if (ptr_ == nullptr) {
+        throw std::runtime_error(Utils::stringf("Could not open the file %s, error=%d", filename.c_str(), errno));
+    }
+    if (json) {
+        fprintf(ptr_, "{ \"rows\": [\n");
+    }
+    reading_ = false;
+    writing_ = true;
 }
 
 void MavLinkFileLog::close()
 {
-	FILE* temp = ptr_;
-	if (json_ && ptr_ != nullptr) {
+    FILE* temp = ptr_;
+    if (json_ && ptr_ != nullptr) {
         fprintf(ptr_, "    {}\n"); // so that trailing comma on last row isn't a problem.
-		fprintf(ptr_, "]}\n");
-	}
-	ptr_ = nullptr;
-	if (temp != nullptr) {
-		fclose(temp);
-	}
-	reading_ = false;
-	writing_ = false;
+        fprintf(ptr_, "]}\n");
+    }
+    ptr_ = nullptr;
+    if (temp != nullptr) {
+        fclose(temp);
+    }
+    reading_ = false;
+    writing_ = false;
 }
 
 
 uint64_t FlipEndianness(uint64_t v)
 {
-	uint64_t result = 0;
-	uint64_t shift = v;
-	const int size = sizeof(uint64_t);
-	for (int i = 0; i < size; i++)
-	{
-		uint64_t low = (shift & 0xff);
-		result = (result << 8) + low;
-		shift >>= 8;
-	}
-	return result;
+    uint64_t result = 0;
+    uint64_t shift = v;
+    const int size = sizeof(uint64_t);
+    for (int i = 0; i < size; i++) {
+        uint64_t low = (shift & 0xff);
+        result = (result << 8) + low;
+        shift >>= 8;
+    }
+    return result;
 }
 
 void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp)
 {
-	if (ptr_ != nullptr) {
-		if (reading_) {
-			throw std::runtime_error("Log file was opened for reading");
-		}
-		if (json_) {
-			MavLinkMessageBase* strongTypedMsg = MavLinkMessageBase::lookup(msg);
-			if (strongTypedMsg != nullptr) {
+    if (ptr_ != nullptr) {
+        if (reading_) {
+            throw std::runtime_error("Log file was opened for reading");
+        }
+        if (json_) {
+            MavLinkMessageBase* strongTypedMsg = MavLinkMessageBase::lookup(msg);
+            if (strongTypedMsg != nullptr) {
                 strongTypedMsg->timestamp = timestamp;
-				std::string line = strongTypedMsg->toJSon();
-				{
-					std::lock_guard<std::mutex> lock(log_lock_);
-					fprintf(ptr_, "    %s\n", line.c_str());
-				}
+                std::string line = strongTypedMsg->toJSon();
+                {
+                    std::lock_guard<std::mutex> lock(log_lock_);
+                    fprintf(ptr_, "    %s\n", line.c_str());
+                }
                 delete strongTypedMsg;
-			}
-		}
-		else {
+            }
+        }
+        else {
             if (timestamp == 0) {
                 timestamp = getTimeStamp();
             }
-			// for compatibility with QGroundControl we have to save the time field in big endian.
+            // for compatibility with QGroundControl we have to save the time field in big endian.
             // todo: mavlink2 support?
             timestamp = FlipEndianness(timestamp);
 
-			uint8_t magic = msg.magic;
-			if (magic != MAVLINK_STX_MAVLINK1) {
-				// has to be one or the other!
-				magic = MAVLINK_STX;
-			}
-
-			std::lock_guard<std::mutex> lock(log_lock_);
-			fwrite(&timestamp, sizeof(uint64_t), 1, ptr_);
-			fwrite(&magic, 1, 1, ptr_);
-			fwrite(&msg.len, 1, 1, ptr_);
-			fwrite(&msg.seq, 1, 1, ptr_);
-			fwrite(&msg.sysid, 1, 1, ptr_);
-			fwrite(&msg.compid, 1, 1, ptr_);
-
-			if (magic == MAVLINK_STX_MAVLINK1) {
-				uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid
-				fwrite(&msgid, 1, 1, ptr_);
-			}
-			else {
-				// 24 bits.
-				uint8_t msgid = msg.msgid & 0xFF;
-				fwrite(&msgid, 1, 1, ptr_);
-				msgid = (msg.msgid >> 8) & 0xFF;
-				fwrite(&msgid, 1, 1, ptr_);
-				msgid = (msg.msgid >> 16) & 0xFF;
-				fwrite(&msgid, 1, 1, ptr_);
-			}
-
-			fwrite(&msg.payload64, 1, msg.len, ptr_);
-			fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_);
-		}
-	}
+            uint8_t magic = msg.magic;
+            if (magic != MAVLINK_STX_MAVLINK1) {
+                // has to be one or the other!
+                magic = MAVLINK_STX;
+            }
+
+            std::lock_guard<std::mutex> lock(log_lock_);
+            fwrite(&timestamp, sizeof(uint64_t), 1, ptr_);
+            fwrite(&magic, 1, 1, ptr_);
+            fwrite(&msg.len, 1, 1, ptr_);
+            fwrite(&msg.seq, 1, 1, ptr_);
+            fwrite(&msg.sysid, 1, 1, ptr_);
+            fwrite(&msg.compid, 1, 1, ptr_);
+
+            if (magic == MAVLINK_STX_MAVLINK1) {
+                uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink 2 msgid
+                fwrite(&msgid, 1, 1, ptr_);
+            }
+            else {
+                // 24 bits.
+                uint8_t msgid = msg.msgid & 0xFF;
+                fwrite(&msgid, 1, 1, ptr_);
+                msgid = (msg.msgid >> 8) & 0xFF;
+                fwrite(&msgid, 1, 1, ptr_);
+                msgid = (msg.msgid >> 16) & 0xFF;
+                fwrite(&msgid, 1, 1, ptr_);
+            }
+
+            fwrite(&msg.payload64, 1, msg.len, ptr_);
+            fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_);
+        }
+    }
 }
 
 bool MavLinkFileLog::read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp)
 {
-	if (ptr_ != nullptr) {
-		if (writing_) {
-			throw std::runtime_error("Log file was opened for writing");
-		}
-		uint64_t time;
-
-		size_t s = fread(&time, 1, sizeof(uint64_t), ptr_);
-		if (s < sizeof(uint64_t)) {
+    if (ptr_ != nullptr) {
+        if (writing_) {
+            throw std::runtime_error("Log file was opened for writing");
+        }
+        uint64_t time;
+
+        size_t s = fread(&time, 1, sizeof(uint64_t), ptr_);
+        if (s < sizeof(uint64_t)) {
             int hr = errno;            
-			return false;
-		}
+            return false;
+        }
 
         timestamp = FlipEndianness(time);
 
-		s = fread(&msg.magic, 1, 1, ptr_);
-		if (s == 0) {
-			return false;
-		}
-		s = fread(&msg.len, 1, 1, ptr_);
-		if (s == 0) {
-			return false;
-		}
-		s = fread(&msg.seq, 1, 1, ptr_);
-		if (s == 0) {
-			return false;
-		}
-		s = fread(&msg.sysid, 1, 1, ptr_);
-		if (s == 0) {
-			return false;
-		}
-		s = fread(&msg.compid, 1, 1, ptr_);
-		if (s == 0) {
-			return false;
-		}
-
-		if (msg.magic == MAVLINK_STX_MAVLINK1) {
-			uint8_t msgid = 0;
-			s = fread(&msgid, 1, 1, ptr_);
-			msg.msgid = msgid;
-		}
-		else {
-			// 24 bits.
-			uint8_t msgid = 0;
-			s = fread(&msgid, 1, 1, ptr_);
-			msg.msgid = msgid;
-			s = fread(&msgid, 1, 1, ptr_);
-			msg.msgid |= (msgid << 8);
-			s = fread(&msgid, 1, 1, ptr_);
-			msg.msgid |= (msgid << 16);
-		}
-
-		if (s < 1) {
-			return false;
-		}
-		s = fread(&msg.payload64, 1, msg.len, ptr_);
-		if (s < msg.len) {
-			return false;
-		}
-		s = fread(&msg.checksum, 1, sizeof(uint16_t), ptr_);
-		if (s < sizeof(uint16_t)) {
-			return false;
-		}
-		return true;
-	}
-	return false;
+        s = fread(&msg.magic, 1, 1, ptr_);
+        if (s == 0) {
+            return false;
+        }
+        s = fread(&msg.len, 1, 1, ptr_);
+        if (s == 0) {
+            return false;
+        }
+        s = fread(&msg.seq, 1, 1, ptr_);
+        if (s == 0) {
+            return false;
+        }
+        s = fread(&msg.sysid, 1, 1, ptr_);
+        if (s == 0) {
+            return false;
+        }
+        s = fread(&msg.compid, 1, 1, ptr_);
+        if (s == 0) {
+            return false;
+        }
+
+        if (msg.magic == MAVLINK_STX_MAVLINK1) {
+            uint8_t msgid = 0;
+            s = fread(&msgid, 1, 1, ptr_);
+            msg.msgid = msgid;
+        }
+        else {
+            // 24 bits.
+            uint8_t msgid = 0;
+            s = fread(&msgid, 1, 1, ptr_);
+            msg.msgid = msgid;
+            s = fread(&msgid, 1, 1, ptr_);
+            msg.msgid |= (msgid << 8);
+            s = fread(&msgid, 1, 1, ptr_);
+            msg.msgid |= (msgid << 16);
+        }
+
+        if (s < 1) {
+            return false;
+        }
+        s = fread(&msg.payload64, 1, msg.len, ptr_);
+        if (s < msg.len) {
+            return false;
+        }
+        s = fread(&msg.checksum, 1, sizeof(uint16_t), ptr_);
+        if (s < sizeof(uint16_t)) {
+            return false;
+        }
+        return true;
+    }
+    return false;
 }
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 2e1d3a4dfe..1ad22c0573 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -278,8 +278,7 @@ void MavLinkMessageBase::unpack_int8_t_array(int len, const char* buffer, int8_t
 }
 void MavLinkMessageBase::unpack_uint16_t_array(int len, const char* buffer, uint16_t* field, int offset)
 {
-    for (int i = 0; i < len; i++)
-    {
+    for (int i = 0; i < len; i++) {
         _mav_get_uint16_t(buffer, offset, field);
         offset += sizeof(uint16_t);
         field++;
@@ -287,8 +286,7 @@ void MavLinkMessageBase::unpack_uint16_t_array(int len, const char* buffer, uint
 }
 void MavLinkMessageBase::unpack_int16_t_array(int len, const char* buffer, int16_t* field, int offset)
 {
-    for (int i = 0; i < len; i++)
-    {
+    for (int i = 0; i < len; i++) {
         _mav_get_int16_t(buffer, offset, field);
         offset += sizeof(int16_t);
         field++;
@@ -296,8 +294,7 @@ void MavLinkMessageBase::unpack_int16_t_array(int len, const char* buffer, int16
 }
 void MavLinkMessageBase::unpack_float_array(int len, const char* buffer, float* field, int offset)
 {
-    for (int i = 0; i < len; i++)
-    {
+    for (int i = 0; i < len; i++) {
         _mav_get_float(buffer, offset, field);
         offset += sizeof(float);
         field++;
@@ -339,8 +336,7 @@ int MavLinkTelemetry::unpack(const char* buffer) {
 std::string MavLinkMessageBase::char_array_tostring(int len, const char* field)
 {
     int i = 0;
-    for (i= 0; i < len; i++)
-    {
+    for (i= 0; i < len; i++) {
         if (field[i] == '\0')
             break;
     }
diff --git a/MavLinkCom/src/MavLinkVehicle.cpp b/MavLinkCom/src/MavLinkVehicle.cpp
index 8ab9fd3040..ebdc95b60b 100644
--- a/MavLinkCom/src/MavLinkVehicle.cpp
+++ b/MavLinkCom/src/MavLinkVehicle.cpp
@@ -13,10 +13,12 @@ MavLinkVehicle::MavLinkVehicle(int localSystemId, int localComponentId)
 	pImpl.reset(new MavLinkVehicleImpl(localSystemId, localComponentId));
 }
 
-MavLinkVehicle::MavLinkVehicle(){
+MavLinkVehicle::MavLinkVehicle()
+{
 }
 
-MavLinkVehicle::~MavLinkVehicle() {
+MavLinkVehicle::~MavLinkVehicle()
+{
 	pImpl = nullptr;
 }
 
diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
index b2c2496871..9b2970e057 100644
--- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp
@@ -215,8 +215,7 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m)
         ::memcpy(&msg, &m, sizeof(MavLinkMessage));
         prepareForSending(msg);
 
-        if (sendLog_ != nullptr)
-        {
+        if (sendLog_ != nullptr) {
             sendLog_->write(msg);
         }
 
@@ -364,10 +363,8 @@ int MavLinkConnectionImpl::subscribe(MessageHandler handler)
 void MavLinkConnectionImpl::unsubscribe(int id)
 {
     std::lock_guard<std::mutex> guard(listener_mutex);
-    for (auto ptr = listeners.begin(), end = listeners.end(); ptr != end; ptr++)
-    {
-        if ((*ptr).id == id)
-        {
+    for (auto ptr = listeners.begin(), end = listeners.end(); ptr != end; ptr++) {
+        if ((*ptr).id == id) {
             listeners.erase(ptr);
             snapshot_stale = true;
             break;
@@ -379,8 +376,7 @@ void MavLinkConnectionImpl::joinLeftSubscriber(std::shared_ptr<MavLinkConnection
 {
     unused(connection);
     // forward messages from our connected node to the remote proxy.
-    if (supports_mavlink2_)
-    {
+    if (supports_mavlink2_) {
         // tell the remote connection to expect mavlink2 messages.
         remote->pImpl->supports_mavlink2_ = true;
     }
@@ -415,11 +411,9 @@ void MavLinkConnectionImpl::readPackets()
     mavlink_intermediate_status_.parse_state = MAVLINK_PARSE_STATE_IDLE;
     int channel = 0;
     int hr = 0;
-    while (hr == 0 && con_ != nullptr && !closed)
-    {
+    while (hr == 0 && con_ != nullptr && !closed) {
         int read = 0;
-        if (safePort->isClosed())
-        {
+        if (safePort->isClosed()) {
             // hmmm, wait till it is opened?
             std::this_thread::sleep_for(std::chrono::milliseconds(10));
             continue;
@@ -431,8 +425,7 @@ void MavLinkConnectionImpl::readPackets()
             std::this_thread::sleep_for(std::chrono::milliseconds(1));
             continue;
         }
-        for (int i = 0; i < count; i++)
-        {
+        for (int i = 0; i < count; i++) {
             uint8_t frame_state = mavlink_frame_char_buffer(&msgBuffer, &mavlink_intermediate_status_, buffer[i], &msg, &mavlink_status_);
 
             if (frame_state == MAVLINK_FRAMING_INCOMPLETE) {
@@ -442,24 +435,21 @@ void MavLinkConnectionImpl::readPackets()
                 std::lock_guard<std::mutex> guard(telemetry_mutex_);
                 telemetry_.crc_errors++;
             }
-            else if (frame_state == MAVLINK_FRAMING_OK)
-            {
+            else if (frame_state == MAVLINK_FRAMING_OK) {
                 // pick up the sysid/compid of the remote node we are connected to.
                 if (other_system_id == -1) {
                     other_system_id = msg.sysid;
                     other_component_id = msg.compid;
                 }
 
-                if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
-                {
+                if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
                     // then this is a mavlink 1 message
                 } else if (!supports_mavlink2_) {
                     // then this mavlink sender supports mavlink 2
                     supports_mavlink2_ = true;
                 }
 
-                if (con_ != nullptr && !closed)
-                {
+                if (con_ != nullptr && !closed) {
                     {
                         std::lock_guard<std::mutex> guard(telemetry_mutex_);
                         telemetry_.messages_received++;
@@ -513,8 +503,7 @@ void MavLinkConnectionImpl::drainQueue()
                 hasMsg = true;
             }
         }
-        if (!hasMsg)
-        {
+        if (!hasMsg) {
             return;
         }        
         
@@ -536,20 +525,17 @@ void MavLinkConnectionImpl::drainQueue()
         }
         auto end = snapshot.end();
 
-        if (message.msgid == static_cast<uint8_t>(MavLinkMessageIds::MAVLINK_MSG_ID_AUTOPILOT_VERSION))
-        {
+        if (message.msgid == static_cast<uint8_t>(MavLinkMessageIds::MAVLINK_MSG_ID_AUTOPILOT_VERSION)) {
             MavLinkAutopilotVersion cap;
             cap.decode(message);
-            if ((cap.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0)
-            {
+            if ((cap.capabilities & MAV_PROTOCOL_CAPABILITY_MAVLINK2) != 0) {
                 this->supports_mavlink2_ = true;
             }
         }
 
         auto startTime = std::chrono::system_clock::now();
         std::shared_ptr<MavLinkConnection> sharedPtr = std::shared_ptr<MavLinkConnection>(this->con_);
-        for (auto ptr = snapshot.begin(); ptr != end; ptr++)
-        {
+        for (auto ptr = snapshot.begin(); ptr != end; ptr++) {
             try {
                 (*ptr).handler(sharedPtr, message);
             }
diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
index be5067fa63..f76e2502f2 100644
--- a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
+++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
@@ -435,8 +435,7 @@ void MavLinkVehicleImpl::handleMessage(std::shared_ptr<MavLinkConnection> connec
         // but we do want to know when we get the ack.  So this is async ACK processing!
         MavLinkCommandAck ack;
         ack.decode(msg);
-        if (ack.command == MavCmdNavGuidedEnable::kCommandId)
-        {
+        if (ack.command == MavCmdNavGuidedEnable::kCommandId) {
             MAV_RESULT ackResult = static_cast<MAV_RESULT>(ack.result);
             if (ackResult == MAV_RESULT::MAV_RESULT_TEMPORARILY_REJECTED) {
                 Utils::log("### command MavCmdNavGuidedEnable result: MAV_RESULT_TEMPORARILY_REJECTED");
@@ -591,13 +590,11 @@ void MavLinkVehicleImpl::releaseControl()
 
 void MavLinkVehicleImpl::checkOffboard()
 {
-    if (!control_requested_)
-    {
+    if (!control_requested_) {
         throw std::runtime_error("You must call requestControl first.");
     }
 
-    if (control_requested_ && !vehicle_state_.controls.offboard)
-    {
+    if (control_requested_ && !vehicle_state_.controls.offboard) {
         // Ok, now's the time to actually request it since the caller is about to send MavLinkSetPositionTargetGlobalInt, but
         // PX4 will reject this thinking 'offboard_control_loss_timeout' because we haven't actually sent any offboard messages
         // yet.  I know the PX4 protocol is kind of weird.  So we prime the pump here with some dummy messages that tell the 
@@ -656,14 +653,12 @@ void MavLinkVehicleImpl::moveToGlobalPosition(float lat, float lon, float alt, b
     msg.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_VELOCITY |
         MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_ACCELERATION;
 
-    if (isYaw)
-    {
+    if (isYaw) {
         msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_RATE;
         msg.yaw = yawOrRate;
         msg.yaw_rate = 0;
     }
-    else
-    {
+    else {
         msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
         msg.yaw = 0;
         msg.yaw_rate = yawOrRate;
@@ -803,8 +798,7 @@ void MavLinkVehicleImpl::moveByLocalVelocityWithAltHold(float vx, float vy, floa
         msg.yaw = yawOrRate;
         msg.yaw_rate = 0;
     }
-    else
-    {
+    else {
         msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
         msg.yaw = 0;
         msg.yaw_rate = yawOrRate;
@@ -833,8 +827,7 @@ void MavLinkVehicleImpl::moveToLocalPosition(float x, float y, float z, bool isY
         msg.yaw = yawOrRate;
         msg.yaw_rate = 0;
     }
-    else
-    {
+    else {
         msg.type_mask |= MAVLINK_MSG_SET_POSITION_TARGET_IGNORE_YAW_ANGLE;
         msg.yaw = 0;
         msg.yaw_rate = yawOrRate;
diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
index 23b7b9ce25..377b91dd18 100644
--- a/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
+++ b/MavLinkCom/src/impl/MavLinkVehicleImpl.hpp
@@ -22,73 +22,74 @@
 using namespace mavlinkcom;
 
 namespace mavlinkcom_impl {
-	class MavLinkVehicleImpl : public MavLinkNodeImpl {
-	public:
-		MavLinkVehicleImpl(int localSystemId, int localComponentId);
-		~MavLinkVehicleImpl();
-	public:
-		AsyncResult<bool> armDisarm(bool arm);
-		AsyncResult<bool> takeoff(float z = -2.5, float pitch = 0, float yaw = 0);
-		AsyncResult<bool> land(float yaw, float lat = 0, float lon = 0, float altitude = 0);
-		AsyncResult<bool> returnToHome();
-		AsyncResult<bool> loiter();
-        AsyncResult<bool> setPositionHoldMode();
-        AsyncResult<bool> setStabilizedFlightMode();
-        AsyncResult<bool> setHomePosition(float lat = 0, float lon = 0, float alt = 0);
-        AsyncResult<bool> setMissionMode();
-		AsyncResult<MavLinkHomePosition> waitForHomePosition();
-		AsyncResult<bool> allowFlightControlOverUsb();
-		AsyncResult<bool> waitForAltitude(float z, float dz, float dvz);
-		AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
+class MavLinkVehicleImpl : public MavLinkNodeImpl
+{
+public:
+	MavLinkVehicleImpl(int localSystemId, int localComponentId);
+	~MavLinkVehicleImpl();
+public:
+	AsyncResult<bool> armDisarm(bool arm);
+	AsyncResult<bool> takeoff(float z = -2.5, float pitch = 0, float yaw = 0);
+	AsyncResult<bool> land(float yaw, float lat = 0, float lon = 0, float altitude = 0);
+	AsyncResult<bool> returnToHome();
+	AsyncResult<bool> loiter();
+    AsyncResult<bool> setPositionHoldMode();
+    AsyncResult<bool> setStabilizedFlightMode();
+    AsyncResult<bool> setHomePosition(float lat = 0, float lon = 0, float alt = 0);
+    AsyncResult<bool> setMissionMode();
+	AsyncResult<MavLinkHomePosition> waitForHomePosition();
+	AsyncResult<bool> allowFlightControlOverUsb();
+	AsyncResult<bool> waitForAltitude(float z, float dz, float dvz);
+	AsyncResult<bool> setMode(int modeFlags, int customMode = 0, int customSubMode = 0, bool waitForAck = true);
 
-		// request OFFBOARD control.  
-        void requestControl();
-		// release OFFBOARD control
-        void releaseControl();
+	// request OFFBOARD control.  
+    void requestControl();
+	// release OFFBOARD control
+    void releaseControl();
 
-		// return true if we still have offboard control (can lose this if user flips the switch).
-		bool hasOffboardControl();
-        // send this to keep offboard control but do no movement.
-        void offboardIdle();
+	// return true if we still have offboard control (can lose this if user flips the switch).
+	bool hasOffboardControl();
+    // send this to keep offboard control but do no movement.
+    void offboardIdle();
 
-		// offboard control methods.
-		bool isLocalControlSupported();
-		void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate);
-		void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate);
-		void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate);
-		void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate);
+	// offboard control methods.
+	bool isLocalControlSupported();
+	void moveToLocalPosition(float x, float y, float z, bool isYaw, float yawOrRate);
+	void moveToGlobalPosition(float lat, float lon, float alt, bool isYaw, float yawOrRate);
+	void moveByLocalVelocity(float vx, float vy, float vz, bool isYaw, float yawOrRate);
+	void moveByLocalVelocityWithAltHold(float vx, float vy, float z, bool isYaw, float yawOrRate);
 
-		// low level control, only use this one if you really know what you are doing!!
-		bool isAttitudeControlSupported();
+	// low level control, only use this one if you really know what you are doing!!
+	bool isAttitudeControlSupported();
 
-		// Move drone by directly controlling the attitude of the drone (units are degrees).
-		// If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone.
-		void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust);
-        void writeMessage(MavLinkMessageBase& message, bool update_stats = true);
+	// Move drone by directly controlling the attitude of the drone (units are degrees).
+	// If the rollRate, pitchRate and yawRate are all zero then you will get the default rates provided by the drone.
+	void moveByAttitude(float roll, float pitch, float yaw, float rollRate, float pitchRate, float yawRate, float thrust);
+    void writeMessage(MavLinkMessageBase& message, bool update_stats = true);
 
-		int getVehicleStateVersion();
-		const VehicleState& getVehicleState();
+	int getVehicleStateVersion();
+	const VehicleState& getVehicleState();
 
-		uint32_t getTimeStamp();
-	private:
-		virtual void handleMessage(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& message);
-		void resetCommandParams(MavLinkCommandLong& cmd);
-		void updateReadStats(const MavLinkMessage& msg);
-		void checkOffboard();
-		bool getRcSwitch(int channel, float threshold);
+	uint32_t getTimeStamp();
+private:
+	virtual void handleMessage(std::shared_ptr<MavLinkConnection> connection, const MavLinkMessage& message);
+	void resetCommandParams(MavLinkCommandLong& cmd);
+	void updateReadStats(const MavLinkMessage& msg);
+	void checkOffboard();
+	bool getRcSwitch(int channel, float threshold);
 
-	private:
-		std::mutex state_mutex_;
-		int state_version_ = 0;
-        bool control_requested_ = false;
-		bool control_request_sent_ = false;
-        int requested_mode_ = 0;
-		int previous_mode_ = 0;
-		// this latch is reset even time we receive a heartbeat, this is useful for operations that we
-		// want to throttle to the heartbeat rate.
-		bool heartbeat_throttle_ = false;
-		VehicleState vehicle_state_;
-	};
+private:
+	std::mutex state_mutex_;
+	int state_version_ = 0;
+    bool control_requested_ = false;
+	bool control_request_sent_ = false;
+    int requested_mode_ = 0;
+	int previous_mode_ = 0;
+	// this latch is reset even time we receive a heartbeat, this is useful for operations that we
+	// want to throttle to the heartbeat rate.
+	bool heartbeat_throttle_ = false;
+	VehicleState vehicle_state_;
+};
 }
 
 #endif
diff --git a/PythonClient/multirotor/path.py b/PythonClient/multirotor/path.py
index d36fc6a900..6d34f13488 100644
--- a/PythonClient/multirotor/path.py
+++ b/PythonClient/multirotor/path.py
@@ -29,9 +29,9 @@
     sys.exit(1)
 
 # AirSim uses NED coordinates so negative axis is up.
-# z of -7 is 7 meters above the original launch point.
-z = -7
-print("make sure we are hovering at 7 meters...")
+# z of -5 is 5 meters above the original launch point.
+z = -5
+print("make sure we are hovering at {} meters...".format(-z))
 client.moveToZAsync(z, 1).join()
 
 # see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo
diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
index 75ff5b739b..d25a174b68 100644
--- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
+++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.cpp
@@ -3,27 +3,26 @@
 
 UnitySensorFactory::UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform)
 {
-	setActor(vehicle_name, ned_transform);
+    setActor(vehicle_name, ned_transform);
 }
 
 void UnitySensorFactory::setActor(std::string vehicle_name, const NedTransform* ned_transform)
 {
-	vehicle_name_ = vehicle_name;
-	ned_transform_ = ned_transform;
+    vehicle_name_ = vehicle_name;
+    ned_transform_ = ned_transform;
 }
 
 std::shared_ptr<msr::airlib::SensorBase> UnitySensorFactory::createSensorFromSettings(const AirSimSettings::SensorSetting * sensor_setting) const
 {
-	
-	using SensorBase = msr::airlib::SensorBase;
+    
+    using SensorBase = msr::airlib::SensorBase;
 
-	switch (sensor_setting->sensor_type)
-	{
-	case SensorBase::SensorType::Distance:
-		return std::make_shared<UnityDistanceSensor>(vehicle_name_, ned_transform_);
-	default:
-		return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting);
-	}
+    switch (sensor_setting->sensor_type) {
+    case SensorBase::SensorType::Distance:
+        return std::make_shared<UnityDistanceSensor>(vehicle_name_, ned_transform_);
+    default:
+        return msr::airlib::SensorFactory::createSensorFromSettings(sensor_setting);
+    }
 
-	return std::shared_ptr<msr::airlib::SensorBase>();
+    return std::shared_ptr<msr::airlib::SensorBase>();
 }
diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
index 75565a7b3d..1a50ed3e51 100644
--- a/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
+++ b/Unity/AirLibWrapper/AirsimWrapper/Source/UnitySensors/UnitySensorFactory.h
@@ -8,11 +8,11 @@ using namespace msr::airlib;
 class UnitySensorFactory : public SensorFactory
 {
 public:
-	UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform);
-	void setActor(std::string vehicle_name, const NedTransform* ned_transform);
-	virtual std::shared_ptr<msr::airlib::SensorBase> createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override;
+    UnitySensorFactory(std::string vehicle_name, const NedTransform* ned_transform);
+    void setActor(std::string vehicle_name, const NedTransform* ned_transform);
+    virtual std::shared_ptr<msr::airlib::SensorBase> createSensorFromSettings(const AirSimSettings::SensorSetting* sensor_setting) const override;
 
 private:
-	std::string vehicle_name_;
-	const NedTransform* ned_transform_;
+    std::string vehicle_name_;
+    const NedTransform* ned_transform_;
 };
diff --git a/docs/log_viewer.md b/docs/log_viewer.md
index 2f97249481..2934413f71 100644
--- a/docs/log_viewer.md
+++ b/docs/log_viewer.md
@@ -40,7 +40,7 @@ For this to work you need to configure the `settings.json` with the following se
 ```
 
 Note: do not use the "Logs" setting when you want realtime LogViewer logging.  Logging to
-a file using "Logs" is mutually exclusive. with LogViewer logging.
+a file using "Logs" is mutually exclusive with LogViewer logging.
 
 Simply press the blue connector button on the top right corner of the window, select the Socket
 `tab`, enter the port number `14388`, and your `localhost` network.  If you are using WSL 2 on
diff --git a/docs/px4_setup.md b/docs/px4_setup.md
index 1a5b1bb178..03914471bf 100644
--- a/docs/px4_setup.md
+++ b/docs/px4_setup.md
@@ -2,7 +2,7 @@
 
 The [PX4 software stack](http://github.com/px4/firmware) is an open source very popular flight controller with support for wide variety of boards and sensors as well as built-in capability for higher level tasks such as mission planning. Please visit [px4.io](http://px4.io) for more information.
 
-**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim. 
+**Warning**: While all releases of AirSim are always tested with PX4 to ensure the support, setting up PX4 is not a trivial task. Unless you have at least intermediate level of experience with PX4 stack, we recommend you use [simple_flight](simple_flight.md), which is now a default in AirSim.
 
 ## Supported Hardware
 
@@ -26,7 +26,7 @@ For this you will need one of the supported device listed above. For manual flig
 3. Use QGroundControl to flash the latest PX4 Flight Stack.
 See also [initial firmware setup video](https://docs.px4.io/master/en/config/).
 4. In QGroundControl, configure your Pixhawk for HIL simulation by selecting the HIL Quadrocopter X airframe.  After PX4 reboots, check that "HIL Quadrocopter X" is indeed selected.
-5. In QGroundControl, go to Radio tab and calibrate (make sure the remote control is on and the receiver is showing the indicator for the binding). 
+5. In QGroundControl, go to Radio tab and calibrate (make sure the remote control is on and the receiver is showing the indicator for the binding).
 6. Go to the Flight Mode tab and chose one of the remote control switches as "Mode Channel". Then set (for example) Stabilized and Attitude flight modes for two positions of the switch.
 7. Go to the Tuning section of QGroundControl and set appropriate values. For example, for Fly Sky's FS-TH9X remote control, the following settings give a more realistic feel: Hover Throttle = mid+1 mark, Roll and pitch sensitivity = mid-3 mark, Altitude and position control sensitivity = mid-2 mark.
 8. In [AirSim settings](settings.md) file, specify PX4 for your vehicle config like this:
@@ -37,7 +37,7 @@ See also [initial firmware setup video](https://docs.px4.io/master/en/config/).
         "Vehicles": {
             "PX4": {
                 "VehicleType": "PX4Multirotor",
-                "UseSerial": true,,
+                "UseSerial": true,
                 "Sensors":{
                     "Barometer":{
                         "SensorType": 1,
@@ -73,7 +73,7 @@ The PX4 SITL mode doesn't require you to have separate device such as a Pixhawk
 
 #### Drone doesn't fly properly, it just goes "crazy".
 
-There are a few reasons that can cause this. First, make sure your drone doesn't fall down large distance when starting the simulator. This might happen if you have created a custom Unreal environment and Player Start is placed too high above the ground. It seems that when this happens internal calibration in PX4 gets confused. 
+There are a few reasons that can cause this. First, make sure your drone doesn't fall down large distance when starting the simulator. This might happen if you have created a custom Unreal environment and Player Start is placed too high above the ground. It seems that when this happens internal calibration in PX4 gets confused.
 
 You should [also use QGroundControl](#setting-up-px4-hardware-in-loop) and make sure you can arm and takeoff in QGroundControl properly.
 
@@ -86,15 +86,15 @@ PX4 custom modes in the MAV_CMD_DO_SET_MODE messages (like PX4_CUSTOM_MAIN_MODE_
 
 #### It is not finding my Pixhawk hardware
 
-Check your settings.json file for this line "SerialPort":"*,115200".  The asterisk here means "find any 
+Check your settings.json file for this line "SerialPort":"*,115200".  The asterisk here means "find any
 serial port that looks like a Pixhawk device, but this doesn't always work for all types of Pixhawk hardware.
-So on Windows you can find the actual COM port using Device Manager, look under "Ports (COM & LPT), plug the 
-device in and see what new COM port shows up.  Let's say you see a new port named "USB Serial Port (COM5)". 
-Well, then change the SerialPort setting to this: "SerialPort":"COM5,115200".  
+So on Windows you can find the actual COM port using Device Manager, look under "Ports (COM & LPT), plug the
+device in and see what new COM port shows up.  Let's say you see a new port named "USB Serial Port (COM5)".
+Well, then change the SerialPort setting to this: "SerialPort":"COM5,115200".
 
 On Linux, the device can be found by running "ls /dev/serial/by-id" if you see a device name listed that looks
 like this `usb-3D_Robotics_PX4_FMU_v2.x_0-if00` then you can use that name to connect, like this:
-"SerialPort":"/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00".  Note this long name is actually a symbolic link to the real 
+"SerialPort":"/dev/serial/by-id/usb-3D_Robotics_PX4_FMU_v2.x_0-if00".  Note this long name is actually a symbolic link to the real
 name, if you use "ls -l ..." you can find that symbolic link, it is usually something like "/dev/ttyACM0",
 so this will also work "SerialPort":"/dev/ttyACM0,115200".  But that mapping is similar to windows, it is
 automatically assigned and can change, whereas the long name will work even if the actual TTY serial device
diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md
index 1e960d0eb3..2c664fa111 100644
--- a/docs/px4_sitl.md
+++ b/docs/px4_sitl.md
@@ -4,7 +4,7 @@ The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (
 their stack that runs in Linux. If you are on Windows then you can use the [Cygwin
 Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) or you can use the
 [Windows subsystem for Linux](https://docs.microsoft.com/en-us/windows/wsl/install-win10) and follow
-the PX4 Linux toolchain setup. 
+the PX4 Linux toolchain setup.
 
 If you are using WSL2 please read these [additional
 instructions](px4_sitl_wsl2.md).
@@ -22,7 +22,7 @@ instructions](px4_sitl_wsl2.md).
     mkdir -p PX4
     cd PX4
     git clone https://github.com/PX4/PX4-Autopilot.git --recursive
-    bash ./PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools 
+    bash ./PX4-Autopilot/Tools/setup/ubuntu.sh --no-nuttx --no-sim-tools
     cd PX4-Autopilot
     ```
     And find the latest stable release from [https://github.com/PX4/PX4-Autopilot/releases](https://github.com/PX4/PX4-Autopilot/releases)
@@ -62,7 +62,7 @@ The default ports have changed recently, so check them closely to make sure AirS
                 "UseSerial": false,
                 "UseTcp": true,
                 "TcpPort": 4560,
-                "ControlPort": 14580,,
+                "ControlPort": 14580,
                 "Sensors":{
                     "Barometer":{
                         "SensorType": 1,

From b717da44db009ae65c71b1e751963a4e5f3b7fe0 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 16:12:24 -0700
Subject: [PATCH 30/37] fix typos.

---
 .../mavlink/MavLinkMultirotorApi.hpp          |  2 +-
 PythonClient/multirotor/speaker.py            |  1 -
 build.cmd                                     | 48 +++++++++----------
 3 files changed, 25 insertions(+), 26 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index e0b297a050..a4946081d4 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -1744,7 +1744,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         }
         else {
             // creates too much log output, only do this when debugging this issue specifically.
-            // Utils::log(Utils::stringf("Ignornig msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
+            // Utils::log(Utils::stringf("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
         }
     }
 
diff --git a/PythonClient/multirotor/speaker.py b/PythonClient/multirotor/speaker.py
index c3beccefb0..acee778942 100644
--- a/PythonClient/multirotor/speaker.py
+++ b/PythonClient/multirotor/speaker.py
@@ -17,7 +17,6 @@ def __init__(self):
         self.audio = pyaudio.PyAudio()
 
     def open(self, audio_format, num_channels, rate):
-
         # open speakers so we can hear what it is processing...
         self.output_stream = self.audio.open(format=audio_format,
                                              channels=num_channels,
diff --git a/build.cmd b/build.cmd
index 2476ebbb02..efc7395e7a 100644
--- a/build.cmd
+++ b/build.cmd
@@ -65,33 +65,33 @@ if ERRORLEVEL 1 (
 REM //---------- get rpclib ----------
 IF NOT EXIST external\rpclib mkdir external\rpclib
 IF NOT EXIST external\rpclib\rpclib-2.2.1 (
-	REM //leave some blank lines because %powershell% shows download banner at top of console
-	ECHO(
-	ECHO(   
-	ECHO(   
-	ECHO *****************************************************************************************
-	ECHO Downloading rpclib
-	ECHO *****************************************************************************************
-	@echo on
+    REM //leave some blank lines because %powershell% shows download banner at top of console
+    ECHO(
+    ECHO(   
+    ECHO(   
+    ECHO *****************************************************************************************
+    ECHO Downloading rpclib
+    ECHO *****************************************************************************************
+    @echo on
     if "%PWSHV7%" == "" (
         %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }"
     ) else (
-	    %powershell% -command "iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip"
+        %powershell% -command "iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip"
+    )
+    @echo off
+    
+    REM //remove any previous versions
+    rmdir external\rpclib /q /s
+
+    %powershell% -command "Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib"
+    del external\rpclib.zip /q
+    
+    REM //Don't fail the build if the high-poly car is unable to be downloaded
+    REM //Instead, just notify users that the gokart will be used.
+    IF NOT EXIST external\rpclib\rpclib-2.2.1 (
+        ECHO Unable to download high-polycount SUV. Your AirSim build will use the default vehicle.
+        goto :buildfailed
     )
-	@echo off
-	
-	REM //remove any previous versions
-	rmdir external\rpclib /q /s
-
-	%powershell% -command "Expand-Archive -Path external\rpclib.zip -DestinationPath external\rpclib"
-	del external\rpclib.zip /q
-	
-	REM //Don't fail the build if the high-poly car is unable to be downloaded
-	REM //Instead, just notify users that the gokart will be used.
-	IF NOT EXIST external\rpclib\rpclib-2.2.1 (
-		ECHO Unable to download high-polycount SUV. Your AirSim build will use the default vehicle.
-		goto :buildfailed
-	)
 )
 
 REM //---------- Build rpclib ------------
@@ -148,7 +148,7 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 (
             %powershell% -command "iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip"
         )
         @echo off
-		rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV
+        rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV
         %powershell% -command "Expand-Archive -Path suv_download_tmp\car_assets.zip -DestinationPath Unreal\Plugins\AirSim\Content\VehicleAdv"
         rmdir suv_download_tmp /q /s
         

From 609031bd0ec9c7b4628b84e60a72cda83a69b25d Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 16:19:32 -0700
Subject: [PATCH 31/37] fix typo in udpate_rate

---
 .../multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp     | 2 +-
 MavLinkCom/include/MavLinkMessageBase.hpp                     | 4 ++--
 MavLinkCom/src/MavLinkMessageBase.cpp                         | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index a4946081d4..11e7a5441d 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -660,7 +660,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 average_update = static_cast<uint32_t>(update_time_ / hil_sensor_count_);
             }
 
-            data.udpate_rate = update_count_;
+            data.update_rate = update_count_;
             data.sensor_rate = hil_sensor_count_;
             data.actuation_delay = average_delay;
             data.lock_step_resets = lock_step_resets_;
diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp
index 9444ee397a..071228fbd7 100644
--- a/MavLinkCom/include/MavLinkMessageBase.hpp
+++ b/MavLinkCom/include/MavLinkMessageBase.hpp
@@ -147,7 +147,7 @@ class MavLinkTelemetry : public MavLinkMessageBase
     uint32_t handler_microseconds = 0; // total time spent in the handlers in microseconds since the last telemetry message
     uint32_t render_time = 0;          // total time spent rendering frames since the last telemetry message
     int32_t wifi_rssi = 0;             // if this device is communicating over wifi this is the signal strength.
-    uint32_t udpate_rate = 0;          // rate at which update() is being called on MavLinkMultiRotorApi
+    uint32_t update_rate = 0;          // rate at which update() is being called on MavLinkMultiRotorApi
     uint32_t actuation_delay = 0;      // delay from HIL_SENSOR to HIL_ACTUATORCONTROLS response
     uint32_t sensor_rate = 0;          // rate we are sending HIL_SENSOR messages
     uint32_t lock_step_resets = 0;     // total number of lock_step resets
@@ -166,7 +166,7 @@ class MavLinkTelemetry : public MavLinkMessageBase
         result << "\"handler_microseconds\":" << this->handler_microseconds << ",";
         result << "\"render_time\":" << this->render_time;
         result << "\"wifi_rssi\":" << this->wifi_rssi;
-        result << "\"udpate_rate\":" << this->udpate_rate;
+        result << "\"update_rate\":" << this->update_rate;
         result << "\"actuation_delay\":" << this->actuation_delay;
         result << "\"sensor_rate\":" << this->sensor_rate;
         result << "\"lock_step_resets\":" << this->lock_step_resets;
diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 1ad22c0573..8abd846a29 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -309,7 +309,7 @@ int MavLinkTelemetry::pack(char* buffer) const {
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->handler_microseconds), 16);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->render_time), 20);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->wifi_rssi), 24);
-    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->udpate_rate), 28);
+    pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->update_rate), 28);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->actuation_delay), 32);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->sensor_rate), 36);
     pack_int32_t(buffer, reinterpret_cast<const int32_t*>(&this->lock_step_resets), 40);
@@ -325,7 +325,7 @@ int MavLinkTelemetry::unpack(const char* buffer) {
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->handler_microseconds), 16);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->render_time), 20);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->wifi_rssi), 24);
-    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->udpate_rate), 28);
+    unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->update_rate), 28);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->actuation_delay), 32);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->sensor_rate), 36);
     unpack_int32_t(buffer, reinterpret_cast<int32_t*>(&this->lock_step_resets), 40);

From def8bc09a53ce5a98e04460ee3a17880faaa77be Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 17:33:34 -0700
Subject: [PATCH 32/37] fix bugs.

---
 PythonClient/multirotor/hello_drone.py | 7 ++++---
 PythonClient/requirements.txt          | 1 +
 2 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/PythonClient/multirotor/hello_drone.py b/PythonClient/multirotor/hello_drone.py
index e51556e1c3..a9b4a4fa9c 100644
--- a/PythonClient/multirotor/hello_drone.py
+++ b/PythonClient/multirotor/hello_drone.py
@@ -1,4 +1,4 @@
-import setup_path 
+import setup_path
 import airsim
 
 import numpy as np
@@ -11,7 +11,6 @@
 client = airsim.MultirotorClient()
 client.confirmConnection()
 client.enableApiControl(True)
-client.armDisarm(True)
 
 state = client.getMultirotorState()
 s = pprint.pformat(state)
@@ -34,6 +33,8 @@
 print("gps_data: %s" % s)
 
 airsim.wait_key('Press any key to takeoff')
+print("Taking off...")
+client.armDisarm(True)
 client.takeoffAsync().join()
 
 state = client.getMultirotorState()
@@ -82,8 +83,8 @@
 
 airsim.wait_key('Press any key to reset to original state')
 
-client.armDisarm(False)
 client.reset()
+client.armDisarm(False)
 
 # that's enough fun for now. let's quit cleanly
 client.enableApiControl(False)
diff --git a/PythonClient/requirements.txt b/PythonClient/requirements.txt
index e329ca18d3..1713d945ac 100644
--- a/PythonClient/requirements.txt
+++ b/PythonClient/requirements.txt
@@ -1,3 +1,4 @@
 msgpack-rpc-python
 numpy
 pyaudio
+opencv-contrib-python

From c04924f058354499dc0a57b22262bd6e4dc95c2c Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 17:51:45 -0700
Subject: [PATCH 33/37] clang format

---
 .clang-format                                 |   56 +
 .../mavlink/MavLinkMultirotorApi.hpp          | 3308 ++++++++---------
 2 files changed, 1709 insertions(+), 1655 deletions(-)
 create mode 100644 .clang-format

diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000000..9f7e399e1d
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,56 @@
+---
+# clang-format documentation
+# http://clang.llvm.org/docs/ClangFormat.html
+# http://clang.llvm.org/docs/ClangFormatStyleOptions.html
+
+# Preexisting formats:
+# LLVM
+# Google
+# Chromium
+# Mozilla
+# WebKit
+
+Language:        Cpp
+BasedOnStyle:  Mozilla
+
+AccessModifierOffset: -4
+AlignAfterOpenBracket: Align
+AlignTrailingComments: false
+AllowShortBlocksOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+BinPackArguments: false
+BinPackParameters: true
+BreakBeforeBraces: Custom
+BraceWrapping:
+  AfterClass:      true
+  AfterControlStatement: false
+  AfterEnum:       true
+  AfterFunction:   true
+  AfterNamespace:  true
+  AfterObjCDeclaration: false
+  AfterStruct:     true
+  AfterUnion:      true
+  BeforeCatch:     true
+  BeforeElse:      true
+  IndentBraces:    false
+ContinuationIndentWidth: 4
+ConstructorInitializerIndentWidth: 4
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ColumnLimit: 0
+Cpp11BracedListStyle: false
+IndentWidth: 4
+IndentCaseLabels: false
+# KeepEmptyLinesAtTheStartOfBlocks: false
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: Inner
+ReflowComments: false
+PenaltyBreakBeforeFirstCallParameter: 100000
+PenaltyBreakComment: 100000
+SortIncludes: true
+SpaceAfterTemplateKeyword: true
+# Standard: Cpp11 # Broken
+UseTab: Never
+...
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 11e7a5441d..6e1977cee9 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -4,2029 +4,2027 @@
 #ifndef msr_airlib_MavLinkDroneController_hpp
 #define msr_airlib_MavLinkDroneController_hpp
 
-#include "MavLinkVehicle.hpp"
 #include "MavLinkConnection.hpp"
 #include "MavLinkMessages.hpp"
 #include "MavLinkNode.hpp"
+#include "MavLinkVehicle.hpp"
 #include "MavLinkVideoStream.hpp"
 
-#include <queue>
+#include <exception>
+#include <memory>
 #include <mutex>
+#include <queue>
 #include <string>
-#include <vector>
-#include <memory>
-#include <exception>
 #include <thread>
 #include <tuple>
+#include <vector>
 
+#include "common/AirSimSettings.hpp"
 #include "common/Common.hpp"
+#include "common/CommonStructs.hpp"
+#include "common/PidController.hpp"
+#include "common/VectorMath.hpp"
 #include "common/common_utils/FileSystem.hpp"
 #include "common/common_utils/SmoothingFilter.hpp"
 #include "common/common_utils/Timer.hpp"
-#include "common/CommonStructs.hpp"
-#include "common/VectorMath.hpp"
-#include "common/AirSimSettings.hpp"
-#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
-#include "common/PidController.hpp"
-#include "sensors/SensorCollection.hpp"
 #include "physics/World.hpp"
+#include "sensors/SensorCollection.hpp"
+#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
 
 //sensors
 #include "sensors/barometer/BarometerBase.hpp"
-#include "sensors/imu/ImuBase.hpp"
+#include "sensors/distance/DistanceSimple.hpp"
 #include "sensors/gps/GpsBase.hpp"
+#include "sensors/imu/ImuBase.hpp"
 #include "sensors/magnetometer/MagnetometerBase.hpp"
-#include "sensors/distance/DistanceSimple.hpp"
 
-namespace msr { namespace airlib {
-
-class MavLinkMultirotorApi : public MultirotorApiBase
+namespace msr
+{
+namespace airlib
 {
-public: //methods
-    virtual ~MavLinkMultirotorApi()
-    {
-        closeAllConnection();
-        if (this->connect_thread_.joinable()) {
-            this->connect_thread_.join();
-        }
-        if (this->telemetry_thread_.joinable()) {
-            this->telemetry_thread_.join();
-        }
-    }
 
-    //non-base interface specific to MavLinKDroneController
-    void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
+    class MavLinkMultirotorApi : public MultirotorApiBase
     {
-        connection_info_ = connection_info;
-        sensors_ = sensors;
-        is_simulation_mode_ = is_simulation;
-
-        try {
-            openAllConnections();
-            is_ready_ = true;
-        }
-        catch (std::exception& ex) {
-            is_ready_ = false;
-            is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
+    public: //methods
+        virtual ~MavLinkMultirotorApi()
+        {
+            closeAllConnection();
+            if (this->connect_thread_.joinable()) {
+                this->connect_thread_.join();
+            }
+            if (this->telemetry_thread_.joinable()) {
+                this->telemetry_thread_.join();
+            }
         }
-    }
-
-    Pose getMocapPose()
-    {
-        std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
-        return mocap_pose_;
-    }
 
-    virtual const SensorCollection& getSensors() const override
-    {
-        return *sensors_;
-    }
-
-    //reset PX4 stack
-    virtual void resetImplementation() override
-    {
-        // note this is called AFTER "initialize" when we've connected to the drone
-        // so this method cannot reset any of that connection state.
-        world_ = nullptr;
+        //non-base interface specific to MavLinKDroneController
+        void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
+        {
+            connection_info_ = connection_info;
+            sensors_ = sensors;
+            is_simulation_mode_ = is_simulation;
 
-        for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) {
-            if (container->getName() == "World") {
-                // cool beans!
-                // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason...
-                world_ = static_cast<World*>(container);
+            try {
+                openAllConnections();
+                is_ready_ = true;
             }
-        }
-        MultirotorApiBase::resetImplementation();
-        was_reset_ = true;
-    }
-
-    unsigned long long getSimTime() 
-    {
-        // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
-        if (lock_step_enabled_) {
-            if (sim_time_us_ == 0) {
-                sim_time_us_ = clock()->nowNanos() / 1000;
+            catch (std::exception& ex) {
+                is_ready_ = false;
+                is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
             }
-            return sim_time_us_;
-        }
-        else {
-            return clock()->nowNanos() / 1000;
         }
-    }
-
-    void advanceTime() 
-    {
-        sim_time_us_ = clock()->nowNanos() / 1000;
-    }
 
-    //update sensors in PX4 stack
-    virtual void update() override
-    {
-        try {
-            auto now = clock()->nowNanos() / 1000;
-            MultirotorApiBase::update();
+        Pose getMocapPose()
+        {
+            std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
+            return mocap_pose_;
+        }
 
-            if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
-                return;
+        virtual const SensorCollection& getSensors() const override
+        {
+            return *sensors_;
+        }
 
-            {
-                std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                update_count_++;
+        //reset PX4 stack
+        virtual void resetImplementation() override
+        {
+            // note this is called AFTER "initialize" when we've connected to the drone
+            // so this method cannot reset any of that connection state.
+            world_ = nullptr;
+
+            for (UpdatableObject* container = this->getParent(); container != nullptr; container = container->getParent()) {
+                if (container->getName() == "World") {
+                    // cool beans!
+                    // note: cannot use dynamic_cast because Unreal builds with /GR- for some unknown reason...
+                    world_ = static_cast<World*>(container);
+                }
             }
+            MultirotorApiBase::resetImplementation();
+            was_reset_ = true;
+        }
 
+        unsigned long long getSimTime()
+        {
+            // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
             if (lock_step_enabled_) {
-                if (last_update_time_ + 100000 < now) {
-                    // if 100 ms passes then something is terribly wrong, reset lockstep mode
-                    lock_step_enabled_ = false;
-                    {
-                        std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                        lock_step_resets_++;
-                    }
-                    addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
-                }
-                else if (!received_actuator_controls_) {
-                    // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
-                    return;
+                if (sim_time_us_ == 0) {
+                    sim_time_us_ = clock()->nowNanos() / 1000;
                 }
+                return sim_time_us_;
             }
-
-            last_update_time_ = now;
-
-            {
-                std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                hil_sensor_count_++;
+            else {
+                return clock()->nowNanos() / 1000;
             }
-            advanceTime();
+        }
 
-            //send sensor updates
-            const auto& imu_output = getImuData("");
-            const auto& mag_output = getMagnetometerData("");
-            const auto& baro_output = getBarometerData("");
+        void advanceTime()
+        {
+            sim_time_us_ = clock()->nowNanos() / 1000;
+        }
 
-            sendHILSensor(imu_output.linear_acceleration,
-                imu_output.angular_velocity,
-                mag_output.magnetic_field_body,
-                baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
+        //update sensors in PX4 stack
+        virtual void update() override
+        {
+            try {
+                auto now = clock()->nowNanos() / 1000;
+                MultirotorApiBase::update();
 
-            sendSystemTime();
+                if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
+                    return;
 
-            const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
-            if (count_distance_sensors != 0) {
-                const auto* distance_sensor = static_cast<const DistanceSimple*>(
-                                                sensors_->getByType(SensorBase::SensorType::Distance));
-                // Don't send the data if sending to external controller is disabled in settings
-                if (distance_sensor && distance_sensor->getParams().external_controller) {
-                    const auto& distance_output = distance_sensor->getOutput();
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    update_count_++;
+                }
 
-                    sendDistanceSensor(distance_output.min_distance * 100, //m -> cm
-                        distance_output.max_distance * 100, //m -> cm
-                        distance_output.distance * 100, //m-> cm
-                        0, //sensor type: //TODO: allow changing in settings?
-                        77, //sensor id, //TODO: should this be something real?
-                        distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
+                if (lock_step_enabled_) {
+                    if (last_update_time_ + 100000 < now) {
+                        // if 100 ms passes then something is terribly wrong, reset lockstep mode
+                        lock_step_enabled_ = false;
+                        {
+                            std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                            lock_step_resets_++;
+                        }
+                        addStatusMessage("timeout on HilActuatorControlsMessage, resetting lock step mode");
+                    }
+                    else if (!received_actuator_controls_) {
+                        // drop this one since we are in LOCKSTEP mode and we have not yet received the HilActuatorControlsMessage.
+                        return;
+                    }
                 }
-            }
 
-            const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
-            if (count_gps_sensors != 0) {
-                const auto& gps_output = getGpsData("");
+                last_update_time_ = now;
 
-                //send GPS
-                if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
-                    last_gps_time_ = gps_output.gnss.time_utc;
-                    Vector3r gps_velocity = gps_output.gnss.velocity;
-                    Vector3r gps_velocity_xy = gps_velocity;
-                    gps_velocity_xy.z() = 0;
-                    float gps_cog;
-                    if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
-                        gps_cog = 0;
-                    else
-                        gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
-                    if (gps_cog < 0)
-                        gps_cog += 360;
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    hil_sensor_count_++;
+                }
+                advanceTime();
+
+                //send sensor updates
+                const auto& imu_output = getImuData("");
+                const auto& mag_output = getMagnetometerData("");
+                const auto& baro_output = getBarometerData("");
+
+                sendHILSensor(imu_output.linear_acceleration,
+                              imu_output.angular_velocity,
+                              mag_output.magnetic_field_body,
+                              baro_output.pressure * 0.01f /*Pa to Millibar */,
+                              baro_output.altitude);
+
+                sendSystemTime();
+
+                const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
+                if (count_distance_sensors != 0) {
+                    const auto* distance_sensor = static_cast<const DistanceSimple*>(
+                        sensors_->getByType(SensorBase::SensorType::Distance));
+                    // Don't send the data if sending to external controller is disabled in settings
+                    if (distance_sensor && distance_sensor->getParams().external_controller) {
+                        const auto& distance_output = distance_sensor->getOutput();
+
+                        sendDistanceSensor(distance_output.min_distance * 100, //m -> cm
+                                           distance_output.max_distance * 100, //m -> cm
+                                           distance_output.distance * 100, //m-> cm
+                                           0, //sensor type: //TODO: allow changing in settings?
+                                           77, //sensor id, //TODO: should this be something real?
+                                           distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
+                    }
+                }
 
-                    sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
-                        gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
+                const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
+                if (count_gps_sensors != 0) {
+                    const auto& gps_output = getGpsData("");
+
+                    //send GPS
+                    if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
+                        last_gps_time_ = gps_output.gnss.time_utc;
+                        Vector3r gps_velocity = gps_output.gnss.velocity;
+                        Vector3r gps_velocity_xy = gps_velocity;
+                        gps_velocity_xy.z() = 0;
+                        float gps_cog;
+                        if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
+                            gps_cog = 0;
+                        else
+                            gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
+                        if (gps_cog < 0)
+                            gps_cog += 360;
+
+                        sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog, gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
+                    }
                 }
-            }
 
-            auto end = clock()->nowNanos() / 1000;
-            {
-                std::lock_guard<std::mutex> guard(telemetry_mutex_);
-                update_time_ += (end - now);
+                auto end = clock()->nowNanos() / 1000;
+                {
+                    std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                    update_time_ += (end - now);
+                }
+            }
+            catch (std::exception& e) {
+                addStatusMessage("Exception sending messages to vehicle");
+                addStatusMessage(e.what());
+                disconnect();
+                connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
             }
-        }
-        catch (std::exception& e) {
-            addStatusMessage("Exception sending messages to vehicle");
-            addStatusMessage(e.what());
-            disconnect();
-            connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
-        }
-
 
-        //must be done at the end
-        if (was_reset_)
-            was_reset_ = false;
-    }
+            //must be done at the end
+            if (was_reset_)
+                was_reset_ = false;
+        }
 
-    virtual bool isReady(std::string& message) const override
-    {
-        if (!is_ready_ && is_ready_message_.size() > 0) {
-            message = is_ready_message_;
+        virtual bool isReady(std::string& message) const override
+        {
+            if (!is_ready_ && is_ready_message_.size() > 0) {
+                message = is_ready_message_;
+            }
+            return is_ready_;
         }
-        return is_ready_;
-    }
 
-    virtual bool canArm() const override
-    {
-        return is_ready_ && has_gps_lock_;
-    }
+        virtual bool canArm() const override
+        {
+            return is_ready_ && has_gps_lock_;
+        }
 
-    //TODO: this method can't be const yet because it clears previous messages
-    virtual void getStatusMessages(std::vector<std::string>& messages) override
-    {
-        updateState();
+        //TODO: this method can't be const yet because it clears previous messages
+        virtual void getStatusMessages(std::vector<std::string>& messages) override
+        {
+            updateState();
 
-        //clear param
-        messages.clear();
+            //clear param
+            messages.clear();
 
-        //move messages from private vars to param
-        std::lock_guard<std::mutex> guard(status_text_mutex_);
-        while (!status_messages_.empty()) {
-            messages.push_back(status_messages_.front());
-            status_messages_.pop();
+            //move messages from private vars to param
+            std::lock_guard<std::mutex> guard(status_text_mutex_);
+            while (!status_messages_.empty()) {
+                messages.push_back(status_messages_.front());
+                status_messages_.pop();
+            }
         }
-    }
 
-    virtual Kinematics::State getKinematicsEstimated() const override
-    {
-        updateState();
-        Kinematics::State state;
-        //TODO: reduce code duplication in getPosition() etc methods?
-        state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
-        state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
-        state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
-        state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate);
-        state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z);
-        //TODO: how do we get angular acceleration?
-        return state;
-    }
-
-    virtual bool isApiControlEnabled() const override
-    {
-        return is_api_control_enabled_;
-    }
+        virtual Kinematics::State getKinematicsEstimated() const override
+        {
+            updateState();
+            Kinematics::State state;
+            //TODO: reduce code duplication in getPosition() etc methods?
+            state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
+            state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
+            state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
+            state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate);
+            state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z);
+            //TODO: how do we get angular acceleration?
+            return state;
+        }
+
+        virtual bool isApiControlEnabled() const override
+        {
+            return is_api_control_enabled_;
+        }
 
-    virtual void enableApiControl(bool is_enabled) override
-    {
-        checkValidVehicle();
-        if (is_enabled) {
-            mav_vehicle_->requestControl();
-            is_api_control_enabled_ = true;
+        virtual void enableApiControl(bool is_enabled) override
+        {
+            checkValidVehicle();
+            if (is_enabled) {
+                mav_vehicle_->requestControl();
+                is_api_control_enabled_ = true;
+            }
+            else {
+                mav_vehicle_->releaseControl();
+                is_api_control_enabled_ = false;
+            }
         }
-        else {
-            mav_vehicle_->releaseControl();
-            is_api_control_enabled_ = false;
+
+        virtual Vector3r getPosition() const override
+        {
+            updateState();
+            return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
         }
-    }
 
-    virtual Vector3r getPosition() const override
-    {
-        updateState();
-        return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
-    }
+        virtual Vector3r getVelocity() const override
+        {
+            updateState();
+            return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
+        }
 
-    virtual Vector3r getVelocity() const override
-    {
-        updateState();
-        return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
-    }
+        virtual Quaternionr getOrientation() const override
+        {
+            updateState();
+            return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
+        }
 
-    virtual Quaternionr getOrientation() const override
-    {
-        updateState();
-        return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
-    }
+        virtual LandedState getLandedState() const override
+        {
+            updateState();
+            return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
+        }
 
-    virtual LandedState getLandedState() const override
-    {
-        updateState();
-        return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
-    }
+        virtual real_T getActuation(unsigned int rotor_index) const override
+        {
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to read motor controls while not in simulation mode");
 
-    virtual real_T getActuation(unsigned int rotor_index) const override
-    {
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to read motor controls while not in simulation mode");
+            std::lock_guard<std::mutex> guard(hil_controls_mutex_);
+            return rotor_controls_[rotor_index];
+        }
 
-        std::lock_guard<std::mutex> guard(hil_controls_mutex_);
-        return rotor_controls_[rotor_index];
-    }
+        virtual size_t getActuatorCount() const override
+        {
+            return RotorControlsCount;
+        }
 
-    virtual size_t getActuatorCount() const override
-    {
-        return RotorControlsCount;
-    }
+        virtual bool armDisarm(bool arm) override
+        {
+            SingleCall lock(this);
 
-    virtual bool armDisarm(bool arm) override
-    {
-        SingleCall lock(this);
+            checkValidVehicle();
+            bool rc = false;
+            if (arm) {
+                float timeout_sec = 10;
+                waitForHomeLocation(timeout_sec);
+                waitForStableGroundPosition(timeout_sec);
+            }
 
-        checkValidVehicle();
-        bool rc = false;
-        if (arm) {
-            float timeout_sec = 10;
-            waitForHomeLocation(timeout_sec);
-            waitForStableGroundPosition(timeout_sec);
+            mav_vehicle_->armDisarm(arm).wait(10000, &rc);
+            return rc;
         }
 
-        mav_vehicle_->armDisarm(arm).wait(10000, &rc);
-        return rc;
-    }
-
-    void onArmed() 
-    {
-        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
-            auto con = mav_vehicle_->getConnection();
-            if (con != nullptr) {
-                if (log_ != nullptr) {
-                    log_->close();
-                    log_ = nullptr;
-                }
+        void onArmed()
+        {
+            if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+                auto con = mav_vehicle_->getConnection();
+                if (con != nullptr) {
+                    if (log_ != nullptr) {
+                        log_->close();
+                        log_ = nullptr;
+                    }
 
-                try {
-                    std::time_t t = std::time(0);   // get time now
-                    std::tm* now = std::localtime(&t);
-                    auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
-                    auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder);
-                    auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec);
-                    auto fullpath = common_utils::FileSystem::combine(path, filename);
-                    addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str()));
-                    log_file_name_ = fullpath;
-                    log_ = std::make_shared<mavlinkcom::MavLinkFileLog>();
-                    log_->openForWriting(fullpath, false);
-                    con->startLoggingSendMessage(log_);
-                    con->startLoggingReceiveMessage(log_);
-                    if (con != connection_) {
-                        // log the SITL channel also
-                        connection_->startLoggingSendMessage(log_);
-                        connection_->startLoggingReceiveMessage(log_);
+                    try {
+                        std::time_t t = std::time(0); // get time now
+                        std::tm* now = std::localtime(&t);
+                        auto folder = Utils::stringf("%04d-%02d-%02d", now->tm_year + 1900, now->tm_mon + 1, now->tm_mday);
+                        auto path = common_utils::FileSystem::ensureFolder(connection_info_.logs, folder);
+                        auto filename = Utils::stringf("%02d-%02d-%02d.mavlink", now->tm_hour, now->tm_min, now->tm_sec);
+                        auto fullpath = common_utils::FileSystem::combine(path, filename);
+                        addStatusMessage(Utils::stringf("Opening log file: %s", fullpath.c_str()));
+                        log_file_name_ = fullpath;
+                        log_ = std::make_shared<mavlinkcom::MavLinkFileLog>();
+                        log_->openForWriting(fullpath, false);
+                        con->startLoggingSendMessage(log_);
+                        con->startLoggingReceiveMessage(log_);
+                        if (con != connection_) {
+                            // log the SITL channel also
+                            connection_->startLoggingSendMessage(log_);
+                            connection_->startLoggingReceiveMessage(log_);
+                        }
+                        start_telemtry_thread();
+                    }
+                    catch (std::exception& ex) {
+                        addStatusMessage(std::string("Opening log file failed: ") + ex.what());
                     }
-                    start_telemtry_thread();
-                }
-                catch (std::exception& ex) {
-                    addStatusMessage(std::string("Opening log file failed: ") + ex.what());
                 }
             }
         }
-    }
 
-    void onDisarmed() 
-    {
-        if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
-            auto con = mav_vehicle_->getConnection();
-            if (con != nullptr) {
-                con->stopLoggingSendMessage();
-                con->stopLoggingReceiveMessage();
+        void onDisarmed()
+        {
+            if (connection_info_.logs.size() > 0 && mav_vehicle_ != nullptr) {
+                auto con = mav_vehicle_->getConnection();
+                if (con != nullptr) {
+                    con->stopLoggingSendMessage();
+                    con->stopLoggingReceiveMessage();
+                }
+                if (connection_ != nullptr) {
+                    connection_->stopLoggingSendMessage();
+                    connection_->stopLoggingReceiveMessage();
+                }
             }
-            if (connection_ != nullptr) {
-                connection_->stopLoggingSendMessage();
-                connection_->stopLoggingReceiveMessage();
+            if (log_ != nullptr) {
+                addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str()));
+                log_->close();
+                log_ = nullptr;
             }
         }
-        if (log_ != nullptr) {
-            addStatusMessage(Utils::stringf("Closing log file: %s", log_file_name_.c_str()));
-            log_->close();
-            log_ = nullptr;
-        }
-    }
 
-    void waitForHomeLocation(float timeout_sec)
-    {
-        if (!current_state_.home.is_set) {
-            addStatusMessage("Waiting for valid GPS home location...");
-            if (!waitForFunction([&]() {
-                return current_state_.home.is_set;
-                }, timeout_sec).isComplete()) {
-                throw VehicleMoveException("Vehicle does not have a valid GPS home location");
+        void waitForHomeLocation(float timeout_sec)
+        {
+            if (!current_state_.home.is_set) {
+                addStatusMessage("Waiting for valid GPS home location...");
+                if (!waitForFunction([&]() {
+                         return current_state_.home.is_set;
+                     },
+                                     timeout_sec)
+                         .isComplete()) {
+                    throw VehicleMoveException("Vehicle does not have a valid GPS home location");
+                }
             }
         }
-    }
 
-    void waitForStableGroundPosition(float timeout_sec)
-    {
-        // wait for ground stabilization
-        if (ground_variance_ > GroundTolerance) {
-            addStatusMessage("Waiting for z-position to stabilize...");
-            if (!waitForFunction([&]() {
-                return ground_variance_ <= GroundTolerance;
-                }, timeout_sec).isComplete()) {
-                auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
-                throw VehicleMoveException(msg);
+        void waitForStableGroundPosition(float timeout_sec)
+        {
+            // wait for ground stabilization
+            if (ground_variance_ > GroundTolerance) {
+                addStatusMessage("Waiting for z-position to stabilize...");
+                if (!waitForFunction([&]() {
+                         return ground_variance_ <= GroundTolerance;
+                     },
+                                     timeout_sec)
+                         .isComplete()) {
+                    auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
+                    throw VehicleMoveException(msg);
+                }
             }
         }
-    }
-
-    virtual bool takeoff(float timeout_sec) override
-    {
-        SingleCall lock(this);
 
-        checkValidVehicle();
-
-        waitForHomeLocation(timeout_sec);
-        waitForStableGroundPosition(timeout_sec);
-
-        bool rc = false;
-        auto vec = getPosition();
-        auto yaw = current_state_.attitude.yaw;
-        float z = vec.z() + getTakeoffZ();
-        if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
-            throw VehicleMoveException("TakeOff command - timeout waiting for response");
-        }
-        if (!rc) {
-            throw VehicleMoveException("TakeOff command rejected by drone");
-        }
-        if (timeout_sec <= 0)
-            return true; // client doesn't want to wait.
+        virtual bool takeoff(float timeout_sec) override
+        {
+            SingleCall lock(this);
 
-        return waitForZ(timeout_sec, z, getDistanceAccuracy());
-    }
+            checkValidVehicle();
 
-    virtual bool land(float timeout_sec) override
-    {
-        SingleCall lock(this);
+            waitForHomeLocation(timeout_sec);
+            waitForStableGroundPosition(timeout_sec);
 
-        //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
-        //we assume the ground is relatively flat an we are landing roughly at the home altitude.
-        updateState();
-        checkValidVehicle();
-        if (current_state_.home.is_set) {
             bool rc = false;
-            if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) {
-                throw VehicleMoveException("Landing command - timeout waiting for response from drone");
+            auto vec = getPosition();
+            auto yaw = current_state_.attitude.yaw;
+            float z = vec.z() + getTakeoffZ();
+            if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
+                throw VehicleMoveException("TakeOff command - timeout waiting for response");
             }
-            else if (!rc) {
-                throw VehicleMoveException("Landing command rejected by drone");
+            if (!rc) {
+                throw VehicleMoveException("TakeOff command rejected by drone");
             }
-        }
-        else {
-            throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude.  Could fix this if we hook up a distance to ground sensor...");
+            if (timeout_sec <= 0)
+                return true; // client doesn't want to wait.
+
+            return waitForZ(timeout_sec, z, getDistanceAccuracy());
         }
 
-        const auto& waiter = waitForFunction([&]() {
-            updateState();
-            return current_state_.controls.landed;
-            }, timeout_sec);
+        virtual bool land(float timeout_sec) override
+        {
+            SingleCall lock(this);
 
-        // Wait for landed state (or user cancellation)
-        if (!waiter.isComplete()) {
-            throw VehicleMoveException("Drone hasn't reported a landing state");
-        }
-        return waiter.isComplete();
-    }
+            //TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
+            //we assume the ground is relatively flat an we are landing roughly at the home altitude.
+            updateState();
+            checkValidVehicle();
+            if (current_state_.home.is_set) {
+                bool rc = false;
+                if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc)) {
+                    throw VehicleMoveException("Landing command - timeout waiting for response from drone");
+                }
+                else if (!rc) {
+                    throw VehicleMoveException("Landing command rejected by drone");
+                }
+            }
+            else {
+                throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude.  Could fix this if we hook up a distance to ground sensor...");
+            }
 
-    virtual bool goHome(float timeout_sec) override
-    {
-        SingleCall lock(this);
+            const auto& waiter = waitForFunction([&]() {
+                updateState();
+                return current_state_.controls.landed;
+            },
+                                                 timeout_sec);
 
-        checkValidVehicle();
-        bool rc = false;
-        if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
-            static_cast<int>(timeout_sec) * 1000, &rc)) {
-            throw VehicleMoveException("goHome - timeout waiting for response from drone");
+            // Wait for landed state (or user cancellation)
+            if (!waiter.isComplete()) {
+                throw VehicleMoveException("Drone hasn't reported a landing state");
+            }
+            return waiter.isComplete();
         }
-        return rc;
-    }
 
-    virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
-        const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
-    {
-        SingleTaskCall lock(this);
-
-        unused(adaptive_lookahead);
-        unused(lookahead);
-        unused(drivetrain);
-
-        // save current manual, cruise, and max velocity parameters
-        bool result = false;
-        mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
-        result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
-        result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
-        result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
-
-        if (result) {
-            // set max velocity parameter
-            mavlinkcom::MavLinkParameter p;
-            p.name = "MPC_XY_VEL_MAX";
-            p.value = velocity;
-            mav_vehicle_->setParameter(p).wait(1000, &result);
+        virtual bool goHome(float timeout_sec) override
+        {
+            SingleCall lock(this);
 
-            if (result) {
-                const Vector3r& goal_pos = Vector3r(x, y, z);
-                Vector3r goal_dist_vect;
-                float goal_dist;
+            checkValidVehicle();
+            bool rc = false;
+            if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
+                                               static_cast<int>(timeout_sec) * 1000, &rc)) {
+                throw VehicleMoveException("goHome - timeout waiting for response from drone");
+            }
+            return rc;
+        }
 
-                Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
+        virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
+                                    const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
+        {
+            SingleTaskCall lock(this);
 
-                while (!waiter.isComplete()) {
-                    goal_dist_vect = getPosition() - goal_pos;
-                    const Vector3r& goal_normalized = goal_dist_vect.normalized();
-                    goal_dist = goal_dist_vect.dot(goal_normalized);
+            unused(adaptive_lookahead);
+            unused(lookahead);
+            unused(drivetrain);
 
-                    if (goal_dist > getDistanceAccuracy()) {
-                        moveToPositionInternal(goal_pos, yaw_mode);
+            // save current manual, cruise, and max velocity parameters
+            bool result = false;
+            mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
+            result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
+            result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
+            result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
 
-                        //sleep for rest of the cycle
-                        if (!waiter.sleep())
-                            return false;
-                    }
-                    else {
-                        waiter.complete();
+            if (result) {
+                // set max velocity parameter
+                mavlinkcom::MavLinkParameter p;
+                p.name = "MPC_XY_VEL_MAX";
+                p.value = velocity;
+                mav_vehicle_->setParameter(p).wait(1000, &result);
+
+                if (result) {
+                    const Vector3r& goal_pos = Vector3r(x, y, z);
+                    Vector3r goal_dist_vect;
+                    float goal_dist;
+
+                    Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
+
+                    while (!waiter.isComplete()) {
+                        goal_dist_vect = getPosition() - goal_pos;
+                        const Vector3r& goal_normalized = goal_dist_vect.normalized();
+                        goal_dist = goal_dist_vect.dot(goal_normalized);
+
+                        if (goal_dist > getDistanceAccuracy()) {
+                            moveToPositionInternal(goal_pos, yaw_mode);
+
+                            //sleep for rest of the cycle
+                            if (!waiter.sleep())
+                                return false;
+                        }
+                        else {
+                            waiter.complete();
+                        }
                     }
-                }
 
-                // reset manual, cruise, and max velocity parameters
-                bool result_temp = false;
-                mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
-                mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
-                result = result && result_temp;
-                mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
-                result = result && result_temp;
+                    // reset manual, cruise, and max velocity parameters
+                    bool result_temp = false;
+                    mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
+                    mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
+                    result = result && result_temp;
+                    mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
+                    result = result && result_temp;
 
-                return result && waiter.isComplete();
+                    return result && waiter.isComplete();
+                }
             }
-        }
 
-        return result;
-    }
+            return result;
+        }
 
-    virtual bool hover() override
-    {
-        SingleCall lock(this);
+        virtual bool hover() override
+        {
+            SingleCall lock(this);
 
-        bool rc = false;
-        checkValidVehicle();
-        mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
-        //auto start_time = std::chrono::system_clock::now();
-        while (!getCancelToken().isCancelled()) {
-            if (result.wait(100, &rc)) {
-                break;
+            bool rc = false;
+            checkValidVehicle();
+            mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
+            //auto start_time = std::chrono::system_clock::now();
+            while (!getCancelToken().isCancelled()) {
+                if (result.wait(100, &rc)) {
+                    break;
+                }
             }
+            return rc;
         }
-        return rc;
-    }
 
-    virtual GeoPoint getHomeGeoPoint() const override
-    {
-        updateState();
-        if (current_state_.home.is_set)
-            return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
-        else
-            return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
-    }
-
-    virtual GeoPoint getGpsLocation() const override
-    {
-        updateState();
-        return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
-    }
+        virtual GeoPoint getHomeGeoPoint() const override
+        {
+            updateState();
+            if (current_state_.home.is_set)
+                return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
+            else
+                return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
+        }
 
-    virtual void sendTelemetry(float last_interval = -1) override
-    {
-        if (connection_ == nullptr || mav_vehicle_ == nullptr) {
-            return;
+        virtual GeoPoint getGpsLocation() const override
+        {
+            updateState();
+            return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
         }
 
-        // This method is called at high frequence from MultirotorPawnSimApi::updateRendering.
-        mavlinkcom::MavLinkTelemetry data;
-        connection_->getTelemetry(data);
-        if (data.messages_received == 0) {
-            if (!hil_message_timer_.started()) {
-                hil_message_timer_.start();
+        virtual void sendTelemetry(float last_interval = -1) override
+        {
+            if (connection_ == nullptr || mav_vehicle_ == nullptr) {
+                return;
+            }
+
+            // This method is called at high frequence from MultirotorPawnSimApi::updateRendering.
+            mavlinkcom::MavLinkTelemetry data;
+            connection_->getTelemetry(data);
+            if (data.messages_received == 0) {
+                if (!hil_message_timer_.started()) {
+                    hil_message_timer_.start();
+                }
+                else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
+                    addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
+                }
             }
-            else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
-                addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
+            else {
+                hil_message_timer_.stop();
             }
         }
-        else {
-            hil_message_timer_.stop();
-        }
-    }
 
-    void writeTelemetry(float last_interval = -1)
-    {
-        auto proxy = logviewer_proxy_;
-        auto log = log_;
+        void writeTelemetry(float last_interval = -1)
+        {
+            auto proxy = logviewer_proxy_;
+            auto log = log_;
 
-        if ((logviewer_proxy_ == nullptr && log_ == nullptr)) {
-            return;
-        }
+            if ((logviewer_proxy_ == nullptr && log_ == nullptr)) {
+                return;
+            }
 
-        mavlinkcom::MavLinkTelemetry data;
-        connection_->getTelemetry(data);
+            mavlinkcom::MavLinkTelemetry data;
+            connection_->getTelemetry(data);
 
-        // listen to the other mavlink connection also
-        auto mavcon = mav_vehicle_->getConnection();
-        if (mavcon != connection_) {
-            mavlinkcom::MavLinkTelemetry gcs;
-            mavcon->getTelemetry(gcs);
+            // listen to the other mavlink connection also
+            auto mavcon = mav_vehicle_->getConnection();
+            if (mavcon != connection_) {
+                mavlinkcom::MavLinkTelemetry gcs;
+                mavcon->getTelemetry(gcs);
 
-            data.handler_microseconds += gcs.handler_microseconds;
-            data.messages_handled += gcs.messages_handled;
-            data.messages_received += gcs.messages_received;
-            data.messages_sent += gcs.messages_sent;
+                data.handler_microseconds += gcs.handler_microseconds;
+                data.messages_handled += gcs.messages_handled;
+                data.messages_received += gcs.messages_received;
+                data.messages_sent += gcs.messages_sent;
 
-            if (gcs.messages_received == 0) {
-                if (!gcs_message_timer_.started()) {
-                    gcs_message_timer_.start();
+                if (gcs.messages_received == 0) {
+                    if (!gcs_message_timer_.started()) {
+                        gcs_message_timer_.start();
+                    }
+                    else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
+                        addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
+                    }
+                }
+                else {
+                    gcs_message_timer_.stop();
                 }
-                else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
-                    addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
+            }
+
+            data.render_time = static_cast<int64_t>(last_interval * 1000000); // microseconds
+
+            {
+                std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                uint32_t average_delay = 0;
+                uint32_t average_update = 0;
+                if (hil_sensor_count_ != 0) {
+                    average_delay = actuator_delay_ / hil_sensor_count_;
+                    average_update = static_cast<uint32_t>(update_time_ / hil_sensor_count_);
                 }
+
+                data.update_rate = update_count_;
+                data.sensor_rate = hil_sensor_count_;
+                data.actuation_delay = average_delay;
+                data.lock_step_resets = lock_step_resets_;
+                data.update_time = average_update;
+                // reset the counters we just captured.
+                update_count_ = 0;
+                hil_sensor_count_ = 0;
+                actuator_delay_ = 0;
+                update_time_ = 0;
             }
-            else {
-                gcs_message_timer_.stop();
+
+            if (proxy != nullptr) {
+                proxy->sendMessage(data);
             }
-        }
 
-        data.render_time = static_cast<int64_t>(last_interval * 1000000);// microseconds
+            if (log != nullptr) {
+                mavlinkcom::MavLinkMessage msg;
+                msg.magic = MAVLINK_STX_MAVLINK1;
+                data.encode(msg);
+                msg.update_checksum();
+                // disk I/O is unpredictable, so we have to get it out of the update loop
+                // which is why this thread exists.
+                log->write(msg);
+            }
+        }
 
+        void start_telemtry_thread()
         {
-            std::lock_guard<std::mutex> guard(telemetry_mutex_);
-            uint32_t average_delay = 0;
-            uint32_t average_update = 0;
-            if (hil_sensor_count_ != 0) {
-                average_delay = actuator_delay_ / hil_sensor_count_;
-                average_update = static_cast<uint32_t>(update_time_ / hil_sensor_count_);
+            if (this->telemetry_thread_.joinable()) {
+                this->telemetry_thread_.join();
             }
 
-            data.update_rate = update_count_;
-            data.sensor_rate = hil_sensor_count_;
-            data.actuation_delay = average_delay;
-            data.lock_step_resets = lock_step_resets_;
-            data.update_time = average_update;
-            // reset the counters we just captured.
+            // reset the counters we use for telemetry.
             update_count_ = 0;
             hil_sensor_count_ = 0;
             actuator_delay_ = 0;
-            update_time_ = 0;
-        }
 
-        if (proxy != nullptr) {
-            proxy->sendMessage(data);
+            this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this);
         }
 
-        if (log != nullptr) {
-            mavlinkcom::MavLinkMessage msg;
-            msg.magic = MAVLINK_STX_MAVLINK1;
-            data.encode(msg);
-            msg.update_checksum();
-            // disk I/O is unpredictable, so we have to get it out of the update loop
-            // which is why this thread exists.
-            log->write(msg);
+        void telemtry_thread()
+        {
+            while (log_ != nullptr) {
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+                writeTelemetry(1);
+            }
         }
-    }
 
-    void start_telemtry_thread()
-    {
-        if (this->telemetry_thread_.joinable()) {
-            this->telemetry_thread_.join();
+        virtual float getCommandPeriod() const override
+        {
+            return 1.0f / 50; //1 period of 50hz
         }
 
-        // reset the counters we use for telemetry.
-        update_count_ = 0;
-        hil_sensor_count_ = 0;
-        actuator_delay_ = 0;
-
-        this->telemetry_thread_ = std::thread(&MavLinkMultirotorApi::telemtry_thread, this);
-    }
-
-    void telemtry_thread()
-    {
-        while (log_ != nullptr) {
-            std::this_thread::sleep_for(std::chrono::seconds(1));
-            writeTelemetry(1);
+        virtual float getTakeoffZ() const override
+        {
+            // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
+            // enough to get out of the backwash turbulence.  Negative due to NED coordinate system.
+            return -3.0f;
         }
-    }
-
-    virtual float getCommandPeriod() const override
-    {
-        return 1.0f / 50; //1 period of 50hz
-    }
 
-    virtual float getTakeoffZ() const override
-    {
-        // pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
-        // enough to get out of the backwash turbulence.  Negative due to NED coordinate system.
-        return -3.0f;
-    }
+        virtual float getDistanceAccuracy() const override
+        {
+            return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
+        }
 
-    virtual float getDistanceAccuracy() const override
-    {
-        return 0.5f;    //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
-    }
+    protected: //methods
+        virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
+        {
+            unused(controllerType);
+            unused(kp);
+            unused(ki);
+            unused(kd);
+            Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
+        }
 
-protected: //methods
-    virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
-    {
-        unused(controllerType);
-        unused(kp);
-        unused(ki);
-        unused(kd);
-        Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
-    }
-
-    virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
-    {
-        unused(front_right_pwm);
-        unused(front_left_pwm);
-        unused(rear_right_pwm);
-        unused(rear_left_pwm);
-        Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
-    }
-
-    virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
-    {
-        if (target_height_ != -z) {
-            // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
-            // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
-            // been tested on a real drone outside jMavSim, so it may need recalibrating...
-            thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
-            target_height_ = -z;
-        }
-        checkValidVehicle();
-        auto state = mav_vehicle_->getVehicleState();
-        float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
-        mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
-    }
-    virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
-    {
-        if (target_height_ != -z) {
-            thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
-            target_height_ = -z;
-        }
-        checkValidVehicle();
-        auto state = mav_vehicle_->getVehicleState();
-        float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
-        mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
-    }
-    
-    virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
-    {
-        checkValidVehicle();
-        mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
-    }
+        virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
+        {
+            unused(front_right_pwm);
+            unused(front_left_pwm);
+            unused(rear_right_pwm);
+            unused(rear_left_pwm);
+            Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
+        }
 
-    virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
-    {
-        checkValidVehicle();
-        mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
-    }
-    
-    virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
-    {
-        if (target_height_ != -z) {
-            thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
-            target_height_ = -z;
+        virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
+        {
+            if (target_height_ != -z) {
+                // these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
+                // control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
+                // been tested on a real drone outside jMavSim, so it may need recalibrating...
+                thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
+                target_height_ = -z;
+            }
+            checkValidVehicle();
+            auto state = mav_vehicle_->getVehicleState();
+            float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
+            mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
+        }
+        virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
+        {
+            if (target_height_ != -z) {
+                thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
+                target_height_ = -z;
+            }
+            checkValidVehicle();
+            auto state = mav_vehicle_->getVehicleState();
+            float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
+            mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
         }
-        checkValidVehicle();
-        auto state = mav_vehicle_->getVehicleState();
-        float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
-        mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
-    }
 
-    virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
-    {
-        checkValidVehicle();
-        mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
-    }
+        virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
+        {
+            checkValidVehicle();
+            mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
+        }
 
-    virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
-    {
-        checkValidVehicle();
-        float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
-        mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
-    }
+        virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
+        {
+            checkValidVehicle();
+            mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
+        }
 
-    virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
-    {
-        checkValidVehicle();
-        float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
-        mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
-    }
+        virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
+        {
+            if (target_height_ != -z) {
+                thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
+                target_height_ = -z;
+            }
+            checkValidVehicle();
+            auto state = mav_vehicle_->getVehicleState();
+            float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
+            mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
+        }
 
-    virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
-    {
-        checkValidVehicle();
-        float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
-        mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
-    }
+        virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
+        {
+            checkValidVehicle();
+            mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
+        }
 
-    //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
-    virtual const MultirotorApiParams& getMultirotorApiParams() const override
-    {
-        //defaults are good for PX4 generic quadcopter.
-        static const MultirotorApiParams vehicle_params_;
-        return vehicle_params_;
-    }
+        virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
+        {
+            checkValidVehicle();
+            float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+            mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
+        }
 
-    virtual void beforeTask() override
-    {
-        startOffboardMode();
-    }
+        virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
+        {
+            checkValidVehicle();
+            float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+            mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
+        }
 
-    virtual void afterTask() override
-    {
-        endOffboardMode();
-    }
+        virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
+        {
+            checkValidVehicle();
+            float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
+            mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
+        }
 
-public:
+        //TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
+        virtual const MultirotorApiParams& getMultirotorApiParams() const override
+        {
+            //defaults are good for PX4 generic quadcopter.
+            static const MultirotorApiParams vehicle_params_;
+            return vehicle_params_;
+        }
 
-    class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
-    {
-    public:
-        MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy) 
+        virtual void beforeTask() override
         {
-            proxy_ = proxy;
+            startOffboardMode();
         }
 
-        ~MavLinkLogViewerLog() 
+        virtual void afterTask() override
         {
-            proxy_ = nullptr;
+            endOffboardMode();
         }
 
-        void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override 
+    public:
+        class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
         {
-            if (proxy_ != nullptr) {
-                unused(timestamp);
-                mavlinkcom::MavLinkMessage copy;
-                ::memcpy(&copy, &msg, sizeof(mavlinkcom::MavLinkMessage));
-                try {
-                    proxy_->sendMessage(copy);
-                }
-                catch (std::exception&) {
-                    failures++;
-                    if (failures == 100) {
-                        // hmmm, doesn't like the proxy, bad socket perhaps, so stop trying...
-                        proxy_ = nullptr;
+        public:
+            MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy)
+            {
+                proxy_ = proxy;
+            }
+
+            ~MavLinkLogViewerLog()
+            {
+                proxy_ = nullptr;
+            }
+
+            void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override
+            {
+                if (proxy_ != nullptr) {
+                    unused(timestamp);
+                    mavlinkcom::MavLinkMessage copy;
+                    ::memcpy(&copy, &msg, sizeof(mavlinkcom::MavLinkMessage));
+                    try {
+                        proxy_->sendMessage(copy);
+                    }
+                    catch (std::exception&) {
+                        failures++;
+                        if (failures == 100) {
+                            // hmmm, doesn't like the proxy, bad socket perhaps, so stop trying...
+                            proxy_ = nullptr;
+                        }
                     }
                 }
             }
-        }
 
-    private:
-        std::shared_ptr<mavlinkcom::MavLinkNode> proxy_;
-        int failures = 0;
-    };
+        private:
+            std::shared_ptr<mavlinkcom::MavLinkNode> proxy_;
+            int failures = 0;
+        };
 
-
-protected: //methods
-    virtual void connect()
-    {
-        if (!connecting_) {
-            connecting_ = true;
-            if (this->connect_thread_.joinable()) {
-                this->connect_thread_.join();
+    protected: //methods
+        virtual void connect()
+        {
+            if (!connecting_) {
+                connecting_ = true;
+                if (this->connect_thread_.joinable()) {
+                    this->connect_thread_.join();
+                }
+                this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this);
             }
-            this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this);
         }
-    }
 
-    virtual void disconnect() 
-    {
-        addStatusMessage("Disconnecting mavlink vehicle");
-        connected_ = false;
-        connecting_ = false;
+        virtual void disconnect()
+        {
+            addStatusMessage("Disconnecting mavlink vehicle");
+            connected_ = false;
+            connecting_ = false;
 
-        if (is_armed_) {
-            // close the telemetry log.
-            is_armed_ = false;
-            onDisarmed();
-        }
+            if (is_armed_) {
+                // close the telemetry log.
+                is_armed_ = false;
+                onDisarmed();
+            }
 
-        if (connection_ != nullptr) {
-            if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
-                setNormalMode();
+            if (connection_ != nullptr) {
+                if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
+                    setNormalMode();
+                }
+
+                connection_->stopLoggingSendMessage();
+                connection_->stopLoggingReceiveMessage();
+                connection_->close();
             }
 
-            connection_->stopLoggingSendMessage();
-            connection_->stopLoggingReceiveMessage();
-            connection_->close();
-        }
+            if (hil_node_ != nullptr) {
+                hil_node_->close();
+            }
 
-        if (hil_node_ != nullptr) {
-            hil_node_->close();
-        }
+            if (mav_vehicle_ != nullptr) {
+                auto c = mav_vehicle_->getConnection();
+                if (c != nullptr) {
+                    c->stopLoggingSendMessage();
+                    c->stopLoggingReceiveMessage();
+                }
+                mav_vehicle_->close();
+                mav_vehicle_ = nullptr;
+            }
+
+            if (video_server_ != nullptr)
+                video_server_->close();
 
-        if (mav_vehicle_ != nullptr) {
-            auto c = mav_vehicle_->getConnection();
-            if (c != nullptr) {
-                c->stopLoggingSendMessage();
-                c->stopLoggingReceiveMessage();
+            if (logviewer_proxy_ != nullptr) {
+                logviewer_proxy_->close();
+                logviewer_proxy_ = nullptr;
+            }
+
+            if (logviewer_out_proxy_ != nullptr) {
+                logviewer_out_proxy_->close();
+                logviewer_out_proxy_ = nullptr;
             }
-            mav_vehicle_->close();
-            mav_vehicle_ = nullptr;
-        }
 
-        if (video_server_ != nullptr)
-            video_server_->close();
+            if (qgc_proxy_ != nullptr) {
+                qgc_proxy_->close();
+                qgc_proxy_ = nullptr;
+            }
 
-        if (logviewer_proxy_ != nullptr) {
-            logviewer_proxy_->close();
-            logviewer_proxy_ = nullptr;
+            resetState();
         }
 
-        if (logviewer_out_proxy_ != nullptr) {
-            logviewer_out_proxy_->close();
-            logviewer_out_proxy_ = nullptr;
+        void connect_thread()
+        {
+            addStatusMessage("Waiting for mavlink vehicle...");
+            connecting_ = true;
+            createMavConnection(connection_info_);
+            if (mav_vehicle_ != nullptr) {
+                connectToLogViewer();
+                connectToQGC();
+            }
+
+            if (connecting_) {
+                // only set connected if we haven't already been told to disconnect.
+                connecting_ = false;
+                connected_ = true;
+            }
         }
 
-        if (qgc_proxy_ != nullptr) {
-            qgc_proxy_->close();
-            qgc_proxy_ = nullptr;
-        }       
-        
-        resetState();
-    }
+        virtual void close()
+        {
+            disconnect();
+        }
 
-    void connect_thread()
-    {
-        addStatusMessage("Waiting for mavlink vehicle...");
-        connecting_ = true;
-        createMavConnection(connection_info_);
-        if (mav_vehicle_ != nullptr) {
-            connectToLogViewer();
-            connectToQGC();
+        void closeAllConnection()
+        {
+            close();
         }
 
-        if (connecting_) {
-            // only set connected if we haven't already been told to disconnect.
-            connecting_ = false;
-            connected_ = true;
+    private: //methods
+        void openAllConnections()
+        {
+            Utils::log("Opening mavlink connection");
+            close(); //just in case if connections were open
+            resetState(); //reset all variables we might have changed during last session
+            connect();
         }
-    }
 
-    virtual void close()
-    {
-        disconnect();
-    }
+        void getMocapPose(Vector3r& position, Quaternionr& orientation) const
+        {
+            position.x() = MocapPoseMessage.x;
+            position.y() = MocapPoseMessage.y;
+            position.z() = MocapPoseMessage.z;
+            orientation.w() = MocapPoseMessage.q[0];
+            orientation.x() = MocapPoseMessage.q[1];
+            orientation.y() = MocapPoseMessage.q[2];
+            orientation.z() = MocapPoseMessage.q[3];
+        }
 
-    void closeAllConnection()
-    {
-        close();
-    }
+        //TODO: this method used to send collision to external sim. Do we still need this?
+        void sendCollision(float normalX, float normalY, float normalZ)
+        {
+            checkValidVehicle();
+
+            mavlinkcom::MavLinkCollision collision{};
+            collision.src = 1; //provider of data is MavLink system in id field
+            collision.id = mav_vehicle_->getLocalSystemId();
+            collision.action = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
+            collision.threat_level = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
+            // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
+            collision.time_to_minimum_delta = normalX;
+            collision.altitude_minimum_delta = normalY;
+            collision.horizontal_minimum_delta = normalZ;
+            mav_vehicle_->sendMessage(collision);
+        }
+
+        //TODO: do we still need this method?
+        bool hasVideoRequest()
+        {
+            mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
+            return video_server_->hasVideoRequest(image_req);
+        }
 
+        //TODO: do we still need this method?
+        void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
+        {
+            const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
+            video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
+        }
 
-private: //methods
+        //put PX4 in normal mode (i.e. non-simulation mode)
+        void setNormalMode()
+        {
+            if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
 
-    void openAllConnections()
-    {
-        Utils::log("Opening mavlink connection");
-        close(); //just in case if connections were open
-        resetState(); //reset all variables we might have changed during last session
-        connect();
-    }
+                // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
+                std::lock_guard<std::mutex> guard(set_mode_mutex_);
+                int mode = mav_vehicle_->getVehicleState().mode;
+                mode &= ~static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
 
-    void getMocapPose(Vector3r& position, Quaternionr& orientation) const
-    {
-        position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z;
-        orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1];
-        orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3];
-    }
+                mavlinkcom::MavCmdDoSetMode cmd;
+                cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
+                cmd.Mode = static_cast<float>(mode);
+                mav_vehicle_->sendCommand(cmd);
 
-    //TODO: this method used to send collision to external sim. Do we still need this?
-    void sendCollision(float normalX, float normalY, float normalZ)
-    {
-        checkValidVehicle();
-
-        mavlinkcom::MavLinkCollision collision{};
-        collision.src = 1;	//provider of data is MavLink system in id field
-        collision.id = mav_vehicle_->getLocalSystemId();
-        collision.action = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
-        collision.threat_level = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
-        // we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
-        collision.time_to_minimum_delta = normalX;
-        collision.altitude_minimum_delta = normalY;
-        collision.horizontal_minimum_delta = normalZ;
-        mav_vehicle_->sendMessage(collision);
-    }
-
-    //TODO: do we still need this method?
-    bool hasVideoRequest()
-    {
-        mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
-        return video_server_->hasVideoRequest(image_req);
-    }
+                is_hil_mode_set_ = false;
+            }
+        }
 
-    //TODO: do we still need this method?
-    void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
-    {
-        const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
-        video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
-    }
+        //put PX4 in simulation mode
+        void setHILMode()
+        {
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
 
-    //put PX4 in normal mode (i.e. non-simulation mode)
-    void setNormalMode()
-    {
-        if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
+            checkValidVehicle();
 
-            // remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
+            // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
             std::lock_guard<std::mutex> guard(set_mode_mutex_);
             int mode = mav_vehicle_->getVehicleState().mode;
-            mode &= ~static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
+            mode |= static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
 
             mavlinkcom::MavCmdDoSetMode cmd;
             cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
             cmd.Mode = static_cast<float>(mode);
             mav_vehicle_->sendCommand(cmd);
 
-            is_hil_mode_set_ = false;
+            is_hil_mode_set_ = true;
         }
-    }
-
-    //put PX4 in simulation mode
-    void setHILMode()
-    {
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
-
 
-        checkValidVehicle();
-
-        // add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
-        std::lock_guard<std::mutex> guard(set_mode_mutex_);
-        int mode = mav_vehicle_->getVehicleState().mode;
-        mode |= static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
-
-        mavlinkcom::MavCmdDoSetMode cmd;
-        cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
-        cmd.Mode = static_cast<float>(mode);
-        mav_vehicle_->sendCommand(cmd);
-
-        is_hil_mode_set_ = true;
-    }
-
-    bool startOffboardMode()
-    {
-        checkValidVehicle();
-        try {
-            mav_vehicle_->requestControl();
+        bool startOffboardMode()
+        {
+            checkValidVehicle();
+            try {
+                mav_vehicle_->requestControl();
+            }
+            catch (std::exception& ex) {
+                ensureSafeMode();
+                addStatusMessage(std::string("Request control failed: ") + ex.what());
+                return false;
+            }
+            return true;
         }
-        catch (std::exception& ex) {
+
+        void endOffboardMode()
+        {
+            // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth.
+            // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout
+            // when you stop sending move commands and the behavior on timeout is then determined by the drone itself.
+            // mav_vehicle_->releaseControl();
             ensureSafeMode();
-            addStatusMessage(std::string("Request control failed: ") + ex.what());
-            return false;
         }
-        return true;
-    }
 
-    void endOffboardMode()
-    {
-        // bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth.
-        // The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout
-        // when you stop sending move commands and the behavior on timeout is then determined by the drone itself.
-        // mav_vehicle_->releaseControl();
-        ensureSafeMode();
-    }
-
-    void ensureSafeMode()
-    {
-        if (mav_vehicle_ != nullptr) {
-            const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState();
-            if (state.controls.landed || !state.controls.armed) {
-                return;
+        void ensureSafeMode()
+        {
+            if (mav_vehicle_ != nullptr) {
+                const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState();
+                if (state.controls.landed || !state.controls.armed) {
+                    return;
+                }
             }
         }
-    }
 
-    void checkValidVehicle()
-    {
-        if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) {
-            throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding");
+        void checkValidVehicle()
+        {
+            if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) {
+                throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding");
+            }
         }
-    }
 
-    //status update methods should call this first!
-    void updateState() const
-    {
-        StatusLock lock(this);
-        if (mav_vehicle_ != nullptr) {
-            int version = mav_vehicle_->getVehicleStateVersion();
-            if (version != state_version_) {
-                current_state_ = mav_vehicle_->getVehicleState();
-                state_version_ = version;
+        //status update methods should call this first!
+        void updateState() const
+        {
+            StatusLock lock(this);
+            if (mav_vehicle_ != nullptr) {
+                int version = mav_vehicle_->getVehicleStateVersion();
+                if (version != state_version_) {
+                    current_state_ = mav_vehicle_->getVehicleState();
+                    state_version_ = version;
+                }
             }
         }
-    }
 
-    virtual void normalizeRotorControls()
-    {
-        //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
-        //we normalize them to 0 to 1 for PX4
-        if (!is_controls_0_1_) {
-            // change -1 to 1 to 0 to 1.
-            for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
-                rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
+        virtual void normalizeRotorControls()
+        {
+            //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
+            //we normalize them to 0 to 1 for PX4
+            if (!is_controls_0_1_) {
+                // change -1 to 1 to 0 to 1.
+                for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+                    rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
+                }
             }
-        }
-        else {
-            //this applies to PX4 and may work differently on other firmwares.
-            //We use 0.2 as idle rotors which leaves out range of 0.8
-            for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
-                rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
+            else {
+                //this applies to PX4 and may work differently on other firmwares.
+                //We use 0.2 as idle rotors which leaves out range of 0.8
+                for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+                    rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
+                }
             }
         }
-    }
 
-    bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node)
-    {
-        try {
-            // try and send a test message.
-            mavlinkcom::MavLinkHeartbeat test;
-            test.autopilot = static_cast<int>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
-            test.type = static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
-            test.base_mode = 0;
-            test.custom_mode = 0;
-            test.mavlink_version = 3;
-            node->sendMessage(test);
-            test.system_status = 0;
-            return true;
-        }
-        catch (std::exception&) {
-            return false;
+        bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node)
+        {
+            try {
+                // try and send a test message.
+                mavlinkcom::MavLinkHeartbeat test;
+                test.autopilot = static_cast<int>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
+                test.type = static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
+                test.base_mode = 0;
+                test.custom_mode = 0;
+                test.mavlink_version = 3;
+                node->sendMessage(test);
+                test.system_status = 0;
+                return true;
+            }
+            catch (std::exception&) {
+                return false;
+            }
         }
-    }
 
-    bool connectToLogViewer()
-    {
-        //set up logviewer proxy
-        if (connection_info_.logviewer_ip_address.size() > 0) {
-            std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
-            createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip,
-                logviewer_proxy_, connection);
-            if (!sendTestMessage(logviewer_proxy_)) {
-                // error talking to log viewer, so don't keep trying, and close the connection also.
-                logviewer_proxy_->getConnection()->close();
-                logviewer_proxy_ = nullptr;
-            }
+        bool connectToLogViewer()
+        {
+            //set up logviewer proxy
+            if (connection_info_.logviewer_ip_address.size() > 0) {
+                std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
+                createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip, logviewer_proxy_, connection);
+                if (!sendTestMessage(logviewer_proxy_)) {
+                    // error talking to log viewer, so don't keep trying, and close the connection also.
+                    logviewer_proxy_->getConnection()->close();
+                    logviewer_proxy_ = nullptr;
+                }
 
-            std::shared_ptr<mavlinkcom::MavLinkConnection> out_connection;
-            createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip,
-                logviewer_out_proxy_, out_connection);
-            if (!sendTestMessage(logviewer_out_proxy_)) {
-                // error talking to log viewer, so don't keep trying, and close the connection also.
-                logviewer_out_proxy_->getConnection()->close();
-                logviewer_out_proxy_ = nullptr;
-            }
-            else if (mav_vehicle_ != nullptr) {
-                auto proxylog = std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_);
-                mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog);
-                mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog);
-                if (connection_ != nullptr) {
-                    connection_->startLoggingSendMessage(proxylog);
-                    connection_->startLoggingReceiveMessage(proxylog);
+                std::shared_ptr<mavlinkcom::MavLinkConnection> out_connection;
+                createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip, logviewer_out_proxy_, out_connection);
+                if (!sendTestMessage(logviewer_out_proxy_)) {
+                    // error talking to log viewer, so don't keep trying, and close the connection also.
+                    logviewer_out_proxy_->getConnection()->close();
+                    logviewer_out_proxy_ = nullptr;
+                }
+                else if (mav_vehicle_ != nullptr) {
+                    auto proxylog = std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_);
+                    mav_vehicle_->getConnection()->startLoggingSendMessage(proxylog);
+                    mav_vehicle_->getConnection()->startLoggingReceiveMessage(proxylog);
+                    if (connection_ != nullptr) {
+                        connection_->startLoggingSendMessage(proxylog);
+                        connection_->startLoggingReceiveMessage(proxylog);
+                    }
                 }
             }
+            return logviewer_proxy_ != nullptr;
         }
-        return logviewer_proxy_ != nullptr;
-    }
 
-    bool connectToQGC()
-    {
-        if (connection_info_.qgc_ip_address.size() > 0) {
-            std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
-            createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
-            if (!sendTestMessage(qgc_proxy_)) {
-                // error talking to QGC, so don't keep trying, and close the connection also.
-                qgc_proxy_->getConnection()->close();
-                qgc_proxy_ = nullptr;
-            }
-            else {
-                connection->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection_val, const mavlinkcom::MavLinkMessage& msg) {
-                    unused(connection_val);
-                    processQgcMessages(msg);
+        bool connectToQGC()
+        {
+            if (connection_info_.qgc_ip_address.size() > 0) {
+                std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
+                createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
+                if (!sendTestMessage(qgc_proxy_)) {
+                    // error talking to QGC, so don't keep trying, and close the connection also.
+                    qgc_proxy_->getConnection()->close();
+                    qgc_proxy_ = nullptr;
+                }
+                else {
+                    connection->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection_val, const mavlinkcom::MavLinkMessage& msg) {
+                        unused(connection_val);
+                        processQgcMessages(msg);
                     });
+                }
             }
+            return qgc_proxy_ != nullptr;
         }
-        return qgc_proxy_ != nullptr;
-    }
-
 
-    void createProxy(std::string name, std::string ip, int port, string local_host_ip,
-        std::shared_ptr<mavlinkcom::MavLinkNode>& node, std::shared_ptr<mavlinkcom::MavLinkConnection>& connection)
-    {
-        if (connection_ == nullptr)
-            throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call");
+        void createProxy(std::string name, std::string ip, int port, string local_host_ip,
+                         std::shared_ptr<mavlinkcom::MavLinkNode>& node, std::shared_ptr<mavlinkcom::MavLinkConnection>& connection)
+        {
+            if (connection_ == nullptr)
+                throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call");
 
-        connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
+            connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
 
-        // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
-        // and all other messages are funneled through from PX4 via the Join method below.
-        node = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
-        node->connect(connection);
+            // it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
+            // and all other messages are funneled through from PX4 via the Join method below.
+            node = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
+            node->connect(connection);
 
-        // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
-        // send directly to the PX4 (using whatever sysid/compid comes from that remote node).
-        connection_->join(connection);
+            // now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
+            // send directly to the PX4 (using whatever sysid/compid comes from that remote node).
+            connection_->join(connection);
 
-        auto mavcon = mav_vehicle_->getConnection();
-        if (mavcon != connection_) {
-            mavcon->join(connection);
+            auto mavcon = mav_vehicle_->getConnection();
+            if (mavcon != connection_) {
+                mavcon->join(connection);
+            }
         }
-    }
 
-    static std::string findPX4()
-    {
-        auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);
-        for (auto iter = result.begin(); iter != result.end(); iter++) {
-            mavlinkcom::SerialPortInfo info = *iter;
-            if ((
-                (info.vid == pixhawkVendorId) &&
-                (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId)
-                ) ||
-                (
-                    (info.displayName.find(L"PX4_") != std::string::npos)
-                    )) {
-                // printf("Auto Selecting COM port: %S\n", info.displayName.c_str());
-
-                std::string portName_str;
-
-                for (wchar_t ch : info.portName) {
-                    portName_str.push_back(static_cast<char>(ch));
+        static std::string findPX4()
+        {
+            auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);
+            for (auto iter = result.begin(); iter != result.end(); iter++) {
+                mavlinkcom::SerialPortInfo info = *iter;
+                if ((
+                        (info.vid == pixhawkVendorId) &&
+                        (info.pid == pixhawkFMUV4ProductId || info.pid == pixhawkFMUV2ProductId || info.pid == pixhawkFMUV2OldBootloaderProductId || info.pid == pixhawkFMUV5ProductId)) ||
+                    ((info.displayName.find(L"PX4_") != std::string::npos))) {
+                    // printf("Auto Selecting COM port: %S\n", info.displayName.c_str());
+
+                    std::string portName_str;
+
+                    for (wchar_t ch : info.portName) {
+                        portName_str.push_back(static_cast<char>(ch));
+                    }
+                    return portName_str;
                 }
-                return portName_str;
             }
+            return "";
         }
-        return "";
-    }
 
-    void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
-    {
-        if (connection_info.use_serial) {
-            createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate);
-        }
-        else {
-            createMavEthernetConnection(connection_info);
+        void createMavConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
+        {
+            if (connection_info.use_serial) {
+                createMavSerialConnection(connection_info.serial_port, connection_info.baud_rate);
+            }
+            else {
+                createMavEthernetConnection(connection_info);
+            }
+
+            //Uncomment below for sending images over MavLink
+            //connectToVideoServer();
         }
 
-        //Uncomment below for sending images over MavLink
-        //connectToVideoServer();
-    }
+        void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
+        {
+            close();
 
-    void createMavEthernetConnection(const AirSimSettings::MavLinkConnectionInfo& connection_info)
-    {
-        close();
+            connecting_ = true;
+            is_controls_0_1_ = true;
+            std::string remoteIpAddr;
+            Utils::setValue(rotor_controls_, 0.0f);
 
-        connecting_ = true;
-        is_controls_0_1_ = true;        
-        std::string remoteIpAddr;
-        Utils::setValue(rotor_controls_, 0.0f);
+            if (connection_info.use_tcp) {
+                if (connection_info.tcp_port == 0) {
+                    throw std::invalid_argument("TcpPort setting has an invalid value.");
+                }
 
-        if (connection_info.use_tcp) {
-            if (connection_info.tcp_port == 0) {
-                throw std::invalid_argument("TcpPort setting has an invalid value.");
+                auto msg = Utils::stringf("Waiting for TCP connection on port %d, local IP %s", connection_info.tcp_port, connection_info_.local_host_ip.c_str());
+                addStatusMessage(msg);
+                try {
+                    connection_ = std::make_shared<mavlinkcom::MavLinkConnection>();
+                    remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port);
+                }
+                catch (std::exception& e) {
+                    addStatusMessage("Accepting TCP socket failed, is another instance running?");
+                    addStatusMessage(e.what());
+                    return;
+                }
             }
+            else if (connection_info.udp_address.size() > 0) {
+                if (connection_info.udp_port == 0) {
+                    throw std::invalid_argument("UdpPort setting has an invalid value.");
+                }
 
-            auto msg = Utils::stringf("Waiting for TCP connection on port %d, local IP %s", connection_info.tcp_port, connection_info_.local_host_ip.c_str());
-            addStatusMessage(msg);
-            try {
-                connection_ = std::make_shared<mavlinkcom::MavLinkConnection>();
-                remoteIpAddr = connection_->acceptTcp("hil", connection_info_.local_host_ip, connection_info.tcp_port);
-            }
-            catch (std::exception& e) {
-                addStatusMessage("Accepting TCP socket failed, is another instance running?");
-                addStatusMessage(e.what());
-                return;
+                connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port);
             }
-        }
-        else if (connection_info.udp_address.size() > 0) {
-            if (connection_info.udp_port == 0) {
-                throw std::invalid_argument("UdpPort setting has an invalid value.");
+            else {
+                throw std::invalid_argument("Please provide valid connection info for your drone.");
             }
 
-            connection_ = mavlinkcom::MavLinkConnection::connectRemoteUdp("hil", connection_info_.local_host_ip, connection_info.udp_address, connection_info.udp_port);
-        }
-        else {
-            throw std::invalid_argument("Please provide valid connection info for your drone.");
-        }
-
-        // start listening to the SITL connection.
-        connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
-            unused(connection);
-            processMavMessages(msg);
+            // start listening to the SITL connection.
+            connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
+                unused(connection);
+                processMavMessages(msg);
             });
 
-        hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
-        hil_node_->connect(connection_);
+            hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
+            hil_node_->connect(connection_);
 
-        if (connection_info.use_tcp) {
-            addStatusMessage(std::string("Connected to SITL over TCP."));
-        }
-        else {
-            addStatusMessage(std::string("Connected to SITL over UDP."));
-        }
-
-        mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
-
-        if (connection_info_.control_ip_address != "") {
-            if (connection_info_.control_port_local == 0) {
-                throw std::invalid_argument("LocalControlPort setting has an invalid value.");
+            if (connection_info.use_tcp) {
+                addStatusMessage(std::string("Connected to SITL over TCP."));
             }
-            if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") {
-                remoteIpAddr = connection_info_.control_ip_address;
-            }
-            if (remoteIpAddr == "local" || remoteIpAddr == "localhost") {
-                remoteIpAddr = "127.0.0.1";
+            else {
+                addStatusMessage(std::string("Connected to SITL over UDP."));
             }
 
-            // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection.
-            // The HIL mavlink connection can only handle HIL_SENSOR messages.  This separate channel is needed for
-            // everything else.
-            addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP %s ...",
-                connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), remoteIpAddr.c_str()));
+            mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
 
-            // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ?
-            for (int retries = 60; retries >= 0 && connecting_; retries--) {
-                try {
-                    std::shared_ptr<mavlinkcom::MavLinkConnection> gcsConnection;
-                    if (remoteIpAddr == "127.0.0.1") {                        
-                        gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs",
-                            connection_info_.local_host_ip,
-                            connection_info_.control_port_local);
+            if (connection_info_.control_ip_address != "") {
+                if (connection_info_.control_port_local == 0) {
+                    throw std::invalid_argument("LocalControlPort setting has an invalid value.");
+                }
+                if (!connection_info.use_tcp || connection_info_.control_ip_address != "remote") {
+                    remoteIpAddr = connection_info_.control_ip_address;
+                }
+                if (remoteIpAddr == "local" || remoteIpAddr == "localhost") {
+                    remoteIpAddr = "127.0.0.1";
+                }
+
+                // The PX4 SITL mode app cannot receive commands to control the drone over the same HIL mavlink connection.
+                // The HIL mavlink connection can only handle HIL_SENSOR messages.  This separate channel is needed for
+                // everything else.
+                addStatusMessage(Utils::stringf("Connecting to PX4 Control UDP port %d, local IP %s, remote IP %s ...",
+                                                connection_info_.control_port_local,
+                                                connection_info_.local_host_ip.c_str(),
+                                                remoteIpAddr.c_str()));
+
+                // if we try and connect the UDP port too quickly it doesn't work, bug in PX4 ?
+                for (int retries = 60; retries >= 0 && connecting_; retries--) {
+                    try {
+                        std::shared_ptr<mavlinkcom::MavLinkConnection> gcsConnection;
+                        if (remoteIpAddr == "127.0.0.1") {
+                            gcsConnection = mavlinkcom::MavLinkConnection::connectLocalUdp("gcs",
+                                                                                           connection_info_.local_host_ip,
+                                                                                           connection_info_.control_port_local);
+                        }
+                        else {
+                            gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
+                                                                                            connection_info_.local_host_ip,
+                                                                                            remoteIpAddr,
+                                                                                            connection_info_.control_port_remote);
+                        }
+                        mav_vehicle_->connect(gcsConnection);
+                        // need to try and send something to make sure the connection is good.
+                        mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
+                        break;
                     }
-                    else {
-                        gcsConnection = mavlinkcom::MavLinkConnection::connectRemoteUdp("gcs",
-                            connection_info_.local_host_ip,
-                            remoteIpAddr,
-                            connection_info_.control_port_remote);
+                    catch (std::exception&) {
+                        std::this_thread::sleep_for(std::chrono::seconds(1));
                     }
-                    mav_vehicle_->connect(gcsConnection);
-                    // need to try and send something to make sure the connection is good.
-                    mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
-                    break;
                 }
-                catch (std::exception&) {
-                    std::this_thread::sleep_for(std::chrono::seconds(1));
+
+                if (mav_vehicle_ == nullptr) {
+                    // play was stopped!
+                    return;
                 }
-            }
 
-            if (mav_vehicle_ == nullptr) {
-                // play was stopped!
-                return;
+                if (mav_vehicle_->getConnection() != nullptr) {
+                    addStatusMessage(std::string("Ground control connected over UDP."));
+                }
+                else {
+                    addStatusMessage(std::string("Timeout trying to connect ground control over UDP."));
+                    return;
+                }
             }
-
-            if (mav_vehicle_->getConnection() != nullptr) {
-                addStatusMessage(std::string("Ground control connected over UDP."));
+            try {
+                connectVehicle();
             }
-            else {
-                addStatusMessage(std::string("Timeout trying to connect ground control over UDP."));
-                return;
+            catch (std::exception& e) {
+                addStatusMessage("Error connecting vehicle:");
+                addStatusMessage(e.what());
             }
-        }        
-        try {
-            connectVehicle();
-        }
-        catch (std::exception& e) {
-            addStatusMessage("Error connecting vehicle:");
-            addStatusMessage(e.what());
         }
-    }
 
-    void processControlMessages(const mavlinkcom::MavLinkMessage& msg)
-    {
-        // Utils::log(Utils::stringf("Control msg %d", msg.msgid));
-        // PX4 usually sends the following on the control channel.
-        // If nothing is arriving here it means our control channel UDP connection isn't working.
-        // MAVLINK_MSG_ID_HIGHRES_IMU
-        // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET
-        // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW
-        // MAVLINK_MSG_ID_GPS_RAW_INT
-        // MAVLINK_MSG_ID_TIMESYNC
-        // MAVLINK_MSG_ID_ALTITUDE
-        // MAVLINK_MSG_ID_VFR_HUD
-        // MAVLINK_MSG_ID_ESTIMATOR_STATUS
-        // MAVLINK_MSG_ID_EXTENDED_SYS_STATE
-        // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN
-        // MAVLINK_MSG_ID_PING
-        // MAVLINK_MSG_ID_SYS_STATUS
-        // MAVLINK_MSG_ID_SYSTEM_TIME
-        // MAVLINK_MSG_ID_LINK_NODE_STATUS
-        // MAVLINK_MSG_ID_AUTOPILOT_VERSION
-        // MAVLINK_MSG_ID_COMMAND_ACK
-        // MAVLINK_MSG_ID_STATUSTEXT
-        // MAVLINK_MSG_ID_PARAM_VALUE
-        processMavMessages(msg);
-    }
-
-    void connectVehicle()
-    {
-        // listen to this UDP mavlink connection also
-        auto mavcon = mav_vehicle_->getConnection();
-        if (mavcon != nullptr && mavcon != connection_) {
-            mavcon->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
-                unused(connection);                
-                processControlMessages(msg);
-            });
-        }
-        else {
-            mav_vehicle_->connect(connection_);
+        void processControlMessages(const mavlinkcom::MavLinkMessage& msg)
+        {
+            // Utils::log(Utils::stringf("Control msg %d", msg.msgid));
+            // PX4 usually sends the following on the control channel.
+            // If nothing is arriving here it means our control channel UDP connection isn't working.
+            // MAVLINK_MSG_ID_HIGHRES_IMU
+            // MAVLINK_MSG_ID_ACTUATOR_CONTROL_TARGET
+            // MAVLINK_MSG_ID_SERVO_OUTPUT_RAW
+            // MAVLINK_MSG_ID_GPS_RAW_INT
+            // MAVLINK_MSG_ID_TIMESYNC
+            // MAVLINK_MSG_ID_ALTITUDE
+            // MAVLINK_MSG_ID_VFR_HUD
+            // MAVLINK_MSG_ID_ESTIMATOR_STATUS
+            // MAVLINK_MSG_ID_EXTENDED_SYS_STATE
+            // MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN
+            // MAVLINK_MSG_ID_PING
+            // MAVLINK_MSG_ID_SYS_STATUS
+            // MAVLINK_MSG_ID_SYSTEM_TIME
+            // MAVLINK_MSG_ID_LINK_NODE_STATUS
+            // MAVLINK_MSG_ID_AUTOPILOT_VERSION
+            // MAVLINK_MSG_ID_COMMAND_ACK
+            // MAVLINK_MSG_ID_STATUSTEXT
+            // MAVLINK_MSG_ID_PARAM_VALUE
+            processMavMessages(msg);
         }
 
-        connected_ = true;
+        void connectVehicle()
+        {
+            // listen to this UDP mavlink connection also
+            auto mavcon = mav_vehicle_->getConnection();
+            if (mavcon != nullptr && mavcon != connection_) {
+                mavcon->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
+                    unused(connection);
+                    processControlMessages(msg);
+                });
+            }
+            else {
+                mav_vehicle_->connect(connection_);
+            }
+
+            connected_ = true;
 
-        // Also request home position messages
-        mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
+            // Also request home position messages
+            mav_vehicle_->setMessageInterval(mavlinkcom::MavLinkHomePosition::kMessageId, 1);
 
-        // now we can start our heartbeats.
-        mav_vehicle_->startHeartbeat();
+            // now we can start our heartbeats.
+            mav_vehicle_->startHeartbeat();
 
-        // wait for px4 to connect so we can safely send any configured parameters
-        while (!send_params_ && connected_) {
-            std::this_thread::sleep_for(std::chrono::seconds(1));
-        }
+            // wait for px4 to connect so we can safely send any configured parameters
+            while (!send_params_ && connected_) {
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
 
-        if (connected_) {
-            sendParams();
-            send_params_ = false;
+            if (connected_) {
+                sendParams();
+                send_params_ = false;
+            }
         }
-    }
 
-    void createMavSerialConnection(const std::string& port_name, int baud_rate)
-    {
-        close();
+        void createMavSerialConnection(const std::string& port_name, int baud_rate)
+        {
+            close();
 
-        connecting_ = true;
-        bool reported = false;
-        std::string port_name_auto = port_name;
-        while (port_name_auto == "" || port_name_auto == "*") {
-            port_name_auto = findPX4();
-            if (port_name_auto == "") {
-                if (!reported) {
-                    reported = true;
-                    addStatusMessage("Could not detect a connected PX4 flight controller on any USB ports.");
-                    addStatusMessage("You can specify USB port in settings.json.");
+            connecting_ = true;
+            bool reported = false;
+            std::string port_name_auto = port_name;
+            while (port_name_auto == "" || port_name_auto == "*") {
+                port_name_auto = findPX4();
+                if (port_name_auto == "") {
+                    if (!reported) {
+                        reported = true;
+                        addStatusMessage("Could not detect a connected PX4 flight controller on any USB ports.");
+                        addStatusMessage("You can specify USB port in settings.json.");
+                    }
+                    std::this_thread::sleep_for(std::chrono::seconds(1));
                 }
-                std::this_thread::sleep_for(std::chrono::seconds(1));
             }
-        }
 
-        if (port_name_auto == "") {
-            addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json.");
-            return;
-        }
+            if (port_name_auto == "") {
+                addStatusMessage("USB port for PX4 flight controller is empty. Please set it in settings.json.");
+                return;
+            }
 
-        if (baud_rate == 0) {
-            addStatusMessage("Baud rate specified in settings.json is 0 which is invalid");
-            return;
-        }
+            if (baud_rate == 0) {
+                addStatusMessage("Baud rate specified in settings.json is 0 which is invalid");
+                return;
+            }
 
-        addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
-        reported = false;
+            addStatusMessage(Utils::stringf("Connecting to PX4 over serial port: %s, baud rate %d ....", port_name_auto.c_str(), baud_rate));
+            reported = false;
 
-        while (connecting_) {
-            try {
-                connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
-                connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mo-cap messages
-                hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
-                hil_node_->connect(connection_);
-                addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str()));
-
-                // start listening to the HITL connection.
-                connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
-                    unused(connection);
-                    processMavMessages(msg);
+            while (connecting_) {
+                try {
+                    connection_ = mavlinkcom::MavLinkConnection::connectSerial("hil", port_name_auto, baud_rate);
+                    connection_->ignoreMessage(mavlinkcom::MavLinkAttPosMocap::kMessageId); //TODO: find better way to communicate debug pose instead of using fake Mo-cap messages
+                    hil_node_ = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
+                    hil_node_->connect(connection_);
+                    addStatusMessage(Utils::stringf("Connected to PX4 over serial port: %s", port_name_auto.c_str()));
+
+                    // start listening to the HITL connection.
+                    connection_->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection, const mavlinkcom::MavLinkMessage& msg) {
+                        unused(connection);
+                        processMavMessages(msg);
                     });
 
-                mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
+                    mav_vehicle_ = std::make_shared<mavlinkcom::MavLinkVehicle>(connection_info_.vehicle_sysid, connection_info_.vehicle_compid);
 
-                connectVehicle();
-                return;
-            }
-            catch (std::exception& e) {
-                if (!reported) {
-                    reported = true;
-                    addStatusMessage("Error connecting to mavlink vehicle.");
-                    addStatusMessage(e.what());
-                    addStatusMessage("Please check your USB port in settings.json.");
+                    connectVehicle();
+                    return;
+                }
+                catch (std::exception& e) {
+                    if (!reported) {
+                        reported = true;
+                        addStatusMessage("Error connecting to mavlink vehicle.");
+                        addStatusMessage(e.what());
+                        addStatusMessage("Please check your USB port in settings.json.");
+                    }
+                    std::this_thread::sleep_for(std::chrono::seconds(1));
                 }
-                std::this_thread::sleep_for(std::chrono::seconds(1));
             }
         }
-    }
 
-    mavlinkcom::MavLinkHilSensor getLastSensorMessage()
-    {
-        std::lock_guard<std::mutex> guard(last_message_mutex_);       
-        return last_sensor_message_;
-    }
+        mavlinkcom::MavLinkHilSensor getLastSensorMessage()
+        {
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            return last_sensor_message_;
+        }
 
-    mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage()
-    {
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
-        return last_distance_message_;
-    }
+        mavlinkcom::MavLinkDistanceSensor getLastDistanceMessage()
+        {
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            return last_distance_message_;
+        }
 
-    mavlinkcom::MavLinkHilGps getLastGpsMessage()
-    {
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
-        return last_gps_message_;
-    }
+        mavlinkcom::MavLinkHilGps getLastGpsMessage()
+        {
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            return last_gps_message_;
+        }
 
-    void sendParams()
-    {
-        // send any mavlink parameters from settings.json through to the connected vehicle.
-        if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) {
-            try {
-                for (auto iter : connection_info_.params) {                
-                    auto key = iter.first;
-                    auto value = iter.second;
-                    mavlinkcom::MavLinkParameter p;
-                    p.name = key;
-                    p.value = value;
-                    bool result = false;
-                    mav_vehicle_->setParameter(p).wait(1000, &result);
-                    if (!result) {
-                        Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str()));
-                    }               
+        void sendParams()
+        {
+            // send any mavlink parameters from settings.json through to the connected vehicle.
+            if (mav_vehicle_ != nullptr && connection_info_.params.size() > 0) {
+                try {
+                    for (auto iter : connection_info_.params) {
+                        auto key = iter.first;
+                        auto value = iter.second;
+                        mavlinkcom::MavLinkParameter p;
+                        p.name = key;
+                        p.value = value;
+                        bool result = false;
+                        mav_vehicle_->setParameter(p).wait(1000, &result);
+                        if (!result) {
+                            Utils::log(Utils::stringf("Failed to set mavlink parameter '%s'", key.c_str()));
+                        }
+                    }
+                }
+                catch (std::exception& ex) {
+                    addStatusMessage("Exception sending parameters to vehicle");
+                    addStatusMessage(ex.what());
                 }
-            }
-            catch (std::exception& ex) {
-                addStatusMessage("Exception sending parameters to vehicle");
-                addStatusMessage(ex.what());
             }
         }
-    }
 
-    void setArmed(bool armed)
-    {
-        if (is_armed_) {
-            if (!armed) {
-                onDisarmed();
+        void setArmed(bool armed)
+        {
+            if (is_armed_) {
+                if (!armed) {
+                    onDisarmed();
+                }
             }
-        }
-        else {
-            if (armed) {
-                onArmed();
+            else {
+                if (armed) {
+                    onArmed();
+                }
             }
-        }
 
-        is_armed_ = armed;
-        if (!armed) {
-            //reset motor controls
-            for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
-                rotor_controls_[i] = 0;
+            is_armed_ = armed;
+            if (!armed) {
+                //reset motor controls
+                for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+                    rotor_controls_[i] = 0;
+                }
             }
         }
-    }
 
-    void processQgcMessages(const mavlinkcom::MavLinkMessage& msg)
-    {
-        if (msg.msgid == MocapPoseMessage.msgid) {
-            std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
-            MocapPoseMessage.decode(msg);
-            getMocapPose(mocap_pose_.position, mocap_pose_.orientation);
-        }
-        //else ignore message
-    }
-
-    void addStatusMessage(const std::string& message)
-    {
-        if (message.size() != 0) {
-            Utils::log(message);
-            std::lock_guard<std::mutex> guard_status(status_text_mutex_);
-            //if queue became too large, clear it first
-            if (status_messages_.size() > status_messages_MaxSize)
-                Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size());
-            status_messages_.push(message);
+        void processQgcMessages(const mavlinkcom::MavLinkMessage& msg)
+        {
+            if (msg.msgid == MocapPoseMessage.msgid) {
+                std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
+                MocapPoseMessage.decode(msg);
+                getMocapPose(mocap_pose_.position, mocap_pose_.orientation);
+            }
+            //else ignore message
         }
-    }
 
-    void handleLockStep()
-    {
-        received_actuator_controls_ = true;
-        // if the timestamps match then it means we are in lockstep mode.
-        if (!lock_step_enabled_) {
-            // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
-            if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
-                addStatusMessage("Enabling lockstep mode");
-                lock_step_enabled_ = true;
+        void addStatusMessage(const std::string& message)
+        {
+            if (message.size() != 0) {
+                Utils::log(message);
+                std::lock_guard<std::mutex> guard_status(status_text_mutex_);
+                //if queue became too large, clear it first
+                if (status_messages_.size() > status_messages_MaxSize)
+                    Utils::clear(status_messages_, status_messages_MaxSize - status_messages_.size());
+                status_messages_.push(message);
             }
         }
-        else
+
+        void handleLockStep()
         {
-            auto now = clock()->nowNanos() / 1000;
-            auto delay = static_cast<uint32_t>(now - last_update_time_);
+            received_actuator_controls_ = true;
+            // if the timestamps match then it means we are in lockstep mode.
+            if (!lock_step_enabled_) {
+                // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
+                if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
+                    addStatusMessage("Enabling lockstep mode");
+                    lock_step_enabled_ = true;
+                }
+            }
+            else {
+                auto now = clock()->nowNanos() / 1000;
+                auto delay = static_cast<uint32_t>(now - last_update_time_);
 
-            std::lock_guard<std::mutex> guard(telemetry_mutex_);
-            actuator_delay_ += delay;
-        }
-        if (world_ != nullptr) {
-            world_->pause(false);
+                std::lock_guard<std::mutex> guard(telemetry_mutex_);
+                actuator_delay_ += delay;
+            }
+            if (world_ != nullptr) {
+                world_->pause(false);
+            }
         }
-    }
-
-    void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
-    {
-        if (msg.msgid == HeartbeatMessage.msgid) {
-            std::lock_guard<std::mutex> guard_heartbeat(heartbeat_mutex_);
-
-            HeartbeatMessage.decode(msg);
 
-            bool armed = (HeartbeatMessage.base_mode & static_cast<uint8_t>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0;
-            setArmed(armed);
-            if (!got_first_heartbeat_) {
-                Utils::log("received first heartbeat");
-
-                got_first_heartbeat_ = true;
-                if (HeartbeatMessage.autopilot == static_cast<uint8_t>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) &&
-                    HeartbeatMessage.type == static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) {
-                    // PX4 will scale fixed wing servo outputs to -1 to 1
-                    // and it scales multi rotor servo output to 0 to 1.
-                    is_controls_0_1_ = false;
+        void processMavMessages(const mavlinkcom::MavLinkMessage& msg)
+        {
+            if (msg.msgid == HeartbeatMessage.msgid) {
+                std::lock_guard<std::mutex> guard_heartbeat(heartbeat_mutex_);
+
+                HeartbeatMessage.decode(msg);
+
+                bool armed = (HeartbeatMessage.base_mode & static_cast<uint8_t>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_SAFETY_ARMED)) > 0;
+                setArmed(armed);
+                if (!got_first_heartbeat_) {
+                    Utils::log("received first heartbeat");
+
+                    got_first_heartbeat_ = true;
+                    if (HeartbeatMessage.autopilot == static_cast<uint8_t>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4) &&
+                        HeartbeatMessage.type == static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_FIXED_WING)) {
+                        // PX4 will scale fixed wing servo outputs to -1 to 1
+                        // and it scales multi rotor servo output to 0 to 1.
+                        is_controls_0_1_ = false;
+                    }
+                }
+                else if (is_simulation_mode_ && !is_hil_mode_set_) {
+                    setHILMode();
                 }
             }
-            else if (is_simulation_mode_ && !is_hil_mode_set_) {
-                setHILMode();
+            else if (msg.msgid == StatusTextMessage.msgid) {
+                StatusTextMessage.decode(msg);
+                //lock is established by below method
+                addStatusMessage(std::string(StatusTextMessage.text));
+            }
+            else if (msg.msgid == CommandLongMessage.msgid) {
+                CommandLongMessage.decode(msg);
+                if (CommandLongMessage.command == static_cast<int>(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) {
+                    int msg_id = static_cast<int>(CommandLongMessage.param1 + 0.5);
+                    if (msg_id == 115) { //HIL_STATE_QUATERNION
+                        hil_state_freq_ = static_cast<int>(CommandLongMessage.param2 + 0.5);
+                    }
+                }
             }
-        }
-        else if (msg.msgid == StatusTextMessage.msgid) {
-            StatusTextMessage.decode(msg);
-            //lock is established by below method
-            addStatusMessage(std::string(StatusTextMessage.text));
-        }
-        else if (msg.msgid == CommandLongMessage.msgid) {
-            CommandLongMessage.decode(msg);
-            if (CommandLongMessage.command == static_cast<int>(mavlinkcom::MAV_CMD::MAV_CMD_SET_MESSAGE_INTERVAL)) {
-                int msg_id = static_cast<int>(CommandLongMessage.param1 + 0.5);
-                if (msg_id == 115) { //HIL_STATE_QUATERNION
-                    hil_state_freq_ = static_cast<int>(CommandLongMessage.param2 + 0.5);
+            else if (msg.msgid == HilControlsMessage.msgid) {
+                if (!actuators_message_supported_) {
+                    std::lock_guard<std::mutex> guard_controls(hil_controls_mutex_);
+
+                    HilControlsMessage.decode(msg);
+                    rotor_controls_[0] = HilControlsMessage.roll_ailerons;
+                    rotor_controls_[1] = HilControlsMessage.pitch_elevator;
+                    rotor_controls_[2] = HilControlsMessage.yaw_rudder;
+                    rotor_controls_[3] = HilControlsMessage.throttle;
+                    rotor_controls_[4] = HilControlsMessage.aux1;
+                    rotor_controls_[5] = HilControlsMessage.aux2;
+                    rotor_controls_[6] = HilControlsMessage.aux3;
+                    rotor_controls_[7] = HilControlsMessage.aux4;
+
+                    normalizeRotorControls();
+                    handleLockStep();
                 }
             }
-        }
-        else if (msg.msgid == HilControlsMessage.msgid) {
-            if (!actuators_message_supported_) {
-                std::lock_guard<std::mutex> guard_controls(hil_controls_mutex_);
+            else if (msg.msgid == HilActuatorControlsMessage.msgid) {
+                actuators_message_supported_ = true;
 
-                HilControlsMessage.decode(msg);
-                rotor_controls_[0] = HilControlsMessage.roll_ailerons;
-                rotor_controls_[1] = HilControlsMessage.pitch_elevator;
-                rotor_controls_[2] = HilControlsMessage.yaw_rudder;
-                rotor_controls_[3] = HilControlsMessage.throttle;
-                rotor_controls_[4] = HilControlsMessage.aux1;
-                rotor_controls_[5] = HilControlsMessage.aux2;
-                rotor_controls_[6] = HilControlsMessage.aux3;
-                rotor_controls_[7] = HilControlsMessage.aux4;
+                std::lock_guard<std::mutex> guard_actuator(hil_controls_mutex_); //use same mutex as HIL_CONTROl
+
+                HilActuatorControlsMessage.decode(msg);
+                bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0;
+                for (auto i = 0; i < 8; ++i) {
+                    if (isarmed) {
+                        rotor_controls_[i] = HilActuatorControlsMessage.controls[i];
+                    }
+                    else {
+                        rotor_controls_[i] = 0;
+                    }
+                }
+                if (isarmed) {
+                    normalizeRotorControls();
+                }
 
-                normalizeRotorControls();
                 handleLockStep();
             }
-        }
-        else if (msg.msgid == HilActuatorControlsMessage.msgid) {
-            actuators_message_supported_ = true;
-
-            std::lock_guard<std::mutex> guard_actuator(hil_controls_mutex_);    //use same mutex as HIL_CONTROl
-
-            HilActuatorControlsMessage.decode(msg);
-            bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0;
-            for (auto i = 0; i < 8; ++i) {
-                if (isarmed) {
-                    rotor_controls_[i] = HilActuatorControlsMessage.controls[i];
+            else if (msg.msgid == MavLinkGpsRawInt.msgid) {
+                MavLinkGpsRawInt.decode(msg);
+                auto fix_type = static_cast<mavlinkcom::GPS_FIX_TYPE>(MavLinkGpsRawInt.fix_type);
+                auto locked = (fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_GPS &&
+                               fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_FIX);
+                if (locked && !has_gps_lock_) {
+                    addStatusMessage("Got GPS lock");
+                    has_gps_lock_ = true;
                 }
-                else {
-                    rotor_controls_[i] = 0;
+                if (!has_home_ && current_state_.home.is_set) {
+                    addStatusMessage("Got GPS Home Location");
+                    has_home_ = true;
                 }
+                send_params_ = true;
             }
-            if (isarmed) {
-                normalizeRotorControls();
+            else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) {
+                // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff.
+                if (current_state_.controls.landed) {
+                    monitorGroundAltitude();
+                }
             }
-            
-            handleLockStep();
-        }
-        else if (msg.msgid == MavLinkGpsRawInt.msgid) {
-            MavLinkGpsRawInt.decode(msg);
-            auto fix_type = static_cast<mavlinkcom::GPS_FIX_TYPE>(MavLinkGpsRawInt.fix_type);
-            auto locked = (fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_GPS &&
-                fix_type != mavlinkcom::GPS_FIX_TYPE::GPS_FIX_TYPE_NO_FIX);
-            if (locked && !has_gps_lock_) {
-                addStatusMessage("Got GPS lock");
-                has_gps_lock_ = true;
+            else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) {
+                // check landed state.
+                getLandedState();
+                send_params_ = true;
             }
-            if (!has_home_ && current_state_.home.is_set) {
-                addStatusMessage("Got GPS Home Location");
-                has_home_ = true;
+            else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) {
+                mavlinkcom::MavLinkHomePosition home;
+                home.decode(msg);
+                // this is a good time to send the params
+                send_params_ = true;
             }
-            send_params_ = true;
-        }
-        else if (msg.msgid == mavlinkcom::MavLinkLocalPositionNed::kMessageId) {
-            // we are getting position information... so we can use this to check the stability of the z coordinate before takeoff.
-            if (current_state_.controls.landed) {
-                monitorGroundAltitude();
+            else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) {
+                // this is a good time to send the params
+                send_params_ = true;
+            }
+            else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) {
+                // this is a good time to send the params
+                send_params_ = true;
+            }
+            else {
+                // creates too much log output, only do this when debugging this issue specifically.
+                // Utils::log(Utils::stringf("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
             }
         }
-        else if (msg.msgid == mavlinkcom::MavLinkExtendedSysState::kMessageId) {
-            // check landed state.
-            getLandedState();
-            send_params_ = true;
-        }
-        else if (msg.msgid == mavlinkcom::MavLinkHomePosition::kMessageId) {
-            mavlinkcom::MavLinkHomePosition home;
-            home.decode(msg);
-            // this is a good time to send the params
-            send_params_ = true;
-        }
-        else if (msg.msgid == mavlinkcom::MavLinkSysStatus::kMessageId) {
-            // this is a good time to send the params
-            send_params_ = true;
-        }
-        else if (msg.msgid == mavlinkcom::MavLinkAutopilotVersion::kMessageId) {
-            // this is a good time to send the params
-            send_params_ = true;
-        }
-        else {
-            // creates too much log output, only do this when debugging this issue specifically.
-            // Utils::log(Utils::stringf("Ignoring msg %d from %d,%d ", msg.msgid, msg.compid, msg.sysid));
-        }
-    }
 
-    void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt)
-    {
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
-        
-        mavlinkcom::MavLinkHilSensor hil_sensor;
-        hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime();
+        void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro, const Vector3r& mag, float abs_pressure, float pressure_alt)
+        {
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
 
-        hil_sensor.xacc = acceleration.x();
-        hil_sensor.yacc = acceleration.y();
-        hil_sensor.zacc = acceleration.z();
-        hil_sensor.fields_updated = 0b111; // Set accel bit fields
+            mavlinkcom::MavLinkHilSensor hil_sensor;
+            hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime();
 
-        hil_sensor.xgyro = gyro.x();
-        hil_sensor.ygyro = gyro.y();
-        hil_sensor.zgyro = gyro.z();
+            hil_sensor.xacc = acceleration.x();
+            hil_sensor.yacc = acceleration.y();
+            hil_sensor.zacc = acceleration.z();
+            hil_sensor.fields_updated = 0b111; // Set accel bit fields
 
-        hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields 
+            hil_sensor.xgyro = gyro.x();
+            hil_sensor.ygyro = gyro.y();
+            hil_sensor.zgyro = gyro.z();
 
-        hil_sensor.xmag = mag.x();
-        hil_sensor.ymag = mag.y();
-        hil_sensor.zmag = mag.z();
+            hil_sensor.fields_updated |= 0b111000; // Set gyro bit fields
 
-        hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields
+            hil_sensor.xmag = mag.x();
+            hil_sensor.ymag = mag.y();
+            hil_sensor.zmag = mag.z();
 
-        hil_sensor.abs_pressure = abs_pressure;
-        hil_sensor.pressure_alt = pressure_alt;
+            hil_sensor.fields_updated |= 0b111000000; // Set mag bit fields
 
-        hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields
+            hil_sensor.abs_pressure = abs_pressure;
+            hil_sensor.pressure_alt = pressure_alt;
 
-        //TODO: enable temperature? diff_pressure
-        if (was_reset_) {
-            hil_sensor.fields_updated = static_cast<uint32_t>(1 << 31);
-        }
+            hil_sensor.fields_updated |= 0b1101000000000; // Set baro bit fields
 
-        if (hil_node_ != nullptr) {
-            hil_node_->sendMessage(hil_sensor);
-            received_actuator_controls_ = false;
-            if (lock_step_enabled_ && world_ != nullptr) {
-                world_->pauseForTime(0.1); // 100 millisecond delay max waiting for actuator controls.
+            //TODO: enable temperature? diff_pressure
+            if (was_reset_) {
+                hil_sensor.fields_updated = static_cast<uint32_t>(1 << 31);
             }
-        }
-
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
-        last_sensor_message_ = hil_sensor;
-    }
 
-    void sendSystemTime()
-    {
-        // SYSTEM TIME from host
-        auto tu = getSimTime();
-        uint32_t ms = (uint32_t)(tu / 1000);
-        if (ms != last_sys_time_) {
-            last_sys_time_ = ms;
-            mavlinkcom::MavLinkSystemTime msg_system_time;
-            msg_system_time.time_unix_usec = tu;
-            msg_system_time.time_boot_ms = last_sys_time_;
             if (hil_node_ != nullptr) {
-                hil_node_->sendMessage(msg_system_time);
+                hil_node_->sendMessage(hil_sensor);
+                received_actuator_controls_ = false;
+                if (lock_step_enabled_ && world_ != nullptr) {
+                    world_->pauseForTime(0.1); // 100 millisecond delay max waiting for actuator controls.
+                }
             }
-        }
-    }
 
-    void sendDistanceSensor(float min_distance, float max_distance, float current_distance,
-        uint8_t sensor_type, uint8_t sensor_id, const Quaternionr& orientation)
-    {
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            last_sensor_message_ = hil_sensor;
+        }
 
-        mavlinkcom::MavLinkDistanceSensor distance_sensor;
-        distance_sensor.time_boot_ms = static_cast<uint32_t>(getSimTime() / 1000);
-        distance_sensor.min_distance = static_cast<uint16_t>(min_distance);
-        distance_sensor.max_distance = static_cast<uint16_t>(max_distance);
-        distance_sensor.current_distance = static_cast<uint16_t>(current_distance);
-        distance_sensor.type = sensor_type;
-        distance_sensor.id = sensor_id;
+        void sendSystemTime()
+        {
+            // SYSTEM TIME from host
+            auto tu = getSimTime();
+            uint32_t ms = (uint32_t)(tu / 1000);
+            if (ms != last_sys_time_) {
+                last_sys_time_ = ms;
+                mavlinkcom::MavLinkSystemTime msg_system_time;
+                msg_system_time.time_unix_usec = tu;
+                msg_system_time.time_boot_ms = last_sys_time_;
+                if (hil_node_ != nullptr) {
+                    hil_node_->sendMessage(msg_system_time);
+                }
+            }
+        }
 
-        // Use custom orientation
-        distance_sensor.orientation = 100;  // MAV_SENSOR_ROTATION_CUSTOM
-        distance_sensor.quaternion[0] = orientation.w();
-        distance_sensor.quaternion[1] = orientation.x();
-        distance_sensor.quaternion[2] = orientation.y();
-        distance_sensor.quaternion[3] = orientation.z();
+        void sendDistanceSensor(float min_distance, float max_distance, float current_distance,
+                                uint8_t sensor_type, uint8_t sensor_id, const Quaternionr& orientation)
+        {
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
+
+            mavlinkcom::MavLinkDistanceSensor distance_sensor;
+            distance_sensor.time_boot_ms = static_cast<uint32_t>(getSimTime() / 1000);
+            distance_sensor.min_distance = static_cast<uint16_t>(min_distance);
+            distance_sensor.max_distance = static_cast<uint16_t>(max_distance);
+            distance_sensor.current_distance = static_cast<uint16_t>(current_distance);
+            distance_sensor.type = sensor_type;
+            distance_sensor.id = sensor_id;
+
+            // Use custom orientation
+            distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM
+            distance_sensor.quaternion[0] = orientation.w();
+            distance_sensor.quaternion[1] = orientation.x();
+            distance_sensor.quaternion[2] = orientation.y();
+            distance_sensor.quaternion[3] = orientation.z();
+
+            //TODO: use covariance parameter?
 
-        //TODO: use covariance parameter?
+            if (hil_node_ != nullptr) {
+                hil_node_->sendMessage(distance_sensor);
+            }
 
-        if (hil_node_ != nullptr) {
-            hil_node_->sendMessage(distance_sensor);
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            last_distance_message_ = distance_sensor;
         }
 
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
-        last_distance_message_ = distance_sensor;
-    }
-
-    void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog,
-        float eph, float epv, int fix_type, unsigned int satellites_visible)
-    {
-        unused(satellites_visible);
+        void sendHILGps(const GeoPoint& geo_point, const Vector3r& velocity, float velocity_xy, float cog,
+                        float eph, float epv, int fix_type, unsigned int satellites_visible)
+        {
+            unused(satellites_visible);
+
+            if (!is_simulation_mode_)
+                throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
+
+            mavlinkcom::MavLinkHilGps hil_gps;
+            hil_gps.time_usec = getSimTime();
+            hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
+            hil_gps.lon = static_cast<int32_t>(geo_point.longitude * 1E7);
+            hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);
+            hil_gps.vn = static_cast<int16_t>(velocity.x() * 100);
+            hil_gps.ve = static_cast<int16_t>(velocity.y() * 100);
+            hil_gps.vd = static_cast<int16_t>(velocity.z() * 100);
+            hil_gps.eph = static_cast<uint16_t>(eph * 100);
+            hil_gps.epv = static_cast<uint16_t>(epv * 100);
+            hil_gps.fix_type = static_cast<uint8_t>(fix_type);
+            hil_gps.vel = static_cast<uint16_t>(velocity_xy * 100);
+            hil_gps.cog = static_cast<uint16_t>(cog * 100);
+            hil_gps.satellites_visible = static_cast<uint8_t>(15);
 
-        if (!is_simulation_mode_)
-            throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
+            if (hil_node_ != nullptr) {
+                hil_node_->sendMessage(hil_gps);
+            }
 
-        mavlinkcom::MavLinkHilGps hil_gps;
-        hil_gps.time_usec = getSimTime();
-        hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
-        hil_gps.lon = static_cast<int32_t>(geo_point.longitude * 1E7);
-        hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);
-        hil_gps.vn = static_cast<int16_t>(velocity.x() * 100);
-        hil_gps.ve = static_cast<int16_t>(velocity.y() * 100);
-        hil_gps.vd = static_cast<int16_t>(velocity.z() * 100);
-        hil_gps.eph = static_cast<uint16_t>(eph * 100);
-        hil_gps.epv = static_cast<uint16_t>(epv * 100);
-        hil_gps.fix_type = static_cast<uint8_t>(fix_type);
-        hil_gps.vel = static_cast<uint16_t>(velocity_xy * 100);
-        hil_gps.cog = static_cast<uint16_t>(cog * 100);
-        hil_gps.satellites_visible = static_cast<uint8_t>(15);
+            if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) {
+                //Utils::DebugBreak();
+                Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError);
+            }
 
-        if (hil_node_ != nullptr) {
-            hil_node_->sendMessage(hil_gps);
+            std::lock_guard<std::mutex> guard(last_message_mutex_);
+            last_gps_message_ = hil_gps;
         }
 
-        if (hil_gps.lat < 0.1f && hil_gps.lat > -0.1f) {
-            //Utils::DebugBreak();
-            Utils::log("hil_gps.lat was too close to 0", Utils::kLogLevelError);
+        void resetState()
+        {
+            //reset state
+            is_hil_mode_set_ = false;
+            is_controls_0_1_ = true;
+            hil_state_freq_ = -1;
+            actuators_message_supported_ = false;
+            state_version_ = 0;
+            current_state_ = mavlinkcom::VehicleState();
+            target_height_ = 0;
+            got_first_heartbeat_ = false;
+            is_armed_ = false;
+            has_home_ = false;
+            sim_time_us_ = 0;
+            last_sys_time_ = 0;
+            last_gps_time_ = 0;
+            last_update_time_ = 0;
+            last_hil_sensor_time_ = 0;
+            update_count_ = 0;
+            hil_sensor_count_ = 0;
+            lock_step_resets_ = 0;
+            actuator_delay_ = 0;
+            is_api_control_enabled_ = false;
+            thrust_controller_ = PidController();
+            Utils::setValue(rotor_controls_, 0.0f);
+            was_reset_ = false;
+            received_actuator_controls_ = false;
+            lock_step_enabled_ = false;
+            has_gps_lock_ = false;
+            send_params_ = false;
+            mocap_pose_ = Pose::nanPose();
+            ground_variance_ = 1;
+            ground_filter_.initialize(25, 0.1f);
+            cancelLastTask();
         }
 
-        std::lock_guard<std::mutex> guard(last_message_mutex_);
-        last_gps_message_ = hil_gps;
-    }
-
-    void resetState()
-    {
-        //reset state
-        is_hil_mode_set_ = false;
-        is_controls_0_1_ = true;
-        hil_state_freq_ = -1;
-        actuators_message_supported_ = false;
-        state_version_ = 0;
-        current_state_ = mavlinkcom::VehicleState();
-        target_height_ = 0;
-        got_first_heartbeat_ = false;
-        is_armed_ = false;
-        has_home_ = false;
-        sim_time_us_ = 0;
-        last_sys_time_ = 0;
-        last_gps_time_ = 0;
-        last_update_time_ = 0;
-        last_hil_sensor_time_ = 0;
-        update_count_ = 0;
-        hil_sensor_count_ = 0;
-        lock_step_resets_ = 0;
-        actuator_delay_ = 0;
-        is_api_control_enabled_ = false;
-        thrust_controller_ = PidController();
-        Utils::setValue(rotor_controls_, 0.0f);
-        was_reset_ = false;
-        received_actuator_controls_ = false;
-        lock_step_enabled_ = false;
-        has_gps_lock_ = false;
-        send_params_ = false;
-        mocap_pose_ = Pose::nanPose();
-        ground_variance_ = 1;
-        ground_filter_.initialize(25, 0.1f);
-        cancelLastTask();
-    }
-
-    void monitorGroundAltitude()
-    {
-        // used to ensure stable altitude before takeoff.
-        auto position = getPosition();
-        auto result = ground_filter_.filter(position.z());
-        auto variance = std::get<1>(result);
-        if (variance >= 0) { // filter returns -1 if we don't have enough data yet.
-            ground_variance_ = variance;
-        }
-    }
-
-
-protected: //variables
-
-    //TODO: below was made protected from private to support Ardupilot
-    //implementation but we need to review this and avoid having protected variables
-    static const int RotorControlsCount = 8;
-
-    const SensorCollection* sensors_;
-    mutable std::mutex hil_controls_mutex_;
-    AirSimSettings::MavLinkConnectionInfo connection_info_;
-    float rotor_controls_[RotorControlsCount];
-    bool is_simulation_mode_;
-
-
-private: //variables
-    static const int pixhawkVendorId = 9900;   ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow
-    static const int pixhawkFMUV4ProductId = 18;     ///< Product ID for Pixhawk V2 board
-    static const int pixhawkFMUV2ProductId = 17;     ///< Product ID for Pixhawk V2 board
-    static const int pixhawkFMUV5ProductId = 50;     ///< Product ID for Pixhawk V5 board
-    static const int pixhawkFMUV2OldBootloaderProductId = 22;     ///< Product ID for Bootloader on older Pixhawk V2 boards
-    static const int pixhawkFMUV1ProductId = 16;     ///< Product ID for PX4 FMU V1 board
-    static const int messageReceivedTimeout = 10; ///< Seconds
-
-    std::shared_ptr<mavlinkcom::MavLinkNode> logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_;
-
-    size_t status_messages_MaxSize = 5000;
-
-    std::shared_ptr<mavlinkcom::MavLinkNode> hil_node_;
-    std::shared_ptr<mavlinkcom::MavLinkConnection> connection_;
-    std::shared_ptr<mavlinkcom::MavLinkVideoServer> video_server_;
-    std::shared_ptr<MultirotorApiBase> mav_vehicle_control_;
-
-    mavlinkcom::MavLinkAttPosMocap MocapPoseMessage;
-    mavlinkcom::MavLinkHeartbeat HeartbeatMessage;
-    mavlinkcom::MavLinkSetMode SetModeMessage;
-    mavlinkcom::MavLinkStatustext StatusTextMessage;
-    mavlinkcom::MavLinkHilControls HilControlsMessage;
-    mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage;
-    mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt;
-    mavlinkcom::MavLinkCommandLong CommandLongMessage;
-    mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed;
-
-    mavlinkcom::MavLinkHilSensor last_sensor_message_;
-    mavlinkcom::MavLinkDistanceSensor last_distance_message_;
-    mavlinkcom::MavLinkHilGps last_gps_message_;
-
-    std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_;
-
-    //variables required for VehicleApiBase implementation
-    bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false;
-    bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1?
-    bool send_params_ = false;
-    std::queue<std::string> status_messages_;
-    int hil_state_freq_;
-    bool actuators_message_supported_ = false;
-    bool was_reset_ = false;
-    bool has_home_ = false;
-    bool is_ready_ = false;
-    bool has_gps_lock_ = false;
-    bool lock_step_enabled_ = false;
-    bool received_actuator_controls_ = false;
-    std::string is_ready_message_;
-    Pose mocap_pose_;
-    std::thread connect_thread_;
-    bool connecting_ = false;
-    bool connected_ = false;
-    common_utils::SmoothingFilter<float> ground_filter_;
-    double ground_variance_ = 1;
-    const double GroundTolerance = 0.1;
-
-    // variables for throttling HIL_SENSOR and SYSTEM_TIME messages
-    uint32_t last_sys_time_ = 0;
-    unsigned long long sim_time_us_ = 0;
-    uint64_t last_gps_time_ = 0;
-    uint64_t last_update_time_ = 0;
-    uint64_t last_hil_sensor_time_ = 0;
-
-    // variables accumulated for MavLinkTelemetry messages.
-    uint64_t update_time_ = 0;
-    uint32_t update_count_ = 0;
-    uint32_t hil_sensor_count_ = 0;
-    uint32_t lock_step_resets_ = 0;
-    uint32_t actuator_delay_ = 0;
-    std::thread telemetry_thread_;
-
-    //additional variables required for MultirotorApiBase implementation
-    //this is optional for methods that might not use vehicle commands
-    std::shared_ptr<mavlinkcom::MavLinkVehicle> mav_vehicle_;
-    float target_height_;
-    bool is_api_control_enabled_;
-    PidController thrust_controller_;
-    common_utils::Timer hil_message_timer_;
-    common_utils::Timer gcs_message_timer_;
-    std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
-    std::string log_file_name_;
-    World* world_;
-
-    //every time we return status update, we need to check if we have new data
-    //this is why below two variables are marked as mutable
-    mutable int state_version_;
-    mutable mavlinkcom::VehicleState current_state_;
-};
+        void monitorGroundAltitude()
+        {
+            // used to ensure stable altitude before takeoff.
+            auto position = getPosition();
+            auto result = ground_filter_.filter(position.z());
+            auto variance = std::get<1>(result);
+            if (variance >= 0) { // filter returns -1 if we don't have enough data yet.
+                ground_variance_ = variance;
+            }
+        }
+
+    protected: //variables
+        //TODO: below was made protected from private to support Ardupilot
+        //implementation but we need to review this and avoid having protected variables
+        static const int RotorControlsCount = 8;
+
+        const SensorCollection* sensors_;
+        mutable std::mutex hil_controls_mutex_;
+        AirSimSettings::MavLinkConnectionInfo connection_info_;
+        float rotor_controls_[RotorControlsCount];
+        bool is_simulation_mode_;
+
+    private: //variables
+        static const int pixhawkVendorId = 9900; ///< Vendor ID for Pixhawk board (V2 and V1) and PX4 Flow
+        static const int pixhawkFMUV4ProductId = 18; ///< Product ID for Pixhawk V2 board
+        static const int pixhawkFMUV2ProductId = 17; ///< Product ID for Pixhawk V2 board
+        static const int pixhawkFMUV5ProductId = 50; ///< Product ID for Pixhawk V5 board
+        static const int pixhawkFMUV2OldBootloaderProductId = 22; ///< Product ID for Bootloader on older Pixhawk V2 boards
+        static const int pixhawkFMUV1ProductId = 16; ///< Product ID for PX4 FMU V1 board
+        static const int messageReceivedTimeout = 10; ///< Seconds
+
+        std::shared_ptr<mavlinkcom::MavLinkNode> logviewer_proxy_, logviewer_out_proxy_, qgc_proxy_;
+
+        size_t status_messages_MaxSize = 5000;
+
+        std::shared_ptr<mavlinkcom::MavLinkNode> hil_node_;
+        std::shared_ptr<mavlinkcom::MavLinkConnection> connection_;
+        std::shared_ptr<mavlinkcom::MavLinkVideoServer> video_server_;
+        std::shared_ptr<MultirotorApiBase> mav_vehicle_control_;
+
+        mavlinkcom::MavLinkAttPosMocap MocapPoseMessage;
+        mavlinkcom::MavLinkHeartbeat HeartbeatMessage;
+        mavlinkcom::MavLinkSetMode SetModeMessage;
+        mavlinkcom::MavLinkStatustext StatusTextMessage;
+        mavlinkcom::MavLinkHilControls HilControlsMessage;
+        mavlinkcom::MavLinkHilActuatorControls HilActuatorControlsMessage;
+        mavlinkcom::MavLinkGpsRawInt MavLinkGpsRawInt;
+        mavlinkcom::MavLinkCommandLong CommandLongMessage;
+        mavlinkcom::MavLinkLocalPositionNed MavLinkLocalPositionNed;
+
+        mavlinkcom::MavLinkHilSensor last_sensor_message_;
+        mavlinkcom::MavLinkDistanceSensor last_distance_message_;
+        mavlinkcom::MavLinkHilGps last_gps_message_;
+
+        std::mutex mocap_pose_mutex_, heartbeat_mutex_, set_mode_mutex_, status_text_mutex_, last_message_mutex_, telemetry_mutex_;
+
+        //variables required for VehicleApiBase implementation
+        bool got_first_heartbeat_ = false, is_hil_mode_set_ = false, is_armed_ = false;
+        bool is_controls_0_1_; //Are motor controls specified in 0..1 or -1..1?
+        bool send_params_ = false;
+        std::queue<std::string> status_messages_;
+        int hil_state_freq_;
+        bool actuators_message_supported_ = false;
+        bool was_reset_ = false;
+        bool has_home_ = false;
+        bool is_ready_ = false;
+        bool has_gps_lock_ = false;
+        bool lock_step_enabled_ = false;
+        bool received_actuator_controls_ = false;
+        std::string is_ready_message_;
+        Pose mocap_pose_;
+        std::thread connect_thread_;
+        bool connecting_ = false;
+        bool connected_ = false;
+        common_utils::SmoothingFilter<float> ground_filter_;
+        double ground_variance_ = 1;
+        const double GroundTolerance = 0.1;
+
+        // variables for throttling HIL_SENSOR and SYSTEM_TIME messages
+        uint32_t last_sys_time_ = 0;
+        unsigned long long sim_time_us_ = 0;
+        uint64_t last_gps_time_ = 0;
+        uint64_t last_update_time_ = 0;
+        uint64_t last_hil_sensor_time_ = 0;
+
+        // variables accumulated for MavLinkTelemetry messages.
+        uint64_t update_time_ = 0;
+        uint32_t update_count_ = 0;
+        uint32_t hil_sensor_count_ = 0;
+        uint32_t lock_step_resets_ = 0;
+        uint32_t actuator_delay_ = 0;
+        std::thread telemetry_thread_;
+
+        //additional variables required for MultirotorApiBase implementation
+        //this is optional for methods that might not use vehicle commands
+        std::shared_ptr<mavlinkcom::MavLinkVehicle> mav_vehicle_;
+        float target_height_;
+        bool is_api_control_enabled_;
+        PidController thrust_controller_;
+        common_utils::Timer hil_message_timer_;
+        common_utils::Timer gcs_message_timer_;
+        std::shared_ptr<mavlinkcom::MavLinkFileLog> log_;
+        std::string log_file_name_;
+        World* world_;
+
+        //every time we return status update, we need to check if we have new data
+        //this is why below two variables are marked as mutable
+        mutable int state_version_;
+        mutable mavlinkcom::VehicleState current_state_;
+    };
 }
 } //namespace
 #endif

From 6c747c79d28c39f088a4e56c97f522b8a52cfe16 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Thu, 29 Apr 2021 23:42:20 -0700
Subject: [PATCH 34/37] cr fixes

---
 MavLinkCom/src/MavLinkMessageBase.cpp |  6 ++--
 PythonClient/multirotor/high.py       | 52 ---------------------------
 PythonClient/multirotor/takeoff.py    | 34 +++++++++++++++++-
 3 files changed, 35 insertions(+), 57 deletions(-)
 delete mode 100644 PythonClient/multirotor/high.py

diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp
index 8abd846a29..0dbbc143c0 100644
--- a/MavLinkCom/src/MavLinkMessageBase.cpp
+++ b/MavLinkCom/src/MavLinkMessageBase.cpp
@@ -53,13 +53,11 @@ int MavLinkMessage::update_checksum()
     }
 
     if (len != msglen) {
+        // mavlink2 supports trimming the payload of trailing zeros so the messages
+        // are variable length as a result.
         if (mavlink1) {
             throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen));
         }
-        else {
-            // mavlink2 supports trimming the payload of trailing zeros so the messages
-            // are variable length as a result.
-        }
     }
     len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen);
     this->len = len;
diff --git a/PythonClient/multirotor/high.py b/PythonClient/multirotor/high.py
deleted file mode 100644
index afdcadff91..0000000000
--- a/PythonClient/multirotor/high.py
+++ /dev/null
@@ -1,52 +0,0 @@
-import setup_path
-import airsim
-
-import sys
-import time
-
-print("""This script is designed to fly on the streets of the Neighborhood environment
-and assumes the unreal position of the drone is [160, -1500, 120].""")
-
-client = airsim.MultirotorClient()
-client.confirmConnection()
-client.enableApiControl(True)
-
-print("arming the drone...")
-client.armDisarm(True)
-
-state = client.getMultirotorState()
-if state.landed_state == airsim.LandedState.Landed:
-    print("taking off...")
-    client.takeoffAsync().join()
-else:
-    client.hoverAsync().join()
-
-time.sleep(1)
-
-state = client.getMultirotorState()
-if state.landed_state == airsim.LandedState.Landed:
-    print("take off failed...")
-    sys.exit(1)
-
-# AirSim uses NED coordinates so negative axis is up.
-# z of -50 is 50 meters above the original launch point.
-z = -50
-print("make sure we are hovering at 50 meters...")
-client.moveToZAsync(z, 1).join()
-
-# see https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo
-
-# this method is async and we are not waiting for the result since we are passing timeout_sec=0.
-
-time.sleep(1)
-
-# come down quickly
-z = -7
-client.moveToZAsync(z, 5).join()
-
-print("landing...")
-client.landAsync().join()
-print("disarming...")
-client.armDisarm(False)
-client.enableApiControl(False)
-print("done.")
diff --git a/PythonClient/multirotor/takeoff.py b/PythonClient/multirotor/takeoff.py
index 035f065442..3d16063a4a 100644
--- a/PythonClient/multirotor/takeoff.py
+++ b/PythonClient/multirotor/takeoff.py
@@ -1,5 +1,15 @@
-import setup_path 
+import setup_path
 import airsim
+import sys
+import time
+
+# For high speed ascent and descent on PX4 you may need to set these properties:
+# param set MPC_Z_VEL_MAX_UP 5
+# param set MPC_Z_VEL_MAX_DN 5
+
+z = 5
+if len(sys.argv) > 1:
+    z = float(sys.argv[1])
 
 client = airsim.MultirotorClient()
 client.confirmConnection()
@@ -14,3 +24,25 @@
 else:
     print("already flying...")
     client.hoverAsync().join()
+
+print("make sure we are hovering at {} meters...".format(z))
+
+if z > 5:
+    # AirSim uses NED coordinates so negative axis is up.
+    # z of -50 is 50 meters above the original launch point.
+    client.moveToZAsync(-z, 5).join()
+    client.hoverAsync().join()
+    time.sleep(5)
+
+if z > 10:
+    print("come down quickly to 10 meters...")
+    z = 10
+    client.moveToZAsync(-z, 3).join()
+    client.hoverAsync().join()
+
+print("landing...")
+client.landAsync().join()
+print("disarming...")
+client.armDisarm(False)
+client.enableApiControl(False)
+print("done.")

From e434f417df5fcf6d84fb49294adb28de97ef2aa0 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Mon, 3 May 2021 13:16:00 -0700
Subject: [PATCH 35/37] Add info on lockstep and steppable clock. Test with
 steppable clocka and fix some bugs.

---
 AirLib/include/common/AirSimSettings.hpp      | 2236 +++++++++--------
 .../mavlink/MavLinkMultirotorApi.hpp          |   34 +-
 docs/px4_lockstep.md                          |   40 +
 docs/px4_setup.md                             |    4 +
 docs/px4_sitl.md                              |    3 +
 docs/px4_sitl_wsl2.md                         |    6 +-
 6 files changed, 1190 insertions(+), 1133 deletions(-)
 create mode 100644 docs/px4_lockstep.md

diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp
index 4d685cbe14..fd9126248d 100644
--- a/AirLib/include/common/AirSimSettings.hpp
+++ b/AirLib/include/common/AirSimSettings.hpp
@@ -4,1311 +4,1313 @@
 #ifndef airsim_core_AirSimSettings_hpp
 #define airsim_core_AirSimSettings_hpp
 
-#include <string>
-#include <vector>
-#include <exception>
-#include <functional>
-#include <map>
-#include "Settings.hpp"
 #include "CommonStructs.hpp"
-#include "common_utils/Utils.hpp"
 #include "ImageCaptureBase.hpp"
+#include "Settings.hpp"
+#include "common_utils/Utils.hpp"
 #include "sensors/SensorBase.hpp"
+#include <exception>
+#include <functional>
+#include <map>
+#include <string>
+#include <vector>
 
-namespace msr { namespace airlib {
-
-struct AirSimSettings 
+namespace msr
+{
+namespace airlib
 {
-private:
-    typedef common_utils::Utils Utils;
-    typedef ImageCaptureBase::ImageType ImageType;
-
-public: //types
-    static constexpr int kSubwindowCount = 3; //must be >= 3 for now
-    static constexpr char const * kVehicleTypePX4 = "px4multirotor";
-    static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo";
-    static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight";
-    static constexpr char const * kVehicleTypeArduCopter = "arducopter";
-    static constexpr char const * kVehicleTypePhysXCar = "physxcar";
-    static constexpr char const * kVehicleTypeArduRover = "ardurover";
-    static constexpr char const * kVehicleTypeComputerVision = "computervision";
-
-    static constexpr char const * kVehicleInertialFrame = "VehicleInertialFrame";
-    static constexpr char const * kSensorLocalFrame = "SensorLocalFrame";
-
-    static constexpr char const * kSimModeTypeMultirotor = "Multirotor";
-    static constexpr char const * kSimModeTypeCar = "Car";
-    static constexpr char const * kSimModeTypeComputerVision = "ComputerVision";
-
-    struct SubwindowSetting 
+
+    struct AirSimSettings
     {
-        int window_index;
-        ImageType image_type;
-        bool visible;
-        std::string camera_name;
-        std::string vehicle_name;
+    private:
+        typedef common_utils::Utils Utils;
+        typedef ImageCaptureBase::ImageType ImageType;
 
-        SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, 
-                bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "")
-            : window_index(window_index_val), image_type(image_type_val), 
-              visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val)
-        {
-        }
-    };
+    public: //types
+        static constexpr int kSubwindowCount = 3; //must be >= 3 for now
+        static constexpr char const* kVehicleTypePX4 = "px4multirotor";
+        static constexpr char const* kVehicleTypeArduCopterSolo = "arducoptersolo";
+        static constexpr char const* kVehicleTypeSimpleFlight = "simpleflight";
+        static constexpr char const* kVehicleTypeArduCopter = "arducopter";
+        static constexpr char const* kVehicleTypePhysXCar = "physxcar";
+        static constexpr char const* kVehicleTypeArduRover = "ardurover";
+        static constexpr char const* kVehicleTypeComputerVision = "computervision";
 
-    struct RecordingSetting 
-    {
-        bool record_on_move = false;
-        float record_interval = 0.05f;
-        std::string folder = "";
-        bool enabled = false;
+        static constexpr char const* kVehicleInertialFrame = "VehicleInertialFrame";
+        static constexpr char const* kSensorLocalFrame = "SensorLocalFrame";
 
-        std::map<std::string, std::vector<ImageCaptureBase::ImageRequest> > requests;
+        static constexpr char const* kSimModeTypeMultirotor = "Multirotor";
+        static constexpr char const* kSimModeTypeCar = "Car";
+        static constexpr char const* kSimModeTypeComputerVision = "ComputerVision";
 
-        RecordingSetting()
+        struct SubwindowSetting
         {
-        }
+            int window_index;
+            ImageType image_type;
+            bool visible;
+            std::string camera_name;
+            std::string vehicle_name;
+
+            SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene,
+                             bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "")
+                : window_index(window_index_val), image_type(image_type_val), visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val)
+            {
+            }
+        };
 
-        RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string& folder_val, bool enabled_val)
-            : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val),
-              enabled(enabled_val)
+        struct RecordingSetting
         {
-        }
-    };
+            bool record_on_move = false;
+            float record_interval = 0.05f;
+            std::string folder = "";
+            bool enabled = false;
 
-    struct PawnPath 
-    {
-        std::string pawn_bp;
-        std::string slippery_mat;
-        std::string non_slippery_mat;
-
-        PawnPath(const std::string& pawn_bp_val = "",
-            const std::string& slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery",
-            const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery")
-            : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val)
-        {
-        }
-    };
+            std::map<std::string, std::vector<ImageCaptureBase::ImageRequest>> requests;
 
-    struct RCSettings 
-    {
-        int remote_control_id = -1;
-        bool allow_api_when_disconnected = false;
-    };
+            RecordingSetting()
+            {
+            }
 
-    struct Rotation
-    {
-        float yaw = 0;
-        float pitch = 0;
-        float roll = 0;
+            RecordingSetting(bool record_on_move_val, float record_interval_val, const std::string& folder_val, bool enabled_val)
+                : record_on_move(record_on_move_val), record_interval(record_interval_val), folder(folder_val), enabled(enabled_val)
+            {
+            }
+        };
 
-        Rotation()
+        struct PawnPath
         {
-        }
+            std::string pawn_bp;
+            std::string slippery_mat;
+            std::string non_slippery_mat;
+
+            PawnPath(const std::string& pawn_bp_val = "",
+                     const std::string& slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/Slippery.Slippery",
+                     const std::string& non_slippery_mat_val = "/AirSim/VehicleAdv/PhysicsMaterials/NonSlippery.NonSlippery")
+                : pawn_bp(pawn_bp_val), slippery_mat(slippery_mat_val), non_slippery_mat(non_slippery_mat_val)
+            {
+            }
+        };
 
-        Rotation(float yaw_val, float pitch_val, float roll_val)
-            : yaw(yaw_val), pitch(pitch_val), roll(roll_val)
+        struct RCSettings
         {
-        }
+            int remote_control_id = -1;
+            bool allow_api_when_disconnected = false;
+        };
 
-        static Rotation nanRotation()
+        struct Rotation
         {
-            static const Rotation val(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
-            return val;
-        }
-    };
+            float yaw = 0;
+            float pitch = 0;
+            float roll = 0;
 
+            Rotation()
+            {
+            }
 
-    struct GimbalSetting 
-    {
-        float stabilization = 0;
-        //bool is_world_frame = false;
-        Rotation rotation = Rotation::nanRotation();
-    };
+            Rotation(float yaw_val, float pitch_val, float roll_val)
+                : yaw(yaw_val), pitch(pitch_val), roll(roll_val)
+            {
+            }
 
-    struct CaptureSetting 
-    {
-        //below settings_json are obtained by using Unreal console command (press ~):
-        // ShowFlag.VisualizeHDR 1.
-        //to replicate camera settings_json to SceneCapture2D
-        //TODO: should we use UAirBlueprintLib::GetDisplayGamma()?
-        static constexpr float kSceneTargetGamma = 1.4f;
-
-        int image_type = 0;
-
-        unsigned int width = 256, height = 144; //960 X 540
-        float fov_degrees = Utils::nan<float>(); //90.0f
-        int auto_exposure_method = -1;   //histogram
-        float auto_exposure_speed = Utils::nan<float>(); // 100.0f;
-        float auto_exposure_bias = Utils::nan<float>(); // 0;
-        float auto_exposure_max_brightness = Utils::nan<float>(); // 0.64f;
-        float auto_exposure_min_brightness = Utils::nan<float>(); // 0.03f;
-        float auto_exposure_low_percent = Utils::nan<float>(); // 80.0f;
-        float auto_exposure_high_percent = Utils::nan<float>(); // 98.3f;
-        float auto_exposure_histogram_log_min = Utils::nan<float>(); // -8;
-        float auto_exposure_histogram_log_max = Utils::nan<float>(); // 4;
-        float motion_blur_amount = Utils::nan<float>();
-        float target_gamma = Utils::nan<float>(); //1.0f; //This would be reset to kSceneTargetGamma for scene as default
-        int projection_mode = 0; // ECameraProjectionMode::Perspective
-        float ortho_width = Utils::nan<float>();
-    };
+            static Rotation nanRotation()
+            {
+                static const Rotation val(Utils::nan<float>(), Utils::nan<float>(), Utils::nan<float>());
+                return val;
+            }
+        };
 
-    struct NoiseSetting 
-    {
-        int ImageType = 0;
+        struct GimbalSetting
+        {
+            float stabilization = 0;
+            //bool is_world_frame = false;
+            Rotation rotation = Rotation::nanRotation();
+        };
 
-        bool Enabled = false;
+        struct CaptureSetting
+        {
+            //below settings_json are obtained by using Unreal console command (press ~):
+            // ShowFlag.VisualizeHDR 1.
+            //to replicate camera settings_json to SceneCapture2D
+            //TODO: should we use UAirBlueprintLib::GetDisplayGamma()?
+            static constexpr float kSceneTargetGamma = 1.4f;
+
+            int image_type = 0;
+
+            unsigned int width = 256, height = 144; //960 X 540
+            float fov_degrees = Utils::nan<float>(); //90.0f
+            int auto_exposure_method = -1; //histogram
+            float auto_exposure_speed = Utils::nan<float>(); // 100.0f;
+            float auto_exposure_bias = Utils::nan<float>(); // 0;
+            float auto_exposure_max_brightness = Utils::nan<float>(); // 0.64f;
+            float auto_exposure_min_brightness = Utils::nan<float>(); // 0.03f;
+            float auto_exposure_low_percent = Utils::nan<float>(); // 80.0f;
+            float auto_exposure_high_percent = Utils::nan<float>(); // 98.3f;
+            float auto_exposure_histogram_log_min = Utils::nan<float>(); // -8;
+            float auto_exposure_histogram_log_max = Utils::nan<float>(); // 4;
+            float motion_blur_amount = Utils::nan<float>();
+            float target_gamma = Utils::nan<float>(); //1.0f; //This would be reset to kSceneTargetGamma for scene as default
+            int projection_mode = 0; // ECameraProjectionMode::Perspective
+            float ortho_width = Utils::nan<float>();
+        };
 
-        float RandContrib = 0.2f;
-        float RandSpeed = 100000.0f;
-        float RandSize = 500.0f;
-        float RandDensity = 2.0f;
+        struct NoiseSetting
+        {
+            int ImageType = 0;
 
-        float HorzWaveContrib = 0.03f;
-        float HorzWaveStrength = 0.08f;
-        float HorzWaveVertSize = 1.0f;
-        float HorzWaveScreenSize = 1.0f;
+            bool Enabled = false;
 
-        float HorzNoiseLinesContrib = 1.0f;
-        float HorzNoiseLinesDensityY = 0.01f;
-        float HorzNoiseLinesDensityXY = 0.5f;
+            float RandContrib = 0.2f;
+            float RandSpeed = 100000.0f;
+            float RandSize = 500.0f;
+            float RandDensity = 2.0f;
 
-        float HorzDistortionContrib = 1.0f;
-        float HorzDistortionStrength = 0.002f;
-    };
+            float HorzWaveContrib = 0.03f;
+            float HorzWaveStrength = 0.08f;
+            float HorzWaveVertSize = 1.0f;
+            float HorzWaveScreenSize = 1.0f;
 
-    struct CameraSetting 
-    {
-        //nan means keep the default values set in components
-        Vector3r position = VectorMath::nanVector();
-        Rotation rotation = Rotation::nanRotation();
+            float HorzNoiseLinesContrib = 1.0f;
+            float HorzNoiseLinesDensityY = 0.01f;
+            float HorzNoiseLinesDensityXY = 0.5f;
 
-        GimbalSetting gimbal;
-        std::map<int, CaptureSetting> capture_settings;
-        std::map<int, NoiseSetting>  noise_settings;
+            float HorzDistortionContrib = 1.0f;
+            float HorzDistortionStrength = 0.002f;
+        };
 
-        CameraSetting()
+        struct CameraSetting
         {
-            initializeCaptureSettings(capture_settings);
-            initializeNoiseSettings(noise_settings);
-        }
-    };
+            //nan means keep the default values set in components
+            Vector3r position = VectorMath::nanVector();
+            Rotation rotation = Rotation::nanRotation();
+
+            GimbalSetting gimbal;
+            std::map<int, CaptureSetting> capture_settings;
+            std::map<int, NoiseSetting> noise_settings;
+
+            CameraSetting()
+            {
+                initializeCaptureSettings(capture_settings);
+                initializeNoiseSettings(noise_settings);
+            }
+        };
 
-    struct CameraDirectorSetting 
-    {
-        Vector3r position = VectorMath::nanVector();
-        Rotation rotation = Rotation::nanRotation();
-        float follow_distance = Utils::nan<float>();
-    };
+        struct CameraDirectorSetting
+        {
+            Vector3r position = VectorMath::nanVector();
+            Rotation rotation = Rotation::nanRotation();
+            float follow_distance = Utils::nan<float>();
+        };
 
-    struct SensorSetting 
-    {
-        SensorBase::SensorType sensor_type;
-        std::string sensor_name;
-        bool enabled = true;
-        Settings settings; // imported json data that needs to be parsed by specific sensors.
-    };
+        struct SensorSetting
+        {
+            SensorBase::SensorType sensor_type;
+            std::string sensor_name;
+            bool enabled = true;
+            Settings settings; // imported json data that needs to be parsed by specific sensors.
+        };
 
-    struct BarometerSetting : SensorSetting 
-    {
-    };
+        struct BarometerSetting : SensorSetting
+        {
+        };
 
-    struct ImuSetting : SensorSetting 
-    {
-    };
+        struct ImuSetting : SensorSetting
+        {
+        };
 
-    struct GpsSetting : SensorSetting 
-    {
-    };
+        struct GpsSetting : SensorSetting
+        {
+        };
 
-    struct MagnetometerSetting : SensorSetting 
-    {
-    };
+        struct MagnetometerSetting : SensorSetting
+        {
+        };
 
-    struct DistanceSetting : SensorSetting 
-    {
-    };
+        struct DistanceSetting : SensorSetting
+        {
+        };
 
-    struct LidarSetting : SensorSetting 
-    {
-    };
+        struct LidarSetting : SensorSetting
+        {
+        };
 
-    struct VehicleSetting 
-    {
-        //required
-        std::string vehicle_name;
-        std::string vehicle_type;
-
-        //optional
-        std::string default_vehicle_state;
-        std::string pawn_path;
-        bool allow_api_always = true;
-        bool auto_create = true;
-        bool enable_collision_passthrough = false;
-        bool enable_trace = false;
-        bool enable_collisions = true;
-        bool is_fpv_vehicle = false;
-
-        //nan means use player start
-        Vector3r position = VectorMath::nanVector(); //in global NED
-        Rotation rotation = Rotation::nanRotation();
-
-        std::map<std::string, CameraSetting> cameras;
-        std::map<std::string, std::shared_ptr<SensorSetting>> sensors;
-
-        RCSettings rc;
-    };
+        struct VehicleSetting
+        {
+            //required
+            std::string vehicle_name;
+            std::string vehicle_type;
+
+            //optional
+            std::string default_vehicle_state;
+            std::string pawn_path;
+            bool allow_api_always = true;
+            bool auto_create = true;
+            bool enable_collision_passthrough = false;
+            bool enable_trace = false;
+            bool enable_collisions = true;
+            bool is_fpv_vehicle = false;
+
+            //nan means use player start
+            Vector3r position = VectorMath::nanVector(); //in global NED
+            Rotation rotation = Rotation::nanRotation();
+
+            std::map<std::string, CameraSetting> cameras;
+            std::map<std::string, std::shared_ptr<SensorSetting>> sensors;
+
+            RCSettings rc;
+        };
 
-    struct MavLinkConnectionInfo 
-    {
-        /* Default values are requires so uninitialized instance doesn't have random values */
-
-        bool use_serial = true; // false means use UDP or TCP instead
-
-        //Used to connect via HITL: needed only if use_serial = true
-        std::string serial_port = "*";
-        int baud_rate = 115200;
-
-        // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false
-        std::string udp_address = "127.0.0.1";
-        int udp_port = 14560;
-
-        // Used to accept connections from drone over TCP: needed only if use_tcp = true
-        bool use_tcp = false;
-        int tcp_port = 4560;
-
-        // The PX4 SITL app requires receiving drone commands over a different mavlink channel called
-        // the "ground control station" (or GCS) channel.
-        std::string control_ip_address = "127.0.0.1";
-        int control_port_local = 14540;
-        int control_port_remote = 14580;
-
-        // The log viewer can be on a different machine, so you can configure it's ip address and port here.
-        int logviewer_ip_port = 14388;
-        int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle.
-        std::string logviewer_ip_address = "";
-
-        // The QGroundControl app can be on a different machine, and AirSim can act as a proxy to forward
-        // the mavlink stream over to that machine if you configure it's ip address and port here.
-        int qgc_ip_port = 0;
-        std::string qgc_ip_address = "";
-
-        // mavlink vehicle identifiers
-        uint8_t sim_sysid = 142;
-        int sim_compid = 42;
-        uint8_t offboard_sysid = 134;
-        int offboard_compid = 1;
-        uint8_t vehicle_sysid = 135;
-        int vehicle_compid = 1;
-
-        // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet)
-        // then you will want to change the LocalHostIp accordingly.  This default only works when log viewer and QGC are also on the
-        // same machine.  Whatever network you choose it has to be the same one for external
-        std::string local_host_ip = "127.0.0.1";
-
-        std::string model = "Generic";
-
-        std::map<std::string, float> params;
-        std::string logs;
-    };
+        struct MavLinkConnectionInfo
+        {
+            /* Default values are requires so uninitialized instance doesn't have random values */
+
+            bool use_serial = true; // false means use UDP or TCP instead
+
+            //Used to connect via HITL: needed only if use_serial = true
+            std::string serial_port = "*";
+            int baud_rate = 115200;
+
+            // Used to connect to drone over UDP: needed only if use_serial = false and use_tcp == false
+            std::string udp_address = "127.0.0.1";
+            int udp_port = 14560;
+
+            // Used to accept connections from drone over TCP: needed only if use_tcp = true
+            bool lock_step = true;
+            bool use_tcp = false;
+            int tcp_port = 4560;
+
+            // The PX4 SITL app requires receiving drone commands over a different mavlink channel called
+            // the "ground control station" (or GCS) channel.
+            std::string control_ip_address = "127.0.0.1";
+            int control_port_local = 14540;
+            int control_port_remote = 14580;
+
+            // The log viewer can be on a different machine, so you can configure it's ip address and port here.
+            int logviewer_ip_port = 14388;
+            int logviewer_ip_sport = 14389; // for logging all messages we send to the vehicle.
+            std::string logviewer_ip_address = "";
+
+            // The QGroundControl app can be on a different machine, and AirSim can act as a proxy to forward
+            // the mavlink stream over to that machine if you configure it's ip address and port here.
+            int qgc_ip_port = 0;
+            std::string qgc_ip_address = "";
+
+            // mavlink vehicle identifiers
+            uint8_t sim_sysid = 142;
+            int sim_compid = 42;
+            uint8_t offboard_sysid = 134;
+            int offboard_compid = 1;
+            uint8_t vehicle_sysid = 135;
+            int vehicle_compid = 1;
+
+            // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet)
+            // then you will want to change the LocalHostIp accordingly.  This default only works when log viewer and QGC are also on the
+            // same machine.  Whatever network you choose it has to be the same one for external
+            std::string local_host_ip = "127.0.0.1";
+
+            std::string model = "Generic";
+
+            std::map<std::string, float> params;
+            std::string logs;
+        };
 
-    struct MavLinkVehicleSetting : public VehicleSetting 
-    {
-        MavLinkConnectionInfo connection_info;
-    };
+        struct MavLinkVehicleSetting : public VehicleSetting
+        {
+            MavLinkConnectionInfo connection_info;
+        };
 
-    struct SegmentationSetting 
-    {
-        enum class InitMethodType {
-            None, CommonObjectsRandomIDs
+        struct SegmentationSetting
+        {
+            enum class InitMethodType
+            {
+                None,
+                CommonObjectsRandomIDs
+            };
+
+            enum class MeshNamingMethodType
+            {
+                OwnerName,
+                StaticMeshName
+            };
+
+            InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs;
+            bool override_existing = false;
+            MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName;
         };
 
-        enum class MeshNamingMethodType {
-            OwnerName, StaticMeshName
+        struct TimeOfDaySetting
+        {
+            bool enabled = false;
+            std::string start_datetime = ""; //format: %Y-%m-%d %H:%M:%S
+            bool is_start_datetime_dst = false;
+            float celestial_clock_speed = 1;
+            float update_interval_secs = 60;
+            bool move_sun = true;
         };
 
-        InitMethodType init_method = InitMethodType::CommonObjectsRandomIDs;
-        bool override_existing = false;
-        MeshNamingMethodType mesh_naming_method = MeshNamingMethodType::OwnerName;
-    };
+    private: //fields
+        float settings_version_actual;
+        float settings_version_minimum = 1.2f;
+
+    public: //fields
+        std::string simmode_name = "";
+        std::string level_name = "";
+
+        std::vector<SubwindowSetting> subwindow_settings;
+        RecordingSetting recording_setting;
+        SegmentationSetting segmentation_setting;
+        TimeOfDaySetting tod_setting;
+
+        std::vector<std::string> warning_messages;
+        std::vector<std::string> error_messages;
+
+        bool is_record_ui_visible = false;
+        int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME
+        bool enable_rpc = true;
+        std::string api_server_address = "";
+        int api_port = RpcLibPort;
+        std::string physics_engine_name = "";
+
+        std::string clock_type = "";
+        float clock_speed = 1.0f;
+        bool engine_sound = false;
+        bool log_messages_visible = true;
+        HomeGeoPoint origin_geopoint{ GeoPoint(47.641468, -122.140165, 122) }; //The geo-coordinate assigned to Unreal coordinate 0,0,0
+        std::map<std::string, PawnPath> pawn_paths; //path for pawn blueprint
+        std::map<std::string, std::unique_ptr<VehicleSetting>> vehicles;
+        CameraSetting camera_defaults;
+        CameraDirectorSetting camera_director;
+        float speed_unit_factor = 1.0f;
+        std::string speed_unit_label = "m\\s";
+        std::map<std::string, std::shared_ptr<SensorSetting>> sensor_defaults;
+        Vector3r wind = Vector3r::Zero();
+
+        std::string settings_text_ = "";
+
+    public: //methods
+        static AirSimSettings& singleton()
+        {
+            static AirSimSettings instance;
+            return instance;
+        }
 
-    struct TimeOfDaySetting 
-    {
-        bool enabled = false;
-        std::string start_datetime = "";    //format: %Y-%m-%d %H:%M:%S
-        bool is_start_datetime_dst = false;
-        float celestial_clock_speed = 1;
-        float update_interval_secs = 60;
-        bool move_sun = true;
-    };
+        AirSimSettings()
+        {
+            initializeSubwindowSettings(subwindow_settings);
+            initializePawnPaths(pawn_paths);
+        }
 
-private: //fields
-    float settings_version_actual;
-    float settings_version_minimum = 1.2f;
-
-public: //fields
-    std::string simmode_name = "";
-    std::string level_name = "";
-
-    std::vector<SubwindowSetting> subwindow_settings;
-    RecordingSetting recording_setting;
-    SegmentationSetting segmentation_setting;
-    TimeOfDaySetting tod_setting;
-
-    std::vector<std::string> warning_messages;
-    std::vector<std::string> error_messages;
-
-    bool is_record_ui_visible = false;
-    int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME
-    bool enable_rpc = true;
-    std::string api_server_address = "";
-    int api_port = RpcLibPort;
-    std::string physics_engine_name = "";
-
-    std::string clock_type = "";
-    float clock_speed = 1.0f;
-    bool engine_sound = false;
-    bool log_messages_visible = true;
-    HomeGeoPoint origin_geopoint{ GeoPoint(47.641468, -122.140165, 122) }; //The geo-coordinate assigned to Unreal coordinate 0,0,0
-    std::map<std::string, PawnPath> pawn_paths; //path for pawn blueprint
-    std::map<std::string, std::unique_ptr<VehicleSetting>> vehicles;
-    CameraSetting camera_defaults;
-    CameraDirectorSetting camera_director;
-    float speed_unit_factor =  1.0f;
-    std::string speed_unit_label = "m\\s";
-    std::map<std::string, std::shared_ptr<SensorSetting>> sensor_defaults;
-    Vector3r wind = Vector3r::Zero();
-
-    std::string settings_text_ = "";
-
-public: //methods
-    static AirSimSettings& singleton()
-    {
-        static AirSimSettings instance;
-        return instance;
-    }
+        //returns number of warnings
+        void load(std::function<std::string(void)> simmode_getter)
+        {
+            warning_messages.clear();
+            error_messages.clear();
+            const Settings& settings_json = Settings::singleton();
+            checkSettingsVersion(settings_json);
+
+            loadCoreSimModeSettings(settings_json, simmode_getter);
+            loadLevelSettings(settings_json);
+            loadDefaultCameraSetting(settings_json, camera_defaults);
+            loadCameraDirectorSetting(settings_json, camera_director, simmode_name);
+            loadSubWindowsSettings(settings_json, subwindow_settings);
+            loadViewModeSettings(settings_json);
+            loadSegmentationSetting(settings_json, segmentation_setting);
+            loadPawnPaths(settings_json, pawn_paths);
+            loadOtherSettings(settings_json);
+            loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults);
+            loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults);
+
+            //this should be done last because it depends on vehicles (and/or their type) we have
+            loadRecordingSetting(settings_json);
+            loadClockSettings(settings_json);
+        }
 
-    AirSimSettings()
-    {
-        initializeSubwindowSettings(subwindow_settings);
-        initializePawnPaths(pawn_paths);
-    }
+        static void initializeSettings(const std::string& json_settings_text)
+        {
+            singleton().settings_text_ = json_settings_text;
+            Settings& settings_json = Settings::loadJSonString(json_settings_text);
+            if (!settings_json.isLoadSuccess())
+                throw std::invalid_argument("Cannot parse JSON settings_json string.");
+        }
 
-    //returns number of warnings
-    void load(std::function<std::string(void)> simmode_getter)
-    {
-        warning_messages.clear();
-        error_messages.clear();
-        const Settings& settings_json = Settings::singleton();
-        checkSettingsVersion(settings_json);
-
-        loadCoreSimModeSettings(settings_json, simmode_getter);
-        loadLevelSettings(settings_json);
-        loadDefaultCameraSetting(settings_json, camera_defaults);
-        loadCameraDirectorSetting(settings_json, camera_director, simmode_name);
-        loadSubWindowsSettings(settings_json, subwindow_settings);
-        loadViewModeSettings(settings_json);
-        loadSegmentationSetting(settings_json, segmentation_setting);
-        loadPawnPaths(settings_json, pawn_paths);
-        loadOtherSettings(settings_json);
-        loadDefaultSensorSettings(simmode_name, settings_json, sensor_defaults);
-        loadVehicleSettings(simmode_name, settings_json, vehicles, sensor_defaults);
-
-        //this should be done last because it depends on vehicles (and/or their type) we have
-        loadRecordingSetting(settings_json);
-        loadClockSettings(settings_json);
-    }
-
-    static void initializeSettings(const std::string& json_settings_text)
-    {
-        singleton().settings_text_ = json_settings_text;
-        Settings& settings_json = Settings::loadJSonString(json_settings_text);
-        if (! settings_json.isLoadSuccess())
-            throw std::invalid_argument("Cannot parse JSON settings_json string.");
-    }
+        static void createDefaultSettingsFile()
+        {
+            initializeSettings("{}");
 
-    static void createDefaultSettingsFile()
-    {
-        initializeSettings("{}");
+            Settings& settings_json = Settings::singleton();
+            //write some settings_json in new file otherwise the string "null" is written if all settings_json are empty
+            settings_json.setString("SeeDocsAt", "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md");
+            settings_json.setDouble("SettingsVersion", 1.2);
 
-        Settings& settings_json = Settings::singleton();
-        //write some settings_json in new file otherwise the string "null" is written if all settings_json are empty
-        settings_json.setString("SeeDocsAt", "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md");
-        settings_json.setDouble("SettingsVersion", 1.2);
+            std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json");
+            //TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only support Unreal 4.17
+            //https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html
+            settings_json.saveJSonFile(settings_filename);
+        }
 
-        std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json");
-        //TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only support Unreal 4.17
-        //https://answers.unrealengine.com/questions/664905/unreal-crashes-on-two-lines-of-extremely-simple-st.html
-        settings_json.saveJSonFile(settings_filename);
-    }
+        // This is for the case when a new vehicle is made on the fly, at runtime
+        void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path = "")
+        {
+            auto vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
 
-    // This is for the case when a new vehicle is made on the fly, at runtime
-    void addVehicleSetting(const std::string& vehicle_name, const std::string& vehicle_type, const Pose& pose, const std::string& pawn_path="")
-    {
-        auto vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+            vehicle_setting->vehicle_name = vehicle_name;
+            vehicle_setting->vehicle_type = vehicle_type;
+            vehicle_setting->position = pose.position;
+            vehicle_setting->pawn_path = pawn_path;
 
-        vehicle_setting->vehicle_name = vehicle_name;
-        vehicle_setting->vehicle_type = vehicle_type;
-        vehicle_setting->position = pose.position;
-        vehicle_setting->pawn_path = pawn_path;
+            VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw);
 
-        VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, 
-                                    vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw);
+            vehicles[vehicle_name] = std::move(vehicle_setting);
+        }
 
-        vehicles[vehicle_name] = std::move(vehicle_setting);
-    }
+        const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const
+        {
+            auto it = vehicles.find(vehicle_name);
+            if (it == vehicles.end())
+                // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight'
+                it = vehicles.find("SimpleFlight");
+            return it->second.get();
+        }
 
-    const VehicleSetting* getVehicleSetting(const std::string& vehicle_name) const
-    {
-        auto it = vehicles.find(vehicle_name);
-        if (it == vehicles.end())
-            // pre-existing flying pawns in Unreal Engine don't have name 'SimpleFlight'
-            it = vehicles.find("SimpleFlight");
-        return it->second.get();
-    }
-
-    static Vector3r createVectorSetting(const Settings& settings_json, const Vector3r& default_vec)
-    {
-        return Vector3r(settings_json.getFloat("X", default_vec.x()),
-            settings_json.getFloat("Y", default_vec.y()),
-            settings_json.getFloat("Z", default_vec.z()));
-    }
-    static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
-    {
-        return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
-            settings_json.getFloat("Pitch", default_rot.pitch),
-            settings_json.getFloat("Roll", default_rot.roll));
-    }
+        static Vector3r createVectorSetting(const Settings& settings_json, const Vector3r& default_vec)
+        {
+            return Vector3r(settings_json.getFloat("X", default_vec.x()),
+                            settings_json.getFloat("Y", default_vec.y()),
+                            settings_json.getFloat("Z", default_vec.z()));
+        }
+        static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
+        {
+            return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
+                            settings_json.getFloat("Pitch", default_rot.pitch),
+                            settings_json.getFloat("Roll", default_rot.roll));
+        }
 
-private:
-    void checkSettingsVersion(const Settings& settings_json)
-    {
-        bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual);
-        bool upgrade_required = settings_version_actual < settings_version_minimum;
-        if (upgrade_required) {
-            bool auto_upgrade = false;
-
-            //if we have default setting file not modified by user then we will
-            //just auto-upgrade it
-            if (has_default_settings) {
-                auto_upgrade = true;
-            }
-            else {
-                //check if auto-upgrade is possible
-                if (settings_version_actual == 1) {
-                    const std::vector<std::string> all_changed_keys = {
-                        "AdditionalCameras", "CaptureSettings", "NoiseSettings",
-                        "UsageScenario", "SimpleFlight", "PX4"
-                    };
-                    std::stringstream detected_keys_ss;
-                    for (const auto& changed_key : all_changed_keys) {
-                        if (settings_json.hasKey(changed_key))
-                            detected_keys_ss << changed_key << ",";
-                    }
-                    std::string detected_keys = detected_keys_ss.str();
-                    if (detected_keys.length()) {
-                        std::string error_message =
-                            "You are using newer version of AirSim with older version of settings.json. "
-                            "You can either delete your settings.json and restart AirSim or use the upgrade "
-                            "instructions at https://git.io/vjefh. \n\n"
-                            "Following keys in your settings.json needs updating: "
-                            ;
-
-                        error_messages.push_back(error_message + detected_keys);
+    private:
+        void checkSettingsVersion(const Settings& settings_json)
+        {
+            bool has_default_settings = hasDefaultSettings(settings_json, settings_version_actual);
+            bool upgrade_required = settings_version_actual < settings_version_minimum;
+            if (upgrade_required) {
+                bool auto_upgrade = false;
+
+                //if we have default setting file not modified by user then we will
+                //just auto-upgrade it
+                if (has_default_settings) {
+                    auto_upgrade = true;
+                }
+                else {
+                    //check if auto-upgrade is possible
+                    if (settings_version_actual == 1) {
+                        const std::vector<std::string> all_changed_keys = {
+                            "AdditionalCameras", "CaptureSettings", "NoiseSettings", "UsageScenario", "SimpleFlight", "PX4"
+                        };
+                        std::stringstream detected_keys_ss;
+                        for (const auto& changed_key : all_changed_keys) {
+                            if (settings_json.hasKey(changed_key))
+                                detected_keys_ss << changed_key << ",";
+                        }
+                        std::string detected_keys = detected_keys_ss.str();
+                        if (detected_keys.length()) {
+                            std::string error_message =
+                                "You are using newer version of AirSim with older version of settings.json. "
+                                "You can either delete your settings.json and restart AirSim or use the upgrade "
+                                "instructions at https://git.io/vjefh. \n\n"
+                                "Following keys in your settings.json needs updating: ";
+
+                            error_messages.push_back(error_message + detected_keys);
+                        }
+                        else
+                            auto_upgrade = true;
                     }
                     else
                         auto_upgrade = true;
                 }
-                else
-                    auto_upgrade = true;
-            }
 
-            if (auto_upgrade) {
-                warning_messages.push_back(
-                    "You are using newer version of AirSim with older version of settings.json. "
-                    "You should delete your settings.json file and restart AirSim.");
+                if (auto_upgrade) {
+                    warning_messages.push_back(
+                        "You are using newer version of AirSim with older version of settings.json. "
+                        "You should delete your settings.json file and restart AirSim.");
+                }
             }
+            //else no action necessary
         }
-        //else no action necessary
-    }
 
-    bool hasDefaultSettings(const Settings& settings_json, float& version)
-    {
-        //if empty settings file
-        bool has_default = settings_json.size() == 0;
+        bool hasDefaultSettings(const Settings& settings_json, float& version)
+        {
+            //if empty settings file
+            bool has_default = settings_json.size() == 0;
 
-        bool has_docs = settings_json.getString("SeeDocsAt", "") != ""
-            || settings_json.getString("see_docs_at", "") != "";
-        //we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :(
-        version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0));
+            bool has_docs = settings_json.getString("SeeDocsAt", "") != "" || settings_json.getString("see_docs_at", "") != "";
+            //we had spelling mistake so we are currently supporting SettingsVersion or SettingdVersion :(
+            version = settings_json.getFloat("SettingsVersion", settings_json.getFloat("SettingdVersion", 0));
 
-        //If we have pre-V1 settings and only element is docs link
-        has_default |= settings_json.size() == 1 && has_docs;
+            //If we have pre-V1 settings and only element is docs link
+            has_default |= settings_json.size() == 1 && has_docs;
 
-        //if we have V1 settings and only elements are docs link and version
-        has_default |= settings_json.size() == 2 && has_docs && version > 0;
+            //if we have V1 settings and only elements are docs link and version
+            has_default |= settings_json.size() == 2 && has_docs && version > 0;
 
-        return has_default;
-    }
+            return has_default;
+        }
 
-    void loadCoreSimModeSettings(const Settings& settings_json, std::function<std::string(void)> simmode_getter)
-    {
-        //get the simmode from user if not specified
-        simmode_name = settings_json.getString("SimMode", "");
-        if (simmode_name == "") {
-            if (simmode_getter)
-                simmode_name = simmode_getter();
-            else
-                throw std::invalid_argument("simmode_name is not expected empty in SimModeBase");
+        void loadCoreSimModeSettings(const Settings& settings_json, std::function<std::string(void)> simmode_getter)
+        {
+            //get the simmode from user if not specified
+            simmode_name = settings_json.getString("SimMode", "");
+            if (simmode_name == "") {
+                if (simmode_getter)
+                    simmode_name = simmode_getter();
+                else
+                    throw std::invalid_argument("simmode_name is not expected empty in SimModeBase");
+            }
+
+            physics_engine_name = settings_json.getString("PhysicsEngineName", "");
+            if (physics_engine_name == "") {
+                if (simmode_name == kSimModeTypeMultirotor)
+                    physics_engine_name = "FastPhysicsEngine";
+                else
+                    physics_engine_name = "PhysX"; //this value is only informational for now
+            }
         }
 
-        physics_engine_name = settings_json.getString("PhysicsEngineName", "");
-        if (physics_engine_name == "") {
-            if (simmode_name == kSimModeTypeMultirotor)
-                physics_engine_name = "FastPhysicsEngine";
-            else
-                physics_engine_name = "PhysX"; //this value is only informational for now
+        void loadLevelSettings(const Settings& settings_json)
+        {
+            level_name = settings_json.getString("Default Environment", "");
         }
-    }
-    
-    void loadLevelSettings(const Settings& settings_json)
-    {
-        level_name = settings_json.getString("Default Environment", "");
-    }
 
-    void loadViewModeSettings(const Settings& settings_json)
-    {
-        std::string view_mode_string = settings_json.getString("ViewMode", "");
+        void loadViewModeSettings(const Settings& settings_json)
+        {
+            std::string view_mode_string = settings_json.getString("ViewMode", "");
+
+            if (view_mode_string == "") {
+                if (simmode_name == kSimModeTypeMultirotor)
+                    view_mode_string = "FlyWithMe";
+                else if (simmode_name == kSimModeTypeComputerVision)
+                    view_mode_string = "Fpv";
+                else
+                    view_mode_string = "SpringArmChase";
+            }
 
-        if (view_mode_string == "") {
-            if (simmode_name == kSimModeTypeMultirotor)
-                view_mode_string = "FlyWithMe";
-            else if (simmode_name == kSimModeTypeComputerVision)
-                view_mode_string = "Fpv";
+            if (view_mode_string == "Fpv")
+                initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV;
+            else if (view_mode_string == "GroundObserver")
+                initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER;
+            else if (view_mode_string == "FlyWithMe")
+                initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME;
+            else if (view_mode_string == "Manual")
+                initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL;
+            else if (view_mode_string == "SpringArmChase")
+                initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE;
+            else if (view_mode_string == "Backup")
+                initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP;
+            else if (view_mode_string == "NoDisplay")
+                initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY;
+            else if (view_mode_string == "Front")
+                initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT;
             else
-                view_mode_string = "SpringArmChase";
+                error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string);
         }
 
-        if (view_mode_string == "Fpv")
-            initial_view_mode = 0; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FPV;
-        else if (view_mode_string == "GroundObserver")
-            initial_view_mode = 1; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_GROUND_OBSERVER;
-        else if (view_mode_string == "FlyWithMe")
-            initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME;
-        else if (view_mode_string == "Manual")
-            initial_view_mode = 3; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_MANUAL;
-        else if (view_mode_string == "SpringArmChase")
-            initial_view_mode = 4; // ECameraDirectorMode::CAMERA_DIRECTOR_MODE_SPRINGARM_CHASE;
-        else if (view_mode_string == "Backup")
-            initial_view_mode = 5; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_BACKUP;
-        else if (view_mode_string == "NoDisplay")
-            initial_view_mode = 6; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_NODISPLAY;
-        else if (view_mode_string == "Front")
-            initial_view_mode = 7; // ECameraDirectorMode::CAMREA_DIRECTOR_MODE_FRONT;
-        else
-            error_messages.push_back("ViewMode setting is not recognized: " + view_mode_string);
-    }
-
-    static void loadRCSetting(const std::string& simmode_name, const Settings& settings_json, RCSettings& rc_setting)
-    {
-        Settings rc_json;
-        if (settings_json.getChild("RC", rc_json)) {
-            rc_setting.remote_control_id = rc_json.getInt("RemoteControlID",
-                simmode_name == kSimModeTypeMultirotor ? 0 : -1);
-            rc_setting.allow_api_when_disconnected = rc_json.getBool("AllowAPIWhenDisconnected",
-                rc_setting.allow_api_when_disconnected);
+        static void loadRCSetting(const std::string& simmode_name, const Settings& settings_json, RCSettings& rc_setting)
+        {
+            Settings rc_json;
+            if (settings_json.getChild("RC", rc_json)) {
+                rc_setting.remote_control_id = rc_json.getInt("RemoteControlID",
+                                                              simmode_name == kSimModeTypeMultirotor ? 0 : -1);
+                rc_setting.allow_api_when_disconnected = rc_json.getBool("AllowAPIWhenDisconnected",
+                                                                         rc_setting.allow_api_when_disconnected);
+            }
         }
-    }
 
-    static std::string getCameraName(const Settings& settings_json)
-    {
-        return settings_json.getString("CameraName",
-            //TODO: below exist only due to legacy reason and can be replaced by "" in future
-            std::to_string(settings_json.getInt("CameraID", 0)));
-    }
+        static std::string getCameraName(const Settings& settings_json)
+        {
+            return settings_json.getString("CameraName",
+                                           //TODO: below exist only due to legacy reason and can be replaced by "" in future
+                                           std::to_string(settings_json.getInt("CameraID", 0)));
+        }
 
-    void loadDefaultRecordingSettings()
-    {
-        recording_setting.requests.clear();
-        // Add Scene image for each vehicle
-        for (const auto& vehicle : vehicles) {
-            recording_setting.requests[vehicle.first].push_back(ImageCaptureBase::ImageRequest(
-                "", ImageType::Scene, false, true));
+        void loadDefaultRecordingSettings()
+        {
+            recording_setting.requests.clear();
+            // Add Scene image for each vehicle
+            for (const auto& vehicle : vehicles) {
+                recording_setting.requests[vehicle.first].push_back(ImageCaptureBase::ImageRequest(
+                    "", ImageType::Scene, false, true));
+            }
         }
-    }
 
-    void loadRecordingSetting(const Settings& settings_json)
-    {
-        loadDefaultRecordingSettings();
-
-        Settings recording_json;
-        if (settings_json.getChild("Recording", recording_json)) {
-            recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move);
-            recording_setting.record_interval = recording_json.getFloat("RecordInterval", recording_setting.record_interval);
-            recording_setting.folder = recording_json.getString("Folder", recording_setting.folder);
-            recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled);
-
-            Settings req_cameras_settings;
-            if (recording_json.getChild("Cameras", req_cameras_settings)) {
-                // If 'Cameras' field is present, clear defaults
-                recording_setting.requests.clear();
-                // Get name of the default vehicle to be used if "VehicleName" isn't specified
-                // Map contains a default vehicle if vehicles haven't been specified
-                std::string default_vehicle_name = vehicles.begin()->first;
-
-                for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) {
-                    Settings req_camera_settings;
-
-                    if (req_cameras_settings.getChild(child_index, req_camera_settings)) {
-                        std::string camera_name = getCameraName(req_camera_settings);
-                        ImageType image_type = Utils::toEnum<ImageType>(
-                                                req_camera_settings.getInt("ImageType", 0));
-                        bool compress = req_camera_settings.getBool("Compress", true);
-                        bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false);
-                        std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name);
-
-                        recording_setting.requests[vehicle_name].push_back(ImageCaptureBase::ImageRequest(
-                            camera_name, image_type, pixels_as_float, compress));
+        void loadRecordingSetting(const Settings& settings_json)
+        {
+            loadDefaultRecordingSettings();
+
+            Settings recording_json;
+            if (settings_json.getChild("Recording", recording_json)) {
+                recording_setting.record_on_move = recording_json.getBool("RecordOnMove", recording_setting.record_on_move);
+                recording_setting.record_interval = recording_json.getFloat("RecordInterval", recording_setting.record_interval);
+                recording_setting.folder = recording_json.getString("Folder", recording_setting.folder);
+                recording_setting.enabled = recording_json.getBool("Enabled", recording_setting.enabled);
+
+                Settings req_cameras_settings;
+                if (recording_json.getChild("Cameras", req_cameras_settings)) {
+                    // If 'Cameras' field is present, clear defaults
+                    recording_setting.requests.clear();
+                    // Get name of the default vehicle to be used if "VehicleName" isn't specified
+                    // Map contains a default vehicle if vehicles haven't been specified
+                    std::string default_vehicle_name = vehicles.begin()->first;
+
+                    for (size_t child_index = 0; child_index < req_cameras_settings.size(); ++child_index) {
+                        Settings req_camera_settings;
+
+                        if (req_cameras_settings.getChild(child_index, req_camera_settings)) {
+                            std::string camera_name = getCameraName(req_camera_settings);
+                            ImageType image_type = Utils::toEnum<ImageType>(
+                                req_camera_settings.getInt("ImageType", 0));
+                            bool compress = req_camera_settings.getBool("Compress", true);
+                            bool pixels_as_float = req_camera_settings.getBool("PixelsAsFloat", false);
+                            std::string vehicle_name = req_camera_settings.getString("VehicleName", default_vehicle_name);
+
+                            recording_setting.requests[vehicle_name].push_back(ImageCaptureBase::ImageRequest(
+                                camera_name, image_type, pixels_as_float, compress));
+                        }
                     }
                 }
             }
         }
-    }
 
-    static void initializeCaptureSettings(std::map<int, CaptureSetting>& capture_settings)
-    {
-        capture_settings.clear();
-        for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) {
-            capture_settings[i] = CaptureSetting();
+        static void initializeCaptureSettings(std::map<int, CaptureSetting>& capture_settings)
+        {
+            capture_settings.clear();
+            for (int i = -1; i < Utils::toNumeric(ImageType::Count); ++i) {
+                capture_settings[i] = CaptureSetting();
+            }
+            capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma;
         }
-        capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma;
-    }
 
-    static void loadCaptureSettings(const Settings& settings_json, std::map<int, CaptureSetting>& capture_settings)
-    {
-        initializeCaptureSettings(capture_settings);
-
-        Settings json_parent;
-        if (settings_json.getChild("CaptureSettings", json_parent)) {
-            for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
-                Settings json_settings_child;
-                if (json_parent.getChild(child_index, json_settings_child)) {
-                    CaptureSetting capture_setting;
-                    createCaptureSettings(json_settings_child, capture_setting);
-                    capture_settings[capture_setting.image_type] = capture_setting;
+        static void loadCaptureSettings(const Settings& settings_json, std::map<int, CaptureSetting>& capture_settings)
+        {
+            initializeCaptureSettings(capture_settings);
+
+            Settings json_parent;
+            if (settings_json.getChild("CaptureSettings", json_parent)) {
+                for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
+                    Settings json_settings_child;
+                    if (json_parent.getChild(child_index, json_settings_child)) {
+                        CaptureSetting capture_setting;
+                        createCaptureSettings(json_settings_child, capture_setting);
+                        capture_settings[capture_setting.image_type] = capture_setting;
+                    }
                 }
             }
         }
-    }
 
-    static std::unique_ptr<VehicleSetting> createMavLinkVehicleSetting(const Settings& settings_json)
-    {
-        //these settings_json are expected in same section, not in another child
-        std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new MavLinkVehicleSetting());
-        MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
+        static std::unique_ptr<VehicleSetting> createMavLinkVehicleSetting(const Settings& settings_json)
+        {
+            //these settings_json are expected in same section, not in another child
+            std::unique_ptr<VehicleSetting> vehicle_setting_p = std::unique_ptr<VehicleSetting>(new MavLinkVehicleSetting());
+            MavLinkVehicleSetting* vehicle_setting = static_cast<MavLinkVehicleSetting*>(vehicle_setting_p.get());
 
-        //TODO: we should be selecting remote if available else keyboard
-        //currently keyboard is not supported so use rc as default
-        vehicle_setting->rc.remote_control_id = 0;
+            //TODO: we should be selecting remote if available else keyboard
+            //currently keyboard is not supported so use rc as default
+            vehicle_setting->rc.remote_control_id = 0;
 
-        MavLinkConnectionInfo &connection_info = vehicle_setting->connection_info;
-        connection_info.sim_sysid = static_cast<uint8_t>(settings_json.getInt("SimSysID", connection_info.sim_sysid));
-        connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid);
+            MavLinkConnectionInfo& connection_info = vehicle_setting->connection_info;
+            connection_info.sim_sysid = static_cast<uint8_t>(settings_json.getInt("SimSysID", connection_info.sim_sysid));
+            connection_info.sim_compid = settings_json.getInt("SimCompID", connection_info.sim_compid);
 
-        connection_info.vehicle_sysid = static_cast<uint8_t>(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid));
-        connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid);
+            connection_info.vehicle_sysid = static_cast<uint8_t>(settings_json.getInt("VehicleSysID", connection_info.vehicle_sysid));
+            connection_info.vehicle_compid = settings_json.getInt("VehicleCompID", connection_info.vehicle_compid);
 
-        connection_info.offboard_sysid = static_cast<uint8_t>(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid));
-        connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid);
+            connection_info.offboard_sysid = static_cast<uint8_t>(settings_json.getInt("OffboardSysID", connection_info.offboard_sysid));
+            connection_info.offboard_compid = settings_json.getInt("OffboardCompID", connection_info.offboard_compid);
 
-        connection_info.logviewer_ip_address = settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address);
-        connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port);
-        connection_info.logviewer_ip_sport = settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport);
+            connection_info.logviewer_ip_address = settings_json.getString("LogViewerHostIp", connection_info.logviewer_ip_address);
+            connection_info.logviewer_ip_port = settings_json.getInt("LogViewerPort", connection_info.logviewer_ip_port);
+            connection_info.logviewer_ip_sport = settings_json.getInt("LogViewerSendPort", connection_info.logviewer_ip_sport);
 
-        connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address);
-        connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port);
+            connection_info.qgc_ip_address = settings_json.getString("QgcHostIp", connection_info.qgc_ip_address);
+            connection_info.qgc_ip_port = settings_json.getInt("QgcPort", connection_info.qgc_ip_port);
 
-        connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address);
-        connection_info.control_port_local = settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy
-        connection_info.control_port_local = settings_json.getInt("ControlPortLocal", connection_info.control_port_local);
-        connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote);
+            connection_info.control_ip_address = settings_json.getString("ControlIp", connection_info.control_ip_address);
+            connection_info.control_port_local = settings_json.getInt("ControlPort", connection_info.control_port_local); // legacy
+            connection_info.control_port_local = settings_json.getInt("ControlPortLocal", connection_info.control_port_local);
+            connection_info.control_port_remote = settings_json.getInt("ControlPortRemote", connection_info.control_port_remote);
 
-        std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address);
-        if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) {
-            // backwards compat
-            connection_info.control_ip_address = sitlip;
-        }
-        if (settings_json.hasKey("SitlPort")) {
-            // backwards compat
-            connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local);
-        }
+            std::string sitlip = settings_json.getString("SitlIp", connection_info.control_ip_address);
+            if (sitlip.size() > 0 && connection_info.control_ip_address.size() == 0) {
+                // backwards compat
+                connection_info.control_ip_address = sitlip;
+            }
+            if (settings_json.hasKey("SitlPort")) {
+                // backwards compat
+                connection_info.control_port_local = settings_json.getInt("SitlPort", connection_info.control_port_local);
+            }
 
-        connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip);
-
-        connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial);
-        connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address);
-        connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port);
-        connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp);
-        connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port);
-        connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port);
-        connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate);
-        connection_info.model = settings_json.getString("Model", connection_info.model);
-        connection_info.logs = settings_json.getString("Logs", connection_info.logs);
-
-        Settings params;
-        if (settings_json.getChild("Parameters", params)) {
-            std::vector<std::string> keys;
-            params.getChildNames(keys);
-            for (auto key: keys) {
-                connection_info.params[key] = params.getFloat(key, 0);
+            connection_info.local_host_ip = settings_json.getString("LocalHostIp", connection_info.local_host_ip);
+
+            connection_info.use_serial = settings_json.getBool("UseSerial", connection_info.use_serial);
+            connection_info.udp_address = settings_json.getString("UdpIp", connection_info.udp_address);
+            connection_info.udp_port = settings_json.getInt("UdpPort", connection_info.udp_port);
+            connection_info.use_tcp = settings_json.getBool("UseTcp", connection_info.use_tcp);
+            connection_info.lock_step = settings_json.getBool("LockStep", connection_info.lock_step);
+            connection_info.tcp_port = settings_json.getInt("TcpPort", connection_info.tcp_port);
+            connection_info.serial_port = settings_json.getString("SerialPort", connection_info.serial_port);
+            connection_info.baud_rate = settings_json.getInt("SerialBaudRate", connection_info.baud_rate);
+            connection_info.model = settings_json.getString("Model", connection_info.model);
+            connection_info.logs = settings_json.getString("Logs", connection_info.logs);
+
+            Settings params;
+            if (settings_json.getChild("Parameters", params)) {
+                std::vector<std::string> keys;
+                params.getChildNames(keys);
+                for (auto key : keys) {
+                    connection_info.params[key] = params.getFloat(key, 0);
+                }
             }
+
+            return vehicle_setting_p;
         }
 
-        return vehicle_setting_p;
-    }
+        static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name, const Settings& settings_json,
+                                                                    const std::string vehicle_name,
+                                                                    std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+        {
+            auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
-    static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name,  const Settings& settings_json,
-        const std::string vehicle_name,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
-    {
-        auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
-
-        std::unique_ptr<VehicleSetting> vehicle_setting;
-        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo 
-            || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover)
-            vehicle_setting = createMavLinkVehicleSetting(settings_json);
-        //for everything else we don't need derived class yet
-        else {
-            vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
-            if (vehicle_type == kVehicleTypeSimpleFlight) {
-                //TODO: we should be selecting remote if available else keyboard
-                //currently keyboard is not supported so use rc as default
-                vehicle_setting->rc.remote_control_id = 0;
+            std::unique_ptr<VehicleSetting> vehicle_setting;
+            if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover)
+                vehicle_setting = createMavLinkVehicleSetting(settings_json);
+            //for everything else we don't need derived class yet
+            else {
+                vehicle_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+                if (vehicle_type == kVehicleTypeSimpleFlight) {
+                    //TODO: we should be selecting remote if available else keyboard
+                    //currently keyboard is not supported so use rc as default
+                    vehicle_setting->rc.remote_control_id = 0;
+                }
             }
+            vehicle_setting->vehicle_name = vehicle_name;
+
+            //required settings_json
+            vehicle_setting->vehicle_type = vehicle_type;
+
+            //optional settings_json
+            vehicle_setting->pawn_path = settings_json.getString("PawnPath", "");
+            vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", "");
+            vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways",
+                                                                      vehicle_setting->allow_api_always);
+            vehicle_setting->auto_create = settings_json.getBool("AutoCreate",
+                                                                 vehicle_setting->auto_create);
+            vehicle_setting->enable_collision_passthrough = settings_json.getBool("EnableCollisionPassthrogh",
+                                                                                  vehicle_setting->enable_collision_passthrough);
+            vehicle_setting->enable_trace = settings_json.getBool("EnableTrace",
+                                                                  vehicle_setting->enable_trace);
+            vehicle_setting->enable_collisions = settings_json.getBool("EnableCollisions",
+                                                                       vehicle_setting->enable_collisions);
+            vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle",
+                                                                    vehicle_setting->is_fpv_vehicle);
+
+            loadRCSetting(simmode_name, settings_json, vehicle_setting->rc);
+
+            vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position);
+            vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation);
+
+            loadCameraSettings(settings_json, vehicle_setting->cameras);
+            loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults);
+
+            return vehicle_setting;
         }
-        vehicle_setting->vehicle_name = vehicle_name;
-
-        //required settings_json
-        vehicle_setting->vehicle_type = vehicle_type;
-
-        //optional settings_json
-        vehicle_setting->pawn_path = settings_json.getString("PawnPath", "");
-        vehicle_setting->default_vehicle_state = settings_json.getString("DefaultVehicleState", "");
-        vehicle_setting->allow_api_always = settings_json.getBool("AllowAPIAlways",
-            vehicle_setting->allow_api_always);
-        vehicle_setting->auto_create = settings_json.getBool("AutoCreate",
-            vehicle_setting->auto_create);
-        vehicle_setting->enable_collision_passthrough = settings_json.getBool("EnableCollisionPassthrogh",
-            vehicle_setting->enable_collision_passthrough);
-        vehicle_setting->enable_trace = settings_json.getBool("EnableTrace",
-            vehicle_setting->enable_trace);
-        vehicle_setting->enable_collisions = settings_json.getBool("EnableCollisions",
-            vehicle_setting->enable_collisions);
-        vehicle_setting->is_fpv_vehicle = settings_json.getBool("IsFpvVehicle",
-            vehicle_setting->is_fpv_vehicle);
-
-        loadRCSetting(simmode_name, settings_json, vehicle_setting->rc);
-
-        vehicle_setting->position = createVectorSetting(settings_json, vehicle_setting->position);
-        vehicle_setting->rotation = createRotationSetting(settings_json, vehicle_setting->rotation);
-
-        loadCameraSettings(settings_json, vehicle_setting->cameras);
-        loadSensorSettings(settings_json, "Sensors", vehicle_setting->sensors, sensor_defaults);
-
-        return vehicle_setting;
-    }
-
-    static void initializeVehicleSettings(const std::string &simmode_name, std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles)
-    {
-        vehicles.clear();
-
-        //NOTE: Do not set defaults for vehicle type here. If you do then make sure
-        //to sync code in createVehicleSetting() as well.
-        if (simmode_name == kSimModeTypeMultirotor) {
-            // create simple flight as default multirotor
-            auto simple_flight_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
-            simple_flight_setting->vehicle_name = "SimpleFlight";
-            simple_flight_setting->vehicle_type = kVehicleTypeSimpleFlight;
-            // TODO: we should be selecting remote if available else keyboard
-            // currently keyboard is not supported so use rc as default
-            simple_flight_setting->rc.remote_control_id = 0;
-            vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting);
-        }
-        else if (simmode_name == kSimModeTypeCar) {
-            // create PhysX as default car vehicle
-            auto physx_car_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
-            physx_car_setting->vehicle_name = "PhysXCar";
-            physx_car_setting->vehicle_type = kVehicleTypePhysXCar;
-            vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting);
-        }
-        else if (simmode_name == kSimModeTypeComputerVision) {
-            // create default computer vision vehicle
-            auto cv_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
-            cv_setting->vehicle_name = "ComputerVision";
-            cv_setting->vehicle_type = kVehicleTypeComputerVision;
-            vehicles[cv_setting->vehicle_name] = std::move(cv_setting);
-        }
-        else {
-            throw std::invalid_argument(Utils::stringf(
-                    "Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str()).c_str());
+
+        static void initializeVehicleSettings(const std::string& simmode_name, std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles)
+        {
+            vehicles.clear();
+
+            //NOTE: Do not set defaults for vehicle type here. If you do then make sure
+            //to sync code in createVehicleSetting() as well.
+            if (simmode_name == kSimModeTypeMultirotor) {
+                // create simple flight as default multirotor
+                auto simple_flight_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+                simple_flight_setting->vehicle_name = "SimpleFlight";
+                simple_flight_setting->vehicle_type = kVehicleTypeSimpleFlight;
+                // TODO: we should be selecting remote if available else keyboard
+                // currently keyboard is not supported so use rc as default
+                simple_flight_setting->rc.remote_control_id = 0;
+                vehicles[simple_flight_setting->vehicle_name] = std::move(simple_flight_setting);
+            }
+            else if (simmode_name == kSimModeTypeCar) {
+                // create PhysX as default car vehicle
+                auto physx_car_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+                physx_car_setting->vehicle_name = "PhysXCar";
+                physx_car_setting->vehicle_type = kVehicleTypePhysXCar;
+                vehicles[physx_car_setting->vehicle_name] = std::move(physx_car_setting);
+            }
+            else if (simmode_name == kSimModeTypeComputerVision) {
+                // create default computer vision vehicle
+                auto cv_setting = std::unique_ptr<VehicleSetting>(new VehicleSetting());
+                cv_setting->vehicle_name = "ComputerVision";
+                cv_setting->vehicle_type = kVehicleTypeComputerVision;
+                vehicles[cv_setting->vehicle_name] = std::move(cv_setting);
+            }
+            else {
+                throw std::invalid_argument(Utils::stringf(
+                                                "Unknown SimMode: %s, failed to set default vehicle settings", simmode_name.c_str())
+                                                .c_str());
+            }
         }
-    }
 
-    static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json,
-        std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
-    {
-        initializeVehicleSettings(simmode_name, vehicles);
+        static void loadVehicleSettings(const std::string& simmode_name, const Settings& settings_json,
+                                        std::map<std::string, std::unique_ptr<VehicleSetting>>& vehicles,
+                                        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+        {
+            initializeVehicleSettings(simmode_name, vehicles);
 
-        msr::airlib::Settings vehicles_child;
-        if (settings_json.getChild("Vehicles", vehicles_child)) {
-            std::vector<std::string> keys;
-            vehicles_child.getChildNames(keys);
+            msr::airlib::Settings vehicles_child;
+            if (settings_json.getChild("Vehicles", vehicles_child)) {
+                std::vector<std::string> keys;
+                vehicles_child.getChildNames(keys);
 
-            //remove default vehicles, if values are specified in settings
-            if (keys.size())
-                vehicles.clear();
+                //remove default vehicles, if values are specified in settings
+                if (keys.size())
+                    vehicles.clear();
 
-            for (const auto& key : keys) {
-                msr::airlib::Settings child;
-                vehicles_child.getChild(key, child);
-                vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults);
+                for (const auto& key : keys) {
+                    msr::airlib::Settings child;
+                    vehicles_child.getChild(key, child);
+                    vehicles[key] = createVehicleSetting(simmode_name, child, key, sensor_defaults);
+                }
             }
         }
-    }
 
-    static void initializePawnPaths(std::map<std::string, PawnPath>& pawn_paths)
-    {
-        pawn_paths.clear();
-        pawn_paths.emplace("BareboneCar",
-            PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'"));
-        pawn_paths.emplace("DefaultCar",
-            PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'"));
-        pawn_paths.emplace("DefaultQuadrotor",
-            PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'"));
-        pawn_paths.emplace("DefaultComputerVision",
-            PawnPath("Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'"));
-
-    }
-
-    static void loadPawnPaths(const Settings& settings_json, std::map<std::string, PawnPath>& pawn_paths)
-    {
-        initializePawnPaths(pawn_paths);
+        static void initializePawnPaths(std::map<std::string, PawnPath>& pawn_paths)
+        {
+            pawn_paths.clear();
+            pawn_paths.emplace("BareboneCar",
+                               PawnPath("Class'/AirSim/VehicleAdv/Vehicle/VehicleAdvPawn.VehicleAdvPawn_C'"));
+            pawn_paths.emplace("DefaultCar",
+                               PawnPath("Class'/AirSim/VehicleAdv/SUV/SuvCarPawn.SuvCarPawn_C'"));
+            pawn_paths.emplace("DefaultQuadrotor",
+                               PawnPath("Class'/AirSim/Blueprints/BP_FlyingPawn.BP_FlyingPawn_C'"));
+            pawn_paths.emplace("DefaultComputerVision",
+                               PawnPath("Class'/AirSim/Blueprints/BP_ComputerVisionPawn.BP_ComputerVisionPawn_C'"));
+        }
 
-        msr::airlib::Settings pawn_paths_child;
-        if (settings_json.getChild("PawnPaths", pawn_paths_child)) {
-            std::vector<std::string> keys;
-            pawn_paths_child.getChildNames(keys);
+        static void loadPawnPaths(const Settings& settings_json, std::map<std::string, PawnPath>& pawn_paths)
+        {
+            initializePawnPaths(pawn_paths);
+
+            msr::airlib::Settings pawn_paths_child;
+            if (settings_json.getChild("PawnPaths", pawn_paths_child)) {
+                std::vector<std::string> keys;
+                pawn_paths_child.getChildNames(keys);
 
-            for (const auto& key : keys) {
-                msr::airlib::Settings child;
-                pawn_paths_child.getChild(key, child);
-                pawn_paths[key] = createPathPawn(child);
+                for (const auto& key : keys) {
+                    msr::airlib::Settings child;
+                    pawn_paths_child.getChild(key, child);
+                    pawn_paths[key] = createPathPawn(child);
+                }
             }
         }
-    }
 
-    static PawnPath createPathPawn(const Settings& settings_json)
-    {
-        auto paths = PawnPath();
-        paths.pawn_bp = settings_json.getString("PawnBP", "");
-        auto slippery_mat = settings_json.getString("SlipperyMat", "");
-        auto non_slippery_mat = settings_json.getString("NonSlipperyMat", "");
+        static PawnPath createPathPawn(const Settings& settings_json)
+        {
+            auto paths = PawnPath();
+            paths.pawn_bp = settings_json.getString("PawnBP", "");
+            auto slippery_mat = settings_json.getString("SlipperyMat", "");
+            auto non_slippery_mat = settings_json.getString("NonSlipperyMat", "");
 
-        if (slippery_mat != "")
-            paths.slippery_mat = slippery_mat;
-        if (non_slippery_mat != "")
-            paths.non_slippery_mat = non_slippery_mat;
+            if (slippery_mat != "")
+                paths.slippery_mat = slippery_mat;
+            if (non_slippery_mat != "")
+                paths.non_slippery_mat = non_slippery_mat;
 
-        return paths;
-    }
+            return paths;
+        }
 
-    static void loadSegmentationSetting(const Settings& settings_json, SegmentationSetting& segmentation_setting)
-    {
-        Settings json_parent;
-        if (settings_json.getChild("SegmentationSettings", json_parent)) {
-            std::string init_method = Utils::toLower(json_parent.getString("InitMethod", ""));
-            if (init_method == "" || init_method == "commonobjectsrandomids")
-                segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs;
-            else if (init_method == "none")
-                segmentation_setting.init_method = SegmentationSetting::InitMethodType::None;
-            else
-                //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine?
-                throw std::invalid_argument(std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method);
+        static void loadSegmentationSetting(const Settings& settings_json, SegmentationSetting& segmentation_setting)
+        {
+            Settings json_parent;
+            if (settings_json.getChild("SegmentationSettings", json_parent)) {
+                std::string init_method = Utils::toLower(json_parent.getString("InitMethod", ""));
+                if (init_method == "" || init_method == "commonobjectsrandomids")
+                    segmentation_setting.init_method = SegmentationSetting::InitMethodType::CommonObjectsRandomIDs;
+                else if (init_method == "none")
+                    segmentation_setting.init_method = SegmentationSetting::InitMethodType::None;
+                else
+                    //TODO: below exception doesn't actually get raised right now because of issue in Unreal Engine?
+                    throw std::invalid_argument(std::string("SegmentationSetting init_method has invalid value in settings_json ") + init_method);
 
-            segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", false);
+                segmentation_setting.override_existing = json_parent.getBool("OverrideExisting", false);
 
-            std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", ""));
-            if (mesh_naming_method == "" || mesh_naming_method == "ownername")
-                segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName;
-            else if (mesh_naming_method == "staticmeshname")
-                segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName;
-            else
-                throw std::invalid_argument(std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + mesh_naming_method);
+                std::string mesh_naming_method = Utils::toLower(json_parent.getString("MeshNamingMethod", ""));
+                if (mesh_naming_method == "" || mesh_naming_method == "ownername")
+                    segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::OwnerName;
+                else if (mesh_naming_method == "staticmeshname")
+                    segmentation_setting.mesh_naming_method = SegmentationSetting::MeshNamingMethodType::StaticMeshName;
+                else
+                    throw std::invalid_argument(std::string("SegmentationSetting MeshNamingMethod has invalid value in settings_json ") + mesh_naming_method);
+            }
         }
-    }
 
-    static void initializeNoiseSettings(std::map<int, NoiseSetting>&  noise_settings)
-    {
-        const int image_count = Utils::toNumeric(ImageType::Count);
-        noise_settings.clear();
-        for (int i = -1; i < image_count; ++i)
-            noise_settings[i] = NoiseSetting();
-    }
+        static void initializeNoiseSettings(std::map<int, NoiseSetting>& noise_settings)
+        {
+            const int image_count = Utils::toNumeric(ImageType::Count);
+            noise_settings.clear();
+            for (int i = -1; i < image_count; ++i)
+                noise_settings[i] = NoiseSetting();
+        }
 
-    static void loadNoiseSettings(const Settings& settings_json, std::map<int, NoiseSetting>&  noise_settings)
-    {
-        initializeNoiseSettings(noise_settings);
-
-        Settings json_parent;
-        if (settings_json.getChild("NoiseSettings", json_parent)) {
-            for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
-                Settings json_settings_child;
-                if (json_parent.getChild(child_index, json_settings_child)) {
-                    NoiseSetting noise_setting;
-                    loadNoiseSetting(json_settings_child, noise_setting);
-                    noise_settings[noise_setting.ImageType] = noise_setting;
+        static void loadNoiseSettings(const Settings& settings_json, std::map<int, NoiseSetting>& noise_settings)
+        {
+            initializeNoiseSettings(noise_settings);
+
+            Settings json_parent;
+            if (settings_json.getChild("NoiseSettings", json_parent)) {
+                for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
+                    Settings json_settings_child;
+                    if (json_parent.getChild(child_index, json_settings_child)) {
+                        NoiseSetting noise_setting;
+                        loadNoiseSetting(json_settings_child, noise_setting);
+                        noise_settings[noise_setting.ImageType] = noise_setting;
+                    }
                 }
             }
         }
-    }
-
-    static void loadNoiseSetting(const msr::airlib::Settings& settings_json, NoiseSetting& noise_setting)
-    {
-        noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled);
-        noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType);
-
-        noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength);
-        noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed);
-        noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize);
-        noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity);
-        noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib);
-        noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib);
-        noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize);
-        noise_setting.HorzWaveScreenSize = settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize);
-        noise_setting.HorzNoiseLinesContrib = settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib);
-        noise_setting.HorzNoiseLinesDensityY = settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY);
-        noise_setting.HorzNoiseLinesDensityXY = settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY);
-        noise_setting.HorzDistortionStrength = settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength);
-        noise_setting.HorzDistortionContrib = settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib);
-    }
-
-    static GimbalSetting createGimbalSetting(const Settings& settings_json)
-    {
-        GimbalSetting gimbal;
-        //capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false);
-        gimbal.stabilization = settings_json.getFloat("Stabilization", false);
-        gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation);
-        return gimbal;
-    }
-
-    static CameraSetting createCameraSetting(const Settings& settings_json)
-    {
-        CameraSetting setting;
 
-        setting.position = createVectorSetting(settings_json, setting.position);
-        setting.rotation = createRotationSetting(settings_json, setting.rotation);
+        static void loadNoiseSetting(const msr::airlib::Settings& settings_json, NoiseSetting& noise_setting)
+        {
+            noise_setting.Enabled = settings_json.getBool("Enabled", noise_setting.Enabled);
+            noise_setting.ImageType = settings_json.getInt("ImageType", noise_setting.ImageType);
+
+            noise_setting.HorzWaveStrength = settings_json.getFloat("HorzWaveStrength", noise_setting.HorzWaveStrength);
+            noise_setting.RandSpeed = settings_json.getFloat("RandSpeed", noise_setting.RandSpeed);
+            noise_setting.RandSize = settings_json.getFloat("RandSize", noise_setting.RandSize);
+            noise_setting.RandDensity = settings_json.getFloat("RandDensity", noise_setting.RandDensity);
+            noise_setting.RandContrib = settings_json.getFloat("RandContrib", noise_setting.RandContrib);
+            noise_setting.HorzWaveContrib = settings_json.getFloat("HorzWaveContrib", noise_setting.HorzWaveContrib);
+            noise_setting.HorzWaveVertSize = settings_json.getFloat("HorzWaveVertSize", noise_setting.HorzWaveVertSize);
+            noise_setting.HorzWaveScreenSize = settings_json.getFloat("HorzWaveScreenSize", noise_setting.HorzWaveScreenSize);
+            noise_setting.HorzNoiseLinesContrib = settings_json.getFloat("HorzNoiseLinesContrib", noise_setting.HorzNoiseLinesContrib);
+            noise_setting.HorzNoiseLinesDensityY = settings_json.getFloat("HorzNoiseLinesDensityY", noise_setting.HorzNoiseLinesDensityY);
+            noise_setting.HorzNoiseLinesDensityXY = settings_json.getFloat("HorzNoiseLinesDensityXY", noise_setting.HorzNoiseLinesDensityXY);
+            noise_setting.HorzDistortionStrength = settings_json.getFloat("HorzDistortionStrength", noise_setting.HorzDistortionStrength);
+            noise_setting.HorzDistortionContrib = settings_json.getFloat("HorzDistortionContrib", noise_setting.HorzDistortionContrib);
+        }
 
-        loadCaptureSettings(settings_json, setting.capture_settings);
-        loadNoiseSettings(settings_json, setting.noise_settings);
-        Settings json_gimbal;
-        if (settings_json.getChild("Gimbal", json_gimbal))
-            setting.gimbal = createGimbalSetting(json_gimbal);
+        static GimbalSetting createGimbalSetting(const Settings& settings_json)
+        {
+            GimbalSetting gimbal;
+            //capture_setting.gimbal.is_world_frame = settings_json.getBool("IsWorldFrame", false);
+            gimbal.stabilization = settings_json.getFloat("Stabilization", false);
+            gimbal.rotation = createRotationSetting(settings_json, gimbal.rotation);
+            return gimbal;
+        }
 
-        return setting;
-    }
+        static CameraSetting createCameraSetting(const Settings& settings_json)
+        {
+            CameraSetting setting;
 
-    static void loadCameraSettings(const Settings& settings_json, std::map<std::string, CameraSetting>& cameras)
-    {
-        cameras.clear();
+            setting.position = createVectorSetting(settings_json, setting.position);
+            setting.rotation = createRotationSetting(settings_json, setting.rotation);
 
-        Settings json_parent;
-        if (settings_json.getChild("Cameras", json_parent)) {
-            std::vector<std::string> keys;
-            json_parent.getChildNames(keys);
+            loadCaptureSettings(settings_json, setting.capture_settings);
+            loadNoiseSettings(settings_json, setting.noise_settings);
+            Settings json_gimbal;
+            if (settings_json.getChild("Gimbal", json_gimbal))
+                setting.gimbal = createGimbalSetting(json_gimbal);
 
-            for (const auto& key : keys) {
-                msr::airlib::Settings child;
-                json_parent.getChild(key, child);
-                cameras[key] = createCameraSetting(child);
-            }
+            return setting;
         }
-    }
 
-    static void createCaptureSettings(const msr::airlib::Settings& settings_json, CaptureSetting& capture_setting)
-    {
-        capture_setting.width = settings_json.getInt("Width", capture_setting.width);
-        capture_setting.height = settings_json.getInt("Height", capture_setting.height);
-        capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees);
-        capture_setting.auto_exposure_speed = settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed);
-        capture_setting.auto_exposure_bias = settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias);
-        capture_setting.auto_exposure_max_brightness = settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness);
-        capture_setting.auto_exposure_min_brightness = settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness);
-        capture_setting.motion_blur_amount = settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount);
-        capture_setting.image_type = settings_json.getInt("ImageType", 0);
-        capture_setting.target_gamma = settings_json.getFloat("TargetGamma",
-            capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan<float>());
-
-        std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", ""));
-        if (projection_mode == "" || projection_mode == "perspective")
-            capture_setting.projection_mode = 0; // Perspective
-        else if (projection_mode == "orthographic")
-            capture_setting.projection_mode = 1; // Orthographic
-        else
-            throw std::invalid_argument(std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode);
-
-        capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width);
-    }
-
-    static void loadSubWindowsSettings(const Settings& settings_json, std::vector<SubwindowSetting>& subwindow_settings)
-    {
-        //load default subwindows
-        initializeSubwindowSettings(subwindow_settings);
-
-        Settings json_parent;
-        if (settings_json.getChild("SubWindows", json_parent)) {
-            for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
-                Settings json_settings_child;
-                if (json_parent.getChild(child_index, json_settings_child)) {
-                    int window_index = json_settings_child.getInt("WindowID", 0);
-                    SubwindowSetting& subwindow_setting = subwindow_settings.at(window_index);
-                    subwindow_setting.window_index = window_index;
-                    subwindow_setting.image_type = Utils::toEnum<ImageType>(
-                        json_settings_child.getInt("ImageType", 0));
-                    subwindow_setting.visible = json_settings_child.getBool("Visible", false);
-                    subwindow_setting.camera_name = getCameraName(json_settings_child);
-                    subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", "");
+        static void loadCameraSettings(const Settings& settings_json, std::map<std::string, CameraSetting>& cameras)
+        {
+            cameras.clear();
+
+            Settings json_parent;
+            if (settings_json.getChild("Cameras", json_parent)) {
+                std::vector<std::string> keys;
+                json_parent.getChildNames(keys);
+
+                for (const auto& key : keys) {
+                    msr::airlib::Settings child;
+                    json_parent.getChild(key, child);
+                    cameras[key] = createCameraSetting(child);
                 }
             }
         }
-    }
 
-    static void initializeSubwindowSettings(std::vector<SubwindowSetting>& subwindow_settings)
-    {
-        subwindow_settings.clear();
-        subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "")); //depth
-        subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "")); //seg
-        subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "")); //vis
-    }
+        static void createCaptureSettings(const msr::airlib::Settings& settings_json, CaptureSetting& capture_setting)
+        {
+            capture_setting.width = settings_json.getInt("Width", capture_setting.width);
+            capture_setting.height = settings_json.getInt("Height", capture_setting.height);
+            capture_setting.fov_degrees = settings_json.getFloat("FOV_Degrees", capture_setting.fov_degrees);
+            capture_setting.auto_exposure_speed = settings_json.getFloat("AutoExposureSpeed", capture_setting.auto_exposure_speed);
+            capture_setting.auto_exposure_bias = settings_json.getFloat("AutoExposureBias", capture_setting.auto_exposure_bias);
+            capture_setting.auto_exposure_max_brightness = settings_json.getFloat("AutoExposureMaxBrightness", capture_setting.auto_exposure_max_brightness);
+            capture_setting.auto_exposure_min_brightness = settings_json.getFloat("AutoExposureMinBrightness", capture_setting.auto_exposure_min_brightness);
+            capture_setting.motion_blur_amount = settings_json.getFloat("MotionBlurAmount", capture_setting.motion_blur_amount);
+            capture_setting.image_type = settings_json.getInt("ImageType", 0);
+            capture_setting.target_gamma = settings_json.getFloat("TargetGamma",
+                                                                  capture_setting.image_type == 0 ? CaptureSetting::kSceneTargetGamma : Utils::nan<float>());
+
+            std::string projection_mode = Utils::toLower(settings_json.getString("ProjectionMode", ""));
+            if (projection_mode == "" || projection_mode == "perspective")
+                capture_setting.projection_mode = 0; // Perspective
+            else if (projection_mode == "orthographic")
+                capture_setting.projection_mode = 1; // Orthographic
+            else
+                throw std::invalid_argument(std::string("CaptureSettings projection_mode has invalid value in settings_json ") + projection_mode);
 
-    void loadOtherSettings(const Settings& settings_json)
-    {
-        //by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below
-        //because for docker container default is 0.0.0.0 and people get really confused why things
-        //don't work
-        api_server_address = settings_json.getString("LocalHostIp", "");
-        api_port = settings_json.getInt("ApiServerPort", RpcLibPort);
-        is_record_ui_visible = settings_json.getBool("RecordUIVisible", true);
-        engine_sound = settings_json.getBool("EngineSound", false);
-        enable_rpc = settings_json.getBool("EnableRpc", enable_rpc);
-        speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f);
-        speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s");
-        log_messages_visible = settings_json.getBool("LogMessagesVisible", true);
-
-        {   //load origin geopoint
-            Settings origin_geopoint_json;
-            if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) {
-                GeoPoint origin = origin_geopoint.home_geo_point;
-                origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude);
-                origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude);
-                origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude);
-                origin_geopoint.initialize(origin);
-            }
+            capture_setting.ortho_width = settings_json.getFloat("OrthoWidth", capture_setting.ortho_width);
         }
 
-        {   //time of day settings_json
-            Settings tod_settings_json;
-            if (settings_json.getChild("TimeOfDay", tod_settings_json)) {
-                tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled);
-                tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime);
-                tod_setting.celestial_clock_speed = tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed);
-                tod_setting.is_start_datetime_dst = tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst);
-                tod_setting.update_interval_secs = tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs);
-                tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun);
+        static void loadSubWindowsSettings(const Settings& settings_json, std::vector<SubwindowSetting>& subwindow_settings)
+        {
+            //load default subwindows
+            initializeSubwindowSettings(subwindow_settings);
+
+            Settings json_parent;
+            if (settings_json.getChild("SubWindows", json_parent)) {
+                for (size_t child_index = 0; child_index < json_parent.size(); ++child_index) {
+                    Settings json_settings_child;
+                    if (json_parent.getChild(child_index, json_settings_child)) {
+                        int window_index = json_settings_child.getInt("WindowID", 0);
+                        SubwindowSetting& subwindow_setting = subwindow_settings.at(window_index);
+                        subwindow_setting.window_index = window_index;
+                        subwindow_setting.image_type = Utils::toEnum<ImageType>(
+                            json_settings_child.getInt("ImageType", 0));
+                        subwindow_setting.visible = json_settings_child.getBool("Visible", false);
+                        subwindow_setting.camera_name = getCameraName(json_settings_child);
+                        subwindow_setting.vehicle_name = json_settings_child.getString("VehicleName", "");
+                    }
+                }
             }
         }
 
+        static void initializeSubwindowSettings(std::vector<SubwindowSetting>& subwindow_settings)
         {
-            // Wind Settings
-            Settings child_json;
-            if (settings_json.getChild("Wind", child_json)) {
-                wind = createVectorSetting(child_json, wind);
-            }
+            subwindow_settings.clear();
+            subwindow_settings.push_back(SubwindowSetting(0, ImageType::DepthVis, false, "", "")); //depth
+            subwindow_settings.push_back(SubwindowSetting(1, ImageType::Segmentation, false, "", "")); //seg
+            subwindow_settings.push_back(SubwindowSetting(2, ImageType::Scene, false, "", "")); //vis
         }
-    }
 
-    static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults)
-    {
-        Settings child_json;
-        if (settings_json.getChild("CameraDefaults", child_json)) {
-            camera_defaults = createCameraSetting(child_json);
-        }
-    }
+        void loadOtherSettings(const Settings& settings_json)
+        {
+            //by default we spawn server at local endpoint. Do not use 127.0.0.1 as default below
+            //because for docker container default is 0.0.0.0 and people get really confused why things
+            //don't work
+            api_server_address = settings_json.getString("LocalHostIp", "");
+            api_port = settings_json.getInt("ApiServerPort", RpcLibPort);
+            is_record_ui_visible = settings_json.getBool("RecordUIVisible", true);
+            engine_sound = settings_json.getBool("EngineSound", false);
+            enable_rpc = settings_json.getBool("EnableRpc", enable_rpc);
+            speed_unit_factor = settings_json.getFloat("SpeedUnitFactor", 1.0f);
+            speed_unit_label = settings_json.getString("SpeedUnitLabel", "m\\s");
+            log_messages_visible = settings_json.getBool("LogMessagesVisible", true);
+
+            { //load origin geopoint
+                Settings origin_geopoint_json;
+                if (settings_json.getChild("OriginGeopoint", origin_geopoint_json)) {
+                    GeoPoint origin = origin_geopoint.home_geo_point;
+                    origin.latitude = origin_geopoint_json.getDouble("Latitude", origin.latitude);
+                    origin.longitude = origin_geopoint_json.getDouble("Longitude", origin.longitude);
+                    origin.altitude = origin_geopoint_json.getFloat("Altitude", origin.altitude);
+                    origin_geopoint.initialize(origin);
+                }
+            }
 
-    static void loadCameraDirectorSetting(const Settings& settings_json,
-        CameraDirectorSetting& camera_director, const std::string& simmode_name)
-    {
-        camera_director = CameraDirectorSetting();
+            { //time of day settings_json
+                Settings tod_settings_json;
+                if (settings_json.getChild("TimeOfDay", tod_settings_json)) {
+                    tod_setting.enabled = tod_settings_json.getBool("Enabled", tod_setting.enabled);
+                    tod_setting.start_datetime = tod_settings_json.getString("StartDateTime", tod_setting.start_datetime);
+                    tod_setting.celestial_clock_speed = tod_settings_json.getFloat("CelestialClockSpeed", tod_setting.celestial_clock_speed);
+                    tod_setting.is_start_datetime_dst = tod_settings_json.getBool("StartDateTimeDst", tod_setting.is_start_datetime_dst);
+                    tod_setting.update_interval_secs = tod_settings_json.getFloat("UpdateIntervalSecs", tod_setting.update_interval_secs);
+                    tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun);
+                }
+            }
 
-        Settings child_json;
-        if (settings_json.getChild("CameraDirector", child_json)) {
-            camera_director.position = createVectorSetting(settings_json, camera_director.position);
-            camera_director.rotation = createRotationSetting(settings_json, camera_director.rotation);
-            camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance);
+            {
+                // Wind Settings
+                Settings child_json;
+                if (settings_json.getChild("Wind", child_json)) {
+                    wind = createVectorSetting(child_json, wind);
+                }
+            }
         }
 
-        if (std::isnan(camera_director.follow_distance)) {
-            if (simmode_name == kSimModeTypeCar)
-                camera_director.follow_distance = -8;
-            else
-                camera_director.follow_distance = -3;
-        }
-        if (std::isnan(camera_director.position.x()))
-            camera_director.position.x() = camera_director.follow_distance;
-        if (std::isnan(camera_director.position.y()))
-            camera_director.position.y() = 0;
-        if (std::isnan(camera_director.position.z())) {
-            if (simmode_name == kSimModeTypeCar)
-                camera_director.position.z() = -4;
-            else
-                camera_director.position.z() = -2;
+        static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults)
+        {
+            Settings child_json;
+            if (settings_json.getChild("CameraDefaults", child_json)) {
+                camera_defaults = createCameraSetting(child_json);
+            }
         }
-    }
 
-    void loadClockSettings(const Settings& settings_json)
-    {
-        clock_type = settings_json.getString("ClockType", "");
+        static void loadCameraDirectorSetting(const Settings& settings_json,
+                                              CameraDirectorSetting& camera_director, const std::string& simmode_name)
+        {
+            camera_director = CameraDirectorSetting();
 
-        if (clock_type == "") {
-            //default value
-            clock_type = "ScalableClock";
+            Settings child_json;
+            if (settings_json.getChild("CameraDirector", child_json)) {
+                camera_director.position = createVectorSetting(settings_json, camera_director.position);
+                camera_director.rotation = createRotationSetting(settings_json, camera_director.rotation);
+                camera_director.follow_distance = child_json.getFloat("FollowDistance", camera_director.follow_distance);
+            }
 
-            //override if multirotor simmode with simple_flight
-            if (simmode_name == kSimModeTypeMultirotor) {
-                //TODO: this won't work if simple_flight and PX4 is combined together!
-
-                //for multirotors we select steppable fixed interval clock unless we have
-                //PX4 enabled vehicle
-                clock_type = "SteppableClock";
-                for (auto const& vehicle : vehicles) {
-                    if (vehicle.second->auto_create &&
-                        vehicle.second->vehicle_type == kVehicleTypePX4) {
-                        clock_type = "ScalableClock";
-                        break;
+            if (std::isnan(camera_director.follow_distance)) {
+                if (simmode_name == kSimModeTypeCar)
+                    camera_director.follow_distance = -8;
+                else
+                    camera_director.follow_distance = -3;
+            }
+            if (std::isnan(camera_director.position.x()))
+                camera_director.position.x() = camera_director.follow_distance;
+            if (std::isnan(camera_director.position.y()))
+                camera_director.position.y() = 0;
+            if (std::isnan(camera_director.position.z())) {
+                if (simmode_name == kSimModeTypeCar)
+                    camera_director.position.z() = -4;
+                else
+                    camera_director.position.z() = -2;
+            }
+        }
+
+        void loadClockSettings(const Settings& settings_json)
+        {
+            clock_type = settings_json.getString("ClockType", "");
+
+            if (clock_type == "") {
+                //default value
+                clock_type = "ScalableClock";
+
+                //override if multirotor simmode with simple_flight
+                if (simmode_name == kSimModeTypeMultirotor) {
+                    //TODO: this won't work if simple_flight and PX4 is combined together!
+
+                    //for multirotors we select steppable fixed interval clock unless we have
+                    //PX4 enabled vehicle
+                    clock_type = "SteppableClock";
+                    for (auto const& vehicle : vehicles) {
+                        if (vehicle.second->auto_create &&
+                            vehicle.second->vehicle_type == kVehicleTypePX4) {
+                            clock_type = "ScalableClock";
+                            break;
+                        }
                     }
                 }
             }
+
+            clock_speed = settings_json.getFloat("ClockSpeed", 1.0f);
         }
 
-        clock_speed = settings_json.getFloat("ClockSpeed", 1.0f);
-    }
+        static std::shared_ptr<SensorSetting> createSensorSetting(
+            SensorBase::SensorType sensor_type, const std::string& sensor_name,
+            bool enabled)
+        {
+            std::shared_ptr<SensorSetting> sensor_setting;
+
+            switch (sensor_type) {
+            case SensorBase::SensorType::Barometer:
+                sensor_setting = std::shared_ptr<SensorSetting>(new BarometerSetting());
+                break;
+            case SensorBase::SensorType::Imu:
+                sensor_setting = std::shared_ptr<SensorSetting>(new ImuSetting());
+                break;
+            case SensorBase::SensorType::Gps:
+                sensor_setting = std::shared_ptr<SensorSetting>(new GpsSetting());
+                break;
+            case SensorBase::SensorType::Magnetometer:
+                sensor_setting = std::shared_ptr<SensorSetting>(new MagnetometerSetting());
+                break;
+            case SensorBase::SensorType::Distance:
+                sensor_setting = std::shared_ptr<SensorSetting>(new DistanceSetting());
+                break;
+            case SensorBase::SensorType::Lidar:
+                sensor_setting = std::shared_ptr<SensorSetting>(new LidarSetting());
+                break;
+            default:
+                throw std::invalid_argument("Unexpected sensor type");
+            }
 
-    static std::shared_ptr<SensorSetting> createSensorSetting(
-        SensorBase::SensorType sensor_type, const std::string& sensor_name,
-        bool enabled)
-    {
-        std::shared_ptr<SensorSetting> sensor_setting;
-
-        switch (sensor_type) {
-        case SensorBase::SensorType::Barometer:
-            sensor_setting = std::shared_ptr<SensorSetting>(new BarometerSetting());
-            break;
-        case SensorBase::SensorType::Imu:
-            sensor_setting = std::shared_ptr<SensorSetting>(new ImuSetting());
-            break;
-        case SensorBase::SensorType::Gps:
-            sensor_setting = std::shared_ptr<SensorSetting>(new GpsSetting());
-            break;
-        case SensorBase::SensorType::Magnetometer:
-            sensor_setting = std::shared_ptr<SensorSetting>(new MagnetometerSetting());
-            break;
-        case SensorBase::SensorType::Distance:
-            sensor_setting = std::shared_ptr<SensorSetting>(new DistanceSetting());
-            break;
-        case SensorBase::SensorType::Lidar:
-            sensor_setting = std::shared_ptr<SensorSetting>(new LidarSetting());
-            break;
-        default:
-            throw std::invalid_argument("Unexpected sensor type");
+            sensor_setting->sensor_type = sensor_type;
+            sensor_setting->sensor_name = sensor_name;
+            sensor_setting->enabled = enabled;
+
+            return sensor_setting;
         }
 
-        sensor_setting->sensor_type = sensor_type;
-        sensor_setting->sensor_name = sensor_name;
-        sensor_setting->enabled = enabled;
+        static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json)
+        {
+            sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled);
 
-        return sensor_setting;
-    }
+            // pass the json Settings property bag through to the specific sensor params object where it is
+            // extracted there.  This way default values can be kept in one place.  For example, see the
+            // BarometerSimpleParams::initializeFromSettings method.
+            sensor_setting->settings = settings_json;
+        }
 
-    static void initializeSensorSetting(SensorSetting* sensor_setting, const Settings& settings_json)
-    {
-        sensor_setting->enabled = settings_json.getBool("Enabled", sensor_setting->enabled);
-        
-        // pass the json Settings property bag through to the specific sensor params object where it is
-        // extracted there.  This way default values can be kept in one place.  For example, see the 
-        // BarometerSimpleParams::initializeFromSettings method.
-        sensor_setting->settings = settings_json;
-    }
-
-    // creates and intializes sensor settings from json
-    static void loadSensorSettings( const Settings& settings_json, const std::string& collectionName,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
+        // creates and intializes sensor settings from json
+        static void loadSensorSettings(const Settings& settings_json, const std::string& collectionName,
+                                       std::map<std::string, std::shared_ptr<SensorSetting>>& sensors,
+                                       std::map<std::string, std::shared_ptr<SensorSetting>>& sensor_defaults)
 
-    {
-        // start with defaults.
-        for (auto ptr = sensor_defaults.begin(); ptr != sensor_defaults.end(); ptr++) {
-            auto key = ptr->first;
-            sensors[key] = sensor_defaults[key];
-        }
+        {
+            // start with defaults.
+            for (auto ptr = sensor_defaults.begin(); ptr != sensor_defaults.end(); ptr++) {
+                auto key = ptr->first;
+                sensors[key] = sensor_defaults[key];
+            }
 
-        msr::airlib::Settings sensors_child;
-        if (settings_json.getChild(collectionName, sensors_child)) {
-            std::vector<std::string> keys;
-            sensors_child.getChildNames(keys);
+            msr::airlib::Settings sensors_child;
+            if (settings_json.getChild(collectionName, sensors_child)) {
+                std::vector<std::string> keys;
+                sensors_child.getChildNames(keys);
 
-            for (const auto& key : keys) {
-                msr::airlib::Settings child;
-                sensors_child.getChild(key, child);
+                for (const auto& key : keys) {
+                    msr::airlib::Settings child;
+                    sensors_child.getChild(key, child);
 
-                auto sensor_type = Utils::toEnum<SensorBase::SensorType>(child.getInt("SensorType", 0));
-                auto enabled = child.getBool("Enabled", false);
+                    auto sensor_type = Utils::toEnum<SensorBase::SensorType>(child.getInt("SensorType", 0));
+                    auto enabled = child.getBool("Enabled", false);
 
-                sensors[key] = createSensorSetting(sensor_type, key, enabled);
-                initializeSensorSetting(sensors[key].get(), child);
+                    sensors[key] = createSensorSetting(sensor_type, key, enabled);
+                    initializeSensorSetting(sensors[key].get(), child);
+                }
             }
         }
-    }
 
-    // creates default sensor list when none specified in json
-    static void createDefaultSensorSettings(const std::string& simmode_name,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
-    {
-        if (simmode_name == kSimModeTypeMultirotor) {
-            sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true);
-            sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true);
-            sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
-            sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true);
-        }
-        else if (simmode_name == kSimModeTypeCar) {
-            sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
+        // creates default sensor list when none specified in json
+        static void createDefaultSensorSettings(const std::string& simmode_name,
+                                                std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
+        {
+            if (simmode_name == kSimModeTypeMultirotor) {
+                sensors["imu"] = createSensorSetting(SensorBase::SensorType::Imu, "imu", true);
+                sensors["magnetometer"] = createSensorSetting(SensorBase::SensorType::Magnetometer, "magnetometer", true);
+                sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
+                sensors["barometer"] = createSensorSetting(SensorBase::SensorType::Barometer, "barometer", true);
+            }
+            else if (simmode_name == kSimModeTypeCar) {
+                sensors["gps"] = createSensorSetting(SensorBase::SensorType::Gps, "gps", true);
+            }
+            else {
+                // no sensors added for other modes
+            }
         }
-        else {
-            // no sensors added for other modes
+
+        // loads or creates default sensor list
+        static void loadDefaultSensorSettings(const std::string& simmode_name,
+                                              const Settings& settings_json,
+                                              std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
+        {
+            msr::airlib::Settings sensors_child;
+            if (settings_json.getChild("DefaultSensors", sensors_child))
+                loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors);
+            else
+                createDefaultSensorSettings(simmode_name, sensors);
         }
-    }
+    };
 
-    // loads or creates default sensor list
-    static void loadDefaultSensorSettings(const std::string& simmode_name,
-        const Settings& settings_json,
-        std::map<std::string, std::shared_ptr<SensorSetting>>& sensors)
-    {
-        msr::airlib::Settings sensors_child;
-        if (settings_json.getChild("DefaultSensors", sensors_child))
-            loadSensorSettings(settings_json, "DefaultSensors", sensors, sensors);
-        else
-            createDefaultSensorSettings(simmode_name, sensors);
-    }
-};
-
-}} //namespace
+}
+} //namespace
 #endif
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 6e1977cee9..36bf96e088 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -63,7 +63,7 @@ namespace airlib
             connection_info_ = connection_info;
             sensors_ = sensors;
             is_simulation_mode_ = is_simulation;
-
+            lock_step_enabled_ = connection_info.lock_step;
             try {
                 openAllConnections();
                 is_ready_ = true;
@@ -106,7 +106,7 @@ namespace airlib
         unsigned long long getSimTime()
         {
             // This ensures HIL_SENSOR and HIL_GPS have matching clocks.
-            if (lock_step_enabled_) {
+            if (lock_step_active_) {
                 if (sim_time_us_ == 0) {
                     sim_time_us_ = clock()->nowNanos() / 1000;
                 }
@@ -137,10 +137,10 @@ namespace airlib
                     update_count_++;
                 }
 
-                if (lock_step_enabled_) {
-                    if (last_update_time_ + 100000 < now) {
-                        // if 100 ms passes then something is terribly wrong, reset lockstep mode
-                        lock_step_enabled_ = false;
+                if (lock_step_active_) {
+                    if (last_update_time_ + 1000000 < now) {
+                        // if 1 second passes then something is terribly wrong, reset lockstep mode
+                        lock_step_active_ = false;
                         {
                             std::lock_guard<std::mutex> guard(telemetry_mutex_);
                             lock_step_resets_++;
@@ -613,6 +613,7 @@ namespace airlib
                 }
                 else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
                     addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
+                    hil_message_timer_.stop();
                 }
             }
             else {
@@ -1607,11 +1608,11 @@ namespace airlib
         {
             received_actuator_controls_ = true;
             // if the timestamps match then it means we are in lockstep mode.
-            if (!lock_step_enabled_) {
+            if (!lock_step_active_ && lock_step_enabled_) {
                 // && (HilActuatorControlsMessage.flags & 0x1))    // todo: enable this check when this flag is widely available...
                 if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
                     addStatusMessage("Enabling lockstep mode");
-                    lock_step_enabled_ = true;
+                    lock_step_active_ = true;
                 }
             }
             else {
@@ -1787,8 +1788,8 @@ namespace airlib
             if (hil_node_ != nullptr) {
                 hil_node_->sendMessage(hil_sensor);
                 received_actuator_controls_ = false;
-                if (lock_step_enabled_ && world_ != nullptr) {
-                    world_->pauseForTime(0.1); // 100 millisecond delay max waiting for actuator controls.
+                if (lock_step_active_ && world_ != nullptr) {
+                    world_->pauseForTime(1); // 1 second delay max waiting for actuator controls.
                 }
             }
 
@@ -1834,10 +1835,13 @@ namespace airlib
             distance_sensor.quaternion[3] = orientation.z();
 
             //TODO: use covariance parameter?
-
-            if (hil_node_ != nullptr) {
-                hil_node_->sendMessage(distance_sensor);
-            }
+            // PX4 doesn't support receiving simulated distance sensor messages this way.
+            // It results in lots of error messages from PX4.  This code is still useful in that
+            // it sets last_distance_message_ and that is returned via Python API.
+            //
+            // if (hil_node_ != nullptr) {
+            //    hil_node_->sendMessage(distance_sensor);
+            // }
 
             std::lock_guard<std::mutex> guard(last_message_mutex_);
             last_distance_message_ = distance_sensor;
@@ -1906,6 +1910,7 @@ namespace airlib
             Utils::setValue(rotor_controls_, 0.0f);
             was_reset_ = false;
             received_actuator_controls_ = false;
+            lock_step_active_ = false;
             lock_step_enabled_ = false;
             has_gps_lock_ = false;
             send_params_ = false;
@@ -1982,6 +1987,7 @@ namespace airlib
         bool has_home_ = false;
         bool is_ready_ = false;
         bool has_gps_lock_ = false;
+        bool lock_step_active_ = false;
         bool lock_step_enabled_ = false;
         bool received_actuator_controls_ = false;
         std::string is_ready_message_;
diff --git a/docs/px4_lockstep.md b/docs/px4_lockstep.md
new file mode 100644
index 0000000000..c12d8b554c
--- /dev/null
+++ b/docs/px4_lockstep.md
@@ -0,0 +1,40 @@
+# LockStep
+
+The latest version of PX4 supports a new [lockstep
+feature](https://docs.px4.io/master/en/simulation/#lockstep-simulation) when communicating with the
+simulator over TCP.  Lockstep is an important feature because it synchronizes PX4 and the simulator
+so they essentially use the same clock time.  This makes PX4 behave normally even during unusually
+long delays in Simulator performance.
+
+It is recommended that when you are running a lockstep enabled version of PX4 in SITL mode that you
+tell AirSim to use a `SteppableClock`, and set `UseTcp` to `true` and `LockStep` to `true`.
+
+```
+    {
+        "SettingsVersion": 1.2,
+        "SimMode": "Multirotor",
+        "ClockType": "SteppableClock",
+        "Vehicles": {
+            "PX4": {
+                "VehicleType": "PX4Multirotor",
+                "UseTcp": true,
+                "LockStep": true,
+                ...
+```
+
+This causes AirSim to not use a "realtime" clock, but instead it advances the clock in step which
+each sensor update sent to PX4.  This way PX4 thinks time is progressing smoothly no matter how long
+it takes AirSim to really process that update loop.
+
+This has the following advantages:
+
+- AirSim can be used on slow machines that cannot process updates quickly.
+- You can debug AirSim and hit a breakpoint, and when you resume PX4 will behave normally.
+- You can enable very slow sensors like the Lidar with large number of simulated points, and PX4
+  will still behave normally.
+
+There will be some side effects to `lockstep`, namely, slower update loops caused by running AirSim
+on an underpowered machine or from expensive sensors (like Lidar) will create some visible jerkiness
+in the simulated flight if you look at the updates on screen in realtime.
+
+
diff --git a/docs/px4_setup.md b/docs/px4_setup.md
index 03914471bf..b1ba3e454d 100644
--- a/docs/px4_setup.md
+++ b/docs/px4_setup.md
@@ -34,10 +34,12 @@ See also [initial firmware setup video](https://docs.px4.io/master/en/config/).
     {
         "SettingsVersion": 1.2,
         "SimMode": "Multirotor",
+        "ClockType": "SteppableClock",
         "Vehicles": {
             "PX4": {
                 "VehicleType": "PX4Multirotor",
                 "UseSerial": true,
+                "LockStep": true,
                 "Sensors":{
                     "Barometer":{
                         "SensorType": 1,
@@ -57,6 +59,8 @@ See also [initial firmware setup video](https://docs.px4.io/master/en/config/).
     }
 ```
 
+See [PX4 LockStep](px4_lockstep.md) for more information.
+
 The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
 noise generation.  This setting clamps that down a bit.
 
diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md
index 2c664fa111..0fa2b8ce40 100644
--- a/docs/px4_sitl.md
+++ b/docs/px4_sitl.md
@@ -56,10 +56,12 @@ The default ports have changed recently, so check them closely to make sure AirS
     {
         "SettingsVersion": 1.2,
         "SimMode": "Multirotor",
+        "ClockType": "SteppableClock",
         "Vehicles": {
             "PX4": {
                 "VehicleType": "PX4Multirotor",
                 "UseSerial": false,
+                "LockStep": true,
                 "UseTcp": true,
                 "TcpPort": 4560,
                 "ControlPort": 14580,
@@ -82,6 +84,7 @@ The default ports have changed recently, so check them closely to make sure AirS
     }
     ```
     Notice the PX4 `[simulator]` is using TCP, which is why we need to add: `"UseTcp": true,`.
+    See [PX4 LockStep](px4_lockstep.md) for more information.
     The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
     noise generation.  This setting clamps that down a bit.
 
diff --git a/docs/px4_sitl_wsl2.md b/docs/px4_sitl_wsl2.md
index 03baaa8936..4627caae28 100644
--- a/docs/px4_sitl_wsl2.md
+++ b/docs/px4_sitl_wsl2.md
@@ -47,10 +47,12 @@ This resolves to the WSL 2 remote ip address found in the TCP socket.
 {
     "SettingsVersion": 1.2,
     "SimMode": "Multirotor",
+    "ClockType": "SteppableClock",
     "Vehicles": {
         "PX4": {
             "VehicleType": "PX4Multirotor",
             "UseSerial": false,
+            "LockStep": true,
             "UseTcp": true,
             "TcpPort": 4560,
             "ControlIp": "remote",
@@ -74,7 +76,7 @@ This resolves to the WSL 2 remote ip address found in the TCP socket.
     }
 }
 ```
-
+See [PX4 LockStep](px4_lockstep.md) for more information.
 The "Barometer" setting keeps PX4 happy because the default AirSim barometer has a bit too much
 noise generation.  This setting clamps that down a bit.
 
@@ -89,7 +91,7 @@ if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
     simulator start -c $simulator_tcp_port
 else
     echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR"
-    simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port 
+    simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port
 fi
 ```
 

From ef5a6a656f4a06b8489ee5d6bbb1b4d603378288 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Tue, 4 May 2021 12:25:05 -0700
Subject: [PATCH 36/37] add lockstep to toc

---
 mkdocs.yml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/mkdocs.yml b/mkdocs.yml
index d0fb62d6a1..37f096a79f 100644
--- a/mkdocs.yml
+++ b/mkdocs.yml
@@ -87,6 +87,7 @@ nav:
       - "PX4 Setup for AirSim": 'px4_setup.md'
       - "PX4 in SITL": 'px4_sitl.md'
       - "PX4 SITL with WSL 2": 'px4_sitl_wsl2.md'
+      - "PX4 Lockstep": 'px4_lockstep.md'
       - "PX4 Multi-vehicle in SITL": 'px4_multi_vehicle.md'
       - "AirSim with Pixhawk": 'https://youtu.be/1oY8Qu5maQQ'
       - "PX4 Setup with AirSim": 'https://youtu.be/HNWdYrtw3f0'

From 6db5c7f80f57ce04b068c8c2aebfd4e5fc3e3599 Mon Sep 17 00:00:00 2001
From: Chris Lovett <clovett@microsoft.com>
Date: Wed, 5 May 2021 16:30:24 -0700
Subject: [PATCH 37/37] Make sure each buffer is zero'd out before reading
 bytes.  Remove unnecessary metadata from mavlink messages and make tooltips
 show on top of close box in the charts.

---
 .gitignore                                    |   1 -
 .../LogViewer/Controls/SimpleLineChart.xaml   |  19 +-
 LogViewer/LogViewer/LogViewer.csproj          |   2 +-
 LogViewer/LogViewer/Model/MavlinkLog.cs       |   8 +-
 LogViewer/LogViewer/publish.cmd               |   6 +
 LogViewer/Networking/Mavlink/MavlinkTypes.cs  | 358 +++++++++---------
 6 files changed, 203 insertions(+), 191 deletions(-)
 create mode 100644 LogViewer/LogViewer/publish.cmd

diff --git a/.gitignore b/.gitignore
index 628c35995d..ec5b0897c7 100644
--- a/.gitignore
+++ b/.gitignore
@@ -386,4 +386,3 @@ build_docs/
 
 # api docs
 PythonClient/docs/_build
-/LogViewer/LogViewer/publish.cmd
diff --git a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml
index ef8824b346..f4043fe64e 100644
--- a/LogViewer/LogViewer/Controls/SimpleLineChart.xaml
+++ b/LogViewer/LogViewer/Controls/SimpleLineChart.xaml
@@ -33,14 +33,6 @@
         <Canvas Grid.Row="0" VerticalAlignment="Top">
             <Path x:Name="Graph" StrokeThickness="5" VerticalAlignment="Top" SnapsToDevicePixels="True" StrokeLineJoin="Bevel">
             </Path>
-            <Path x:Name="Pointer" Fill="{StaticResource TooltipForeground}" Data="M0,-5 L 5,0 0,5 -5 0z" Visibility="Collapsed"/>
-            <local:PointerBorder x:Name="PointerBorder"  Visibility="Collapsed" Padding="2" HorizontalAlignment="Left" VerticalAlignment="Top" BorderThickness="1"
-                    CornerRadius="3" BorderBrush="{StaticResource TooltipForeground}" Background="#80303030">
-                <TextBlock x:Name="PointerLabel" Foreground="{StaticResource TooltipForeground}"/>
-            </local:PointerBorder>
-            <Canvas x:Name="AdornerCanvas">
-                
-            </Canvas>
         </Canvas>
 
         <Grid>
@@ -54,5 +46,16 @@
         </Grid>
 
         <local:CloseBox HorizontalAlignment="Right" VerticalAlignment="Top" Margin="5" Click="CloseBoxClick"/>
+        
+        <Canvas Grid.Row="0" VerticalAlignment="Top">
+            <Path x:Name="Pointer" Fill="{StaticResource TooltipForeground}" Data="M0,-5 L 5,0 0,5 -5 0z" Visibility="Collapsed"/>
+            <local:PointerBorder x:Name="PointerBorder"  Visibility="Collapsed" Padding="2" HorizontalAlignment="Left" VerticalAlignment="Top" BorderThickness="1"
+                    CornerRadius="3" BorderBrush="{StaticResource TooltipForeground}" Background="#80303030">
+                <TextBlock x:Name="PointerLabel" Foreground="{StaticResource TooltipForeground}"/>
+            </local:PointerBorder>
+            <Canvas x:Name="AdornerCanvas">
+
+            </Canvas>
+        </Canvas>
     </Grid>
 </UserControl>
diff --git a/LogViewer/LogViewer/LogViewer.csproj b/LogViewer/LogViewer/LogViewer.csproj
index 3dd09bb285..a0a5065def 100644
--- a/LogViewer/LogViewer/LogViewer.csproj
+++ b/LogViewer/LogViewer/LogViewer.csproj
@@ -33,7 +33,7 @@
     <SuiteName>PX4 Log Viewer</SuiteName>
     <CreateWebPageOnPublish>true</CreateWebPageOnPublish>
     <WebPage>publish.htm</WebPage>
-    <ApplicationRevision>62</ApplicationRevision>
+    <ApplicationRevision>63</ApplicationRevision>
     <ApplicationVersion>1.0.0.%2a</ApplicationVersion>
     <UseApplicationTrust>false</UseApplicationTrust>
     <PublishWizardCompleted>true</PublishWizardCompleted>
diff --git a/LogViewer/LogViewer/Model/MavlinkLog.cs b/LogViewer/LogViewer/Model/MavlinkLog.cs
index 7b92b7dfde..bb0572f0f8 100644
--- a/LogViewer/LogViewer/Model/MavlinkLog.cs
+++ b/LogViewer/LogViewer/Model/MavlinkLog.cs
@@ -584,7 +584,8 @@ public async Task Load(string file, ProgressUtility progress)
             this.duration = TimeSpan.Zero;
             bool first = true;
             // QT time is milliseconds from the following epoch.
-            byte[] msgBuf = new byte[256];
+            const int BUFFER_SIZE = 8000;
+            byte[] msgBuf = new byte[BUFFER_SIZE];
             GCHandle handle = GCHandle.Alloc(msgBuf, GCHandleType.Pinned);
             IntPtr ptr = handle.AddrOfPinnedObject();
             
@@ -605,14 +606,17 @@ await Task.Run(() =>
                         {
                             MavLinkMessage header = new MavLinkMessage();
                             header.ReadHeader(reader);
+
+                            Array.Clear(msgBuf, 0, BUFFER_SIZE);
                             int read = s.Read(msgBuf, 0, header.Length);
 
                             if (read == header.Length)
                             {
+                                int id = (int)header.MsgId;
                                 header.Crc = reader.ReadUInt16();
 
                                 bool checkCrc = true;
-                                if ((int)header.MsgId == MAVLink.mavlink_telemetry.MessageId)
+                                if (id == MAVLink.mavlink_telemetry.MessageId)
                                 {
                                     if (header.Crc == 0)  // telemetry has no crc.
                                     {
diff --git a/LogViewer/LogViewer/publish.cmd b/LogViewer/LogViewer/publish.cmd
new file mode 100644
index 0000000000..7c55bbcb06
--- /dev/null
+++ b/LogViewer/LogViewer/publish.cmd
@@ -0,0 +1,6 @@
+@echo off
+cd %~dp0
+
+echo Uploading ClickOnce installer for Px4LogViewer
+AzurePublishClickOnce %~dp0publish downloads/Px4LogViewer "%LOVETTSOFTWARE_STORAGE_CONNECTION_STRING%"
+if ERRORLEVEL 1 goto :eof
diff --git a/LogViewer/Networking/Mavlink/MavlinkTypes.cs b/LogViewer/Networking/Mavlink/MavlinkTypes.cs
index 549f056b50..ceecab6229 100644
--- a/LogViewer/Networking/Mavlink/MavlinkTypes.cs
+++ b/LogViewer/Networking/Mavlink/MavlinkTypes.cs
@@ -2350,7 +2350,7 @@ public enum ADSB_FLAGS
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_sensor_offsets_t
         {
             /// <summary> magnetic declination (radians) </summary>
@@ -2381,7 +2381,7 @@ public struct mavlink_sensor_offsets_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_mag_offsets_t
         {
             /// <summary> magnetometer X offset </summary>
@@ -2398,7 +2398,7 @@ public struct mavlink_set_mag_offsets_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_meminfo_t
         {
             /// <summary> heap top </summary>
@@ -2409,7 +2409,7 @@ public struct mavlink_meminfo_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ap_adc_t
         {
             /// <summary> ADC output 1 </summary>
@@ -2428,7 +2428,7 @@ public struct mavlink_ap_adc_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 15)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_digicam_configure_t
         {
             /// <summary> Correspondent value to given extra_param </summary>
@@ -2457,7 +2457,7 @@ public struct mavlink_digicam_configure_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_digicam_control_t
         {
             /// <summary> Correspondent value to given extra_param </summary>
@@ -2484,7 +2484,7 @@ public struct mavlink_digicam_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mount_configure_t
         {
             /// <summary> System ID </summary>
@@ -2503,7 +2503,7 @@ public struct mavlink_mount_configure_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 15)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mount_control_t
         {
             /// <summary> pitch(deg*100) or lat, depending on mount mode </summary>
@@ -2522,7 +2522,7 @@ public struct mavlink_mount_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mount_status_t
         {
             /// <summary> pitch(deg*100) </summary>
@@ -2539,7 +2539,7 @@ public struct mavlink_mount_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_fence_point_t
         {
             /// <summary> Latitude of point </summary>
@@ -2558,7 +2558,7 @@ public struct mavlink_fence_point_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_fence_fetch_point_t
         {
             /// <summary> System ID </summary>
@@ -2571,7 +2571,7 @@ public struct mavlink_fence_fetch_point_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_fence_status_t
         {
             /// <summary> time of last breach in milliseconds since boot </summary>
@@ -2586,7 +2586,7 @@ public struct mavlink_fence_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ahrs_t
         {
             /// <summary> X gyro drift estimate rad/s </summary>
@@ -2607,7 +2607,7 @@ public struct mavlink_ahrs_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_simstate_t
         {
             /// <summary> Roll angle (rad) </summary>
@@ -2636,7 +2636,7 @@ public struct mavlink_simstate_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hwstatus_t
         {
             /// <summary> board voltage (mV) </summary>
@@ -2647,7 +2647,7 @@ public struct mavlink_hwstatus_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_radio_t
         {
             /// <summary> receive errors </summary>
@@ -2668,7 +2668,7 @@ public struct mavlink_radio_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_limits_status_t
         {
             /// <summary> time of last breach in milliseconds since boot </summary>
@@ -2693,7 +2693,7 @@ public struct mavlink_limits_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_wind_t
         {
             /// <summary> wind direction that wind is coming from (degrees) </summary>
@@ -2706,7 +2706,7 @@ public struct mavlink_wind_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data16_t
         {
             /// <summary> data type </summary>
@@ -2720,7 +2720,7 @@ public struct mavlink_data16_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 34)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data32_t
         {
             /// <summary> data type </summary>
@@ -2734,7 +2734,7 @@ public struct mavlink_data32_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 66)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data64_t
         {
             /// <summary> data type </summary>
@@ -2748,7 +2748,7 @@ public struct mavlink_data64_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 98)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data96_t
         {
             /// <summary> data type </summary>
@@ -2762,7 +2762,7 @@ public struct mavlink_data96_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rangefinder_t
         {
             /// <summary> distance in meters </summary>
@@ -2773,7 +2773,7 @@ public struct mavlink_rangefinder_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 48)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_airspeed_autocal_t
         {
             /// <summary> GPS velocity north m/s </summary>
@@ -2804,7 +2804,7 @@ public struct mavlink_airspeed_autocal_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 19)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rally_point_t
         {
             /// <summary> Latitude of point in degrees * 1E7 </summary>
@@ -2831,7 +2831,7 @@ public struct mavlink_rally_point_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rally_fetch_point_t
         {
             /// <summary> System ID </summary>
@@ -2844,7 +2844,7 @@ public struct mavlink_rally_fetch_point_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_compassmot_status_t
         {
             /// <summary> current (amps) </summary>
@@ -2863,7 +2863,7 @@ public struct mavlink_compassmot_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 24)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ahrs2_t
         {
             /// <summary> Roll angle (rad) </summary>
@@ -2882,7 +2882,7 @@ public struct mavlink_ahrs2_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 29)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_camera_status_t
         {
             /// <summary> Image timestamp (microseconds since UNIX epoch, according to camera clock) </summary>
@@ -2907,7 +2907,7 @@ public struct mavlink_camera_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 45)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_camera_feedback_t
         {
             /// <summary> Image timestamp (microseconds since UNIX epoch), as passed in by CAMERA_STATUS message (or autopilot if no CCB) </summary>
@@ -2940,7 +2940,7 @@ public struct mavlink_camera_feedback_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_battery2_t
         {
             /// <summary> voltage in millivolts </summary>
@@ -2951,7 +2951,7 @@ public struct mavlink_battery2_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 40)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ahrs3_t
         {
             /// <summary> Roll angle (rad) </summary>
@@ -2978,7 +2978,7 @@ public struct mavlink_ahrs3_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_autopilot_version_request_t
         {
             /// <summary> System ID </summary>
@@ -2989,7 +2989,7 @@ public struct mavlink_autopilot_version_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 206)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_remote_log_data_block_t
         {
             /// <summary> log data block sequence number </summary>
@@ -3005,7 +3005,7 @@ public struct mavlink_remote_log_data_block_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 7)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_remote_log_block_status_t
         {
             /// <summary> log data block sequence number </summary>
@@ -3020,7 +3020,7 @@ public struct mavlink_remote_log_block_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 29)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_led_control_t
         {
             /// <summary> System ID </summary>
@@ -3040,7 +3040,7 @@ public struct mavlink_led_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 27)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mag_cal_progress_t
         {
             /// <summary> Body frame direction vector for display </summary>
@@ -3066,7 +3066,7 @@ public struct mavlink_mag_cal_progress_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mag_cal_report_t
         {
             /// <summary> RMS milligauss residuals </summary>
@@ -3101,7 +3101,7 @@ public struct mavlink_mag_cal_report_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ekf_status_report_t
         {
             /// <summary> Velocity variance </summary>
@@ -3120,7 +3120,7 @@ public struct mavlink_ekf_status_report_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_pid_tuning_t
         {
             /// <summary> desired rate (degrees/s) </summary>
@@ -3141,7 +3141,7 @@ public struct mavlink_pid_tuning_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gimbal_report_t
         {
             /// <summary> Time since last update (seconds) </summary>
@@ -3172,7 +3172,7 @@ public struct mavlink_gimbal_report_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gimbal_control_t
         {
             /// <summary> Demanded angular rate X (rad/s) </summary>
@@ -3189,7 +3189,7 @@ public struct mavlink_gimbal_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gimbal_torque_cmd_report_t
         {
             /// <summary> Roll Torque Command </summary>
@@ -3206,7 +3206,7 @@ public struct mavlink_gimbal_torque_cmd_report_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gopro_heartbeat_t
         {
             /// <summary> Status </summary>
@@ -3219,7 +3219,7 @@ public struct mavlink_gopro_heartbeat_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gopro_get_request_t
         {
             /// <summary> System ID </summary>
@@ -3232,7 +3232,7 @@ public struct mavlink_gopro_get_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gopro_get_response_t
         {
             /// <summary> Command ID </summary>
@@ -3246,7 +3246,7 @@ public struct mavlink_gopro_get_response_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 7)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gopro_set_request_t
         {
             /// <summary> System ID </summary>
@@ -3262,7 +3262,7 @@ public struct mavlink_gopro_set_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gopro_set_response_t
         {
             /// <summary> Command ID </summary>
@@ -3273,7 +3273,7 @@ public struct mavlink_gopro_set_response_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rpm_t
         {
             /// <summary> RPM Sensor1 </summary>
@@ -3284,7 +3284,7 @@ public struct mavlink_rpm_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_heartbeat_t
         {
             /// <summary> A bitfield for use for autopilot-specific flags. </summary>
@@ -3303,7 +3303,7 @@ public struct mavlink_heartbeat_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 31)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_sys_status_t
         {
             /// <summary> Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR </summary>
@@ -3336,7 +3336,7 @@ public struct mavlink_sys_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_system_time_t
         {
             /// <summary> Timestamp of the master clock in microseconds since UNIX epoch. </summary>
@@ -3347,7 +3347,7 @@ public struct mavlink_system_time_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_ping_t
         {
             /// <summary> Unix timestamp in microseconds or since system boot if smaller than MAVLink epoch (1.1.2009) </summary>
@@ -3362,7 +3362,7 @@ public struct mavlink_ping_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_change_operator_control_t
         {
             /// <summary> System the GCS requests control for </summary>
@@ -3378,7 +3378,7 @@ public struct mavlink_change_operator_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_change_operator_control_ack_t
         {
             /// <summary> ID of the GCS this message  </summary>
@@ -3391,7 +3391,7 @@ public struct mavlink_change_operator_control_ack_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_auth_key_t
         {
             /// <summary> key </summary>
@@ -3401,7 +3401,7 @@ public struct mavlink_auth_key_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_mode_t
         {
             /// <summary> The new autopilot-specific mode. This field can be ignored by an autopilot. </summary>
@@ -3414,7 +3414,7 @@ public struct mavlink_set_mode_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_param_request_read_t
         {
             /// <summary> Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) </summary>
@@ -3430,7 +3430,7 @@ public struct mavlink_param_request_read_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_param_request_list_t
         {
             /// <summary> System ID </summary>
@@ -3441,7 +3441,7 @@ public struct mavlink_param_request_list_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_param_value_t
         {
             /// <summary> Onboard parameter value </summary>
@@ -3459,7 +3459,7 @@ public struct mavlink_param_value_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 23)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_param_set_t
         {
             /// <summary> Onboard parameter value </summary>
@@ -3477,7 +3477,7 @@ public struct mavlink_param_set_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps_raw_int_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -3504,7 +3504,7 @@ public struct mavlink_gps_raw_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 101)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps_status_t
         {
             /// <summary> Number of satellites visible </summary>
@@ -3528,7 +3528,7 @@ public struct mavlink_gps_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_imu_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3555,7 +3555,7 @@ public struct mavlink_scaled_imu_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_raw_imu_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -3582,7 +3582,7 @@ public struct mavlink_raw_imu_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_raw_pressure_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -3599,7 +3599,7 @@ public struct mavlink_raw_pressure_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_pressure_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3614,7 +3614,7 @@ public struct mavlink_scaled_pressure_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_attitude_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3635,7 +3635,7 @@ public struct mavlink_attitude_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_attitude_quaternion_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3658,7 +3658,7 @@ public struct mavlink_attitude_quaternion_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_local_position_ned_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3679,7 +3679,7 @@ public struct mavlink_local_position_ned_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_global_position_int_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3704,7 +3704,7 @@ public struct mavlink_global_position_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rc_channels_scaled_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3733,7 +3733,7 @@ public struct mavlink_rc_channels_scaled_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rc_channels_raw_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -3762,7 +3762,7 @@ public struct mavlink_rc_channels_raw_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 21)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_servo_output_raw_t
         {
             /// <summary> Timestamp (microseconds since system boot) </summary>
@@ -3789,7 +3789,7 @@ public struct mavlink_servo_output_raw_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_request_partial_list_t
         {
             /// <summary> Start index, 0 by default </summary>
@@ -3804,7 +3804,7 @@ public struct mavlink_mission_request_partial_list_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_write_partial_list_t
         {
             /// <summary> Start index, 0 by default and smaller / equal to the largest index of the current onboard list. </summary>
@@ -3819,7 +3819,7 @@ public struct mavlink_mission_write_partial_list_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_item_t
         {
             /// <summary> PARAM1, see MAV_CMD enum </summary>
@@ -3854,7 +3854,7 @@ public struct mavlink_mission_item_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_request_t
         {
             /// <summary> Sequence </summary>
@@ -3867,7 +3867,7 @@ public struct mavlink_mission_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_set_current_t
         {
             /// <summary> Sequence </summary>
@@ -3880,7 +3880,7 @@ public struct mavlink_mission_set_current_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_current_t
         {
             /// <summary> Sequence </summary>
@@ -3889,7 +3889,7 @@ public struct mavlink_mission_current_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_request_list_t
         {
             /// <summary> System ID </summary>
@@ -3900,7 +3900,7 @@ public struct mavlink_mission_request_list_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_count_t
         {
             /// <summary> Number of mission items in the sequence </summary>
@@ -3913,7 +3913,7 @@ public struct mavlink_mission_count_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_clear_all_t
         {
             /// <summary> System ID </summary>
@@ -3924,7 +3924,7 @@ public struct mavlink_mission_clear_all_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_item_reached_t
         {
             /// <summary> Sequence </summary>
@@ -3933,7 +3933,7 @@ public struct mavlink_mission_item_reached_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_ack_t
         {
             /// <summary> System ID </summary>
@@ -3946,7 +3946,7 @@ public struct mavlink_mission_ack_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_gps_global_origin_t
         {
             /// <summary> Latitude (WGS84), in degrees * 1E7 </summary>
@@ -3961,7 +3961,7 @@ public struct mavlink_set_gps_global_origin_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps_global_origin_t
         {
             /// <summary> Latitude (WGS84), in degrees * 1E7 </summary>
@@ -3974,7 +3974,7 @@ public struct mavlink_gps_global_origin_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_param_map_rc_t
         {
             /// <summary> Initial parameter value </summary>
@@ -4000,7 +4000,7 @@ public struct mavlink_param_map_rc_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_request_int_t
         {
             /// <summary> Sequence </summary>
@@ -4013,7 +4013,7 @@ public struct mavlink_mission_request_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 27)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_safety_set_allowed_area_t
         {
             /// <summary> x position 1 / Latitude 1 </summary>
@@ -4038,7 +4038,7 @@ public struct mavlink_safety_set_allowed_area_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 25)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_safety_allowed_area_t
         {
             /// <summary> x position 1 / Latitude 1 </summary>
@@ -4059,7 +4059,7 @@ public struct mavlink_safety_allowed_area_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 68)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_attitude_quaternion_cov_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -4080,7 +4080,7 @@ public struct mavlink_attitude_quaternion_cov_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_nav_controller_output_t
         {
             /// <summary> Current desired roll in degrees </summary>
@@ -4103,7 +4103,7 @@ public struct mavlink_nav_controller_output_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 185)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_global_position_int_cov_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. </summary>
@@ -4133,7 +4133,7 @@ public struct mavlink_global_position_int_cov_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 229)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_local_position_ned_cov_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch) in UTC. 0 for unknown. Commonly filled by the precision time source of a GPS receiver. </summary>
@@ -4167,7 +4167,7 @@ public struct mavlink_local_position_ned_cov_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rc_channels_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -4216,7 +4216,7 @@ public struct mavlink_rc_channels_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_request_data_stream_t
         {
             /// <summary> The requested message rate </summary>
@@ -4233,7 +4233,7 @@ public struct mavlink_request_data_stream_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data_stream_t
         {
             /// <summary> The message rate </summary>
@@ -4246,7 +4246,7 @@ public struct mavlink_data_stream_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 11)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_manual_control_t
         {
             /// <summary> X-axis, normalized to the range [-1000,1000]. A value of INT16_MAX indicates that this axis is invalid. Generally corresponds to forward(1000)-backward(-1000) movement on a joystick and the pitch of a vehicle. </summary>
@@ -4265,7 +4265,7 @@ public struct mavlink_manual_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_rc_channels_override_t
         {
             /// <summary> RC channel 1 value, in microseconds. A value of UINT16_MAX means to ignore this field. </summary>
@@ -4292,7 +4292,7 @@ public struct mavlink_rc_channels_override_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_mission_item_int_t
         {
             /// <summary> PARAM1, see MAV_CMD enum </summary>
@@ -4327,7 +4327,7 @@ public struct mavlink_mission_item_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_vfr_hud_t
         {
             /// <summary> Current airspeed in m/s </summary>
@@ -4346,7 +4346,7 @@ public struct mavlink_vfr_hud_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_command_int_t
         {
             /// <summary> PARAM1, see MAV_CMD enum </summary>
@@ -4379,7 +4379,7 @@ public struct mavlink_command_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 33)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_command_long_t
         {
             /// <summary> Parameter 1, as defined by MAV_CMD enum. </summary>
@@ -4408,7 +4408,7 @@ public struct mavlink_command_long_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 3)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_command_ack_t
         {
             /// <summary> Command ID, as defined by MAV_CMD enum. </summary>
@@ -4419,7 +4419,7 @@ public struct mavlink_command_ack_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_manual_setpoint_t
         {
             /// <summary> Timestamp in milliseconds since system boot </summary>
@@ -4440,7 +4440,7 @@ public struct mavlink_manual_setpoint_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 39)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_attitude_target_t
         {
             /// <summary> Timestamp in milliseconds since system boot </summary>
@@ -4466,7 +4466,7 @@ public struct mavlink_set_attitude_target_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 37)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_attitude_target_t
         {
             /// <summary> Timestamp in milliseconds since system boot </summary>
@@ -4489,7 +4489,7 @@ public struct mavlink_attitude_target_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_position_target_local_ned_t
         {
             /// <summary> Timestamp in milliseconds since system boot </summary>
@@ -4528,7 +4528,7 @@ public struct mavlink_set_position_target_local_ned_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_position_target_local_ned_t
         {
             /// <summary> Timestamp in milliseconds since system boot </summary>
@@ -4563,7 +4563,7 @@ public struct mavlink_position_target_local_ned_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_position_target_global_int_t
         {
             /// <summary> Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. </summary>
@@ -4602,7 +4602,7 @@ public struct mavlink_set_position_target_global_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_position_target_global_int_t
         {
             /// <summary> Timestamp in milliseconds since system boot. The rationale for the timestamp in the setpoint is to allow the system to compensate for the transport delay of the setpoint. This allows the system to compensate processing latency. </summary>
@@ -4637,7 +4637,7 @@ public struct mavlink_position_target_global_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 28)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_local_position_ned_system_global_offset_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -4658,7 +4658,7 @@ public struct mavlink_local_position_ned_system_global_offset_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 56)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_state_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -4697,7 +4697,7 @@ public struct mavlink_hil_state_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 42)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_controls_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -4727,7 +4727,7 @@ public struct mavlink_hil_controls_t
 
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 81)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_actuator_controls_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -4742,7 +4742,7 @@ public struct mavlink_hil_actuator_controls_t
 
         };
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 33)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_rc_inputs_raw_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -4777,7 +4777,7 @@ public struct mavlink_hil_rc_inputs_raw_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 26)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_optical_flow_t
         {
             /// <summary> Timestamp (UNIX) </summary>
@@ -4800,7 +4800,7 @@ public struct mavlink_optical_flow_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_global_vision_position_estimate_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4821,7 +4821,7 @@ public struct mavlink_global_vision_position_estimate_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_vision_position_estimate_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4842,7 +4842,7 @@ public struct mavlink_vision_position_estimate_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 20)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_vision_speed_estimate_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4857,7 +4857,7 @@ public struct mavlink_vision_speed_estimate_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_vicon_position_estimate_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4878,7 +4878,7 @@ public struct mavlink_vicon_position_estimate_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 62)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_highres_imu_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4915,7 +4915,7 @@ public struct mavlink_highres_imu_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_optical_flow_rad_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4946,7 +4946,7 @@ public struct mavlink_optical_flow_rad_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 64)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_sensor_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -4983,7 +4983,7 @@ public struct mavlink_hil_sensor_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 84)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_sim_state_t
         {
             /// <summary> True attitude quaternion component 1, w (1 in null-rotation) </summary>
@@ -5032,7 +5032,7 @@ public struct mavlink_sim_state_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_radio_status_t
         {
             /// <summary> Receive errors </summary>
@@ -5053,7 +5053,7 @@ public struct mavlink_radio_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 254)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_file_transfer_protocol_t
         {
             /// <summary> Network ID (0 for broadcast) </summary>
@@ -5069,7 +5069,7 @@ public struct mavlink_file_transfer_protocol_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 16)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_timesync_t
         {
             /// <summary> Time sync timestamp 1 </summary>
@@ -5080,7 +5080,7 @@ public struct mavlink_timesync_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_camera_trigger_t
         {
             /// <summary> Timestamp for the image frame in microseconds </summary>
@@ -5091,7 +5091,7 @@ public struct mavlink_camera_trigger_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_gps_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -5124,7 +5124,7 @@ public struct mavlink_hil_gps_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 44)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_optical_flow_t
         {
             /// <summary> Timestamp (microseconds, synced to UNIX time or since system boot) </summary>
@@ -5155,7 +5155,7 @@ public struct mavlink_hil_optical_flow_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 64)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_hil_state_quaternion_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -5195,7 +5195,7 @@ public struct mavlink_hil_state_quaternion_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_imu2_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -5222,7 +5222,7 @@ public struct mavlink_scaled_imu2_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_request_list_t
         {
             /// <summary> First log id (0 for first available) </summary>
@@ -5237,7 +5237,7 @@ public struct mavlink_log_request_list_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_entry_t
         {
             /// <summary> UTC timestamp of log in seconds since 1970, or 0 if not available </summary>
@@ -5254,7 +5254,7 @@ public struct mavlink_log_entry_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_request_data_t
         {
             /// <summary> Offset into the log </summary>
@@ -5271,7 +5271,7 @@ public struct mavlink_log_request_data_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 97)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_data_t
         {
             /// <summary> Offset into the log </summary>
@@ -5287,7 +5287,7 @@ public struct mavlink_log_data_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_erase_t
         {
             /// <summary> System ID </summary>
@@ -5298,7 +5298,7 @@ public struct mavlink_log_erase_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_log_request_end_t
         {
             /// <summary> System ID </summary>
@@ -5309,7 +5309,7 @@ public struct mavlink_log_request_end_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 113)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps_inject_data_t
         {
             /// <summary> System ID </summary>
@@ -5325,7 +5325,7 @@ public struct mavlink_gps_inject_data_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps2_raw_t
         {
             /// <summary> Timestamp (microseconds since UNIX epoch or microseconds since system boot) </summary>
@@ -5356,7 +5356,7 @@ public struct mavlink_gps2_raw_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_power_status_t
         {
             /// <summary> 5V rail voltage in millivolts </summary>
@@ -5369,7 +5369,7 @@ public struct mavlink_power_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 79)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_serial_control_t
         {
             /// <summary> Baudrate of transfer. Zero means no change. </summary>
@@ -5389,7 +5389,7 @@ public struct mavlink_serial_control_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps_rtk_t
         {
             /// <summary> Time since boot of last baseline message received in ms. </summary>
@@ -5422,7 +5422,7 @@ public struct mavlink_gps_rtk_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 35)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_gps2_rtk_t
         {
             /// <summary> Time since boot of last baseline message received in ms. </summary>
@@ -5455,7 +5455,7 @@ public struct mavlink_gps2_rtk_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_imu3_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -5482,7 +5482,7 @@ public struct mavlink_scaled_imu3_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 13)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_data_transmission_handshake_t
         {
             /// <summary> total data size in bytes (set on ACK only) </summary>
@@ -5503,7 +5503,7 @@ public struct mavlink_data_transmission_handshake_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 255)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_encapsulated_data_t
         {
             /// <summary> sequence number (starting with 0 on every transmission) </summary>
@@ -5515,7 +5515,7 @@ public struct mavlink_encapsulated_data_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_distance_sensor_t
         {
             /// <summary> Time since system boot </summary>
@@ -5538,7 +5538,7 @@ public struct mavlink_distance_sensor_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_terrain_request_t
         {
             /// <summary> Bitmask of requested 4x4 grids (row major 8x7 array of grids, 56 bits) </summary>
@@ -5553,7 +5553,7 @@ public struct mavlink_terrain_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 43)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_terrain_data_t
         {
             /// <summary> Latitude of SW corner of first grid (degrees *10^7) </summary>
@@ -5571,7 +5571,7 @@ public struct mavlink_terrain_data_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 8)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_terrain_check_t
         {
             /// <summary> Latitude (degrees *10^7) </summary>
@@ -5582,7 +5582,7 @@ public struct mavlink_terrain_check_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 22)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_terrain_report_t
         {
             /// <summary> Latitude (degrees *10^7) </summary>
@@ -5603,7 +5603,7 @@ public struct mavlink_terrain_report_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_pressure2_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -5618,7 +5618,7 @@ public struct mavlink_scaled_pressure2_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_att_pos_mocap_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5636,7 +5636,7 @@ public struct mavlink_att_pos_mocap_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 43)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_actuator_control_target_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5654,7 +5654,7 @@ public struct mavlink_set_actuator_control_target_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 41)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_actuator_control_target_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5668,7 +5668,7 @@ public struct mavlink_actuator_control_target_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_altitude_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -5689,7 +5689,7 @@ public struct mavlink_altitude_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 243)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_resource_request_t
         {
             /// <summary> Request ID. This ID should be re-used when sending back URI contents </summary>
@@ -5708,7 +5708,7 @@ public struct mavlink_resource_request_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 14)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_scaled_pressure3_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -5723,7 +5723,7 @@ public struct mavlink_scaled_pressure3_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 100)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_control_system_state_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5767,7 +5767,7 @@ public struct mavlink_control_system_state_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_battery_status_t
         {
             /// <summary> Consumed charge, in milliampere hours (1 = 1 mAh), -1: autopilot does not provide mAh consumption estimate </summary>
@@ -5793,7 +5793,7 @@ public struct mavlink_battery_status_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 60)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_autopilot_version_t
         {
             /// <summary> bitmask of capabilities (see MAV_PROTOCOL_CAPABILITY enum) </summary>
@@ -5825,7 +5825,7 @@ public struct mavlink_autopilot_version_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_landing_target_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5848,7 +5848,7 @@ public struct mavlink_landing_target_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 32)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_vibration_t
         {
             /// <summary> Timestamp (micros since boot or Unix epoch) </summary>
@@ -5869,7 +5869,7 @@ public struct mavlink_vibration_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 52)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_home_position_t
         {
             /// <summary> Latitude (WGS84), in degrees * 1E7 </summary>
@@ -5898,7 +5898,7 @@ public struct mavlink_home_position_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 53)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_set_home_position_t
         {
             /// <summary> Latitude (WGS84), in degrees * 1E7 </summary>
@@ -5929,7 +5929,7 @@ public struct mavlink_set_home_position_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 6)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_message_interval_t
         {
             /// <summary> The interval between two messages, in microseconds. A value of -1 indicates this stream is disabled, 0 indicates it is not available, > 0 indicates the interval at which it is sent. </summary>
@@ -5940,7 +5940,7 @@ public struct mavlink_message_interval_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 2)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_extended_sys_state_t
         {
             /// <summary> The VTOL state if applicable. Is set to MAV_VTOL_STATE_UNDEFINED if UAV is not in VTOL configuration. </summary>
@@ -5951,7 +5951,7 @@ public struct mavlink_extended_sys_state_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 38)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_adsb_vehicle_t
         {
             /// <summary> ICAO address </summary>
@@ -6006,7 +6006,7 @@ public struct mavlink_collision_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 254)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_v2_extension_t
         {
             /// <summary> A code that identifies the software component that understands this message (analogous to usb device classes or mime type strings).  If this code is less than 32768, it is considered a 'registered' protocol extension and the corresponding entry should be added to https://github.com/mavlink/mavlink/extension-message-ids.xml.  Software creators can register blocks of message IDs as needed (useful for GCS specific metadata, etc...). Message_types greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. </summary>
@@ -6024,7 +6024,7 @@ public struct mavlink_v2_extension_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 36)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_memory_vect_t
         {
             /// <summary> Starting address of the debug variables </summary>
@@ -6040,7 +6040,7 @@ public struct mavlink_memory_vect_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 30)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_debug_vect_t
         {
             /// <summary> Timestamp </summary>
@@ -6058,7 +6058,7 @@ public struct mavlink_debug_vect_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_named_value_float_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -6072,7 +6072,7 @@ public struct mavlink_named_value_float_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 18)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_named_value_int_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -6086,7 +6086,7 @@ public struct mavlink_named_value_int_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 51)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_statustext_t
         {
             /// <summary> Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. </summary>
@@ -6098,7 +6098,7 @@ public struct mavlink_statustext_t
         };
 
 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 9)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_debug_t
         {
             /// <summary> Timestamp (milliseconds since system boot) </summary>
@@ -6111,7 +6111,7 @@ public struct mavlink_debug_t
         };
 
         // custom message from the simulator 
-        [StructLayout(LayoutKind.Sequential, Pack = 1, Size = 12*4)]
+        [StructLayout(LayoutKind.Sequential)]
         public struct mavlink_telemetry
         {
             public const int MessageId = 204;
