diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
index e365c5eec6..68ce32760a 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
@@ -324,27 +324,6 @@ class ArduCopterApi : public MultirotorApiBase {
         udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
     }
 
-    const GpsBase* getGps() const
-    {
-        return static_cast<const GpsBase*>(sensors_->getByType(SensorBase::SensorType::Gps));
-    }
-    const ImuBase* getImu() const
-    {
-        return static_cast<const ImuBase*>(sensors_->getByType(SensorBase::SensorType::Imu));
-    }
-    const MagnetometerBase* getMagnetometer() const
-    {
-        return static_cast<const MagnetometerBase*>(sensors_->getByType(SensorBase::SensorType::Magnetometer));
-    }
-    const BarometerBase* getBarometer() const
-    {
-        return static_cast<const BarometerBase*>(sensors_->getByType(SensorBase::SensorType::Barometer));
-    }
-    const LidarBase* getLidar() const
-    {
-        return static_cast<const LidarBase*>(sensors_->getByType(SensorBase::SensorType::Lidar));
-    }
-
 private:
     virtual void normalizeRotorControls()
     {
@@ -359,10 +338,8 @@ class ArduCopterApi : public MultirotorApiBase {
         if (sensors_ == nullptr)
             return;
 
-        const auto& gps_output = getGps()->getOutput();
-        const auto& imu_output = getImu()->getOutput();
-        // const auto& baro_output = getBarometer()->getOutput();
-        // const auto& mag_output = getMagnetometer()->getOutput();
+        const auto& gps_output = getGpsData("");
+        const auto& imu_output = getImuData("");
 
         std::ostringstream oss;
 
@@ -382,11 +359,11 @@ class ArduCopterApi : public MultirotorApiBase {
                 << "]}";
         }
 
-        const auto lidar = getLidar();
+        const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
         // TODO: Add bool value in settings to check whether to send lidar data or not
         // Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic)
-        if (lidar != nullptr) {
-            const auto& lidar_output = lidar->getOutput();
+        if (count_lidars != 0) {
+            const auto& lidar_output = getLidarData("");
             oss << ","
                    "\"lidar\": {"
                    "\"point_cloud\": [";
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
index 8b8c06d91a..d37e9d1803 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
@@ -22,12 +22,10 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
 			return;
 
 		// send GPS and other sensor updates
-		const auto gps = getGps();
-		if (gps != nullptr) {
-			const auto& gps_output = gps->getOutput();
-			const auto& imu_output = getImu()->getOutput();
-			//                    const auto& mag_output = getMagnetometer()->getOutput();
-			//                    const auto& baro_output = getBarometer()->getOutput();
+        const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
+		if (count_gps_sensors != 0) {
+            const auto& gps_output = getGpsData("");
+            const auto& imu_output = getImuData("");
 
 			SensorMessage packet;
 			packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000;
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
index 1e7a9bb823..a54a9f6b25 100644
--- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
+++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -104,9 +104,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             }
 
             //send sensor updates
-            const auto& imu_output = getImu()->getOutput();
-            const auto& mag_output = getMagnetometer()->getOutput();
-            const auto& baro_output = getBarometer()->getOutput();
+            const auto& imu_output = getImuData("");
+            const auto& mag_output = getMagnetometerData("");
+            const auto& baro_output = getBarometerData("");
 
             sendHILSensor(imu_output.linear_acceleration,
                 imu_output.angular_velocity,
@@ -114,9 +114,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                 baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
 
 
-            const auto * distance = getDistance();
-            if (distance) {
-                const auto& distance_output = distance->getOutput();
+            const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
+            if (count_distance_sensors != 0) {
+                const auto& distance_output = getDistanceSensorData("");
                 float pitch, roll, yaw;
                 VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);
 
@@ -128,9 +128,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                     pitch); //TODO: convert from radians to degrees?
             }
 
-            const auto gps = getGps();
-            if (gps != nullptr) {
-                const auto& gps_output = gps->getOutput();
+            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_) {
@@ -669,27 +669,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         disconnect();
     }
 
-    const ImuBase* getImu() const
-    {
-        return static_cast<const ImuBase*>(sensors_->getByType(SensorBase::SensorType::Imu));
-    }
-    const MagnetometerBase* getMagnetometer() const
-    {
-        return static_cast<const MagnetometerBase*>(sensors_->getByType(SensorBase::SensorType::Magnetometer));
-    }
-    const BarometerBase* getBarometer() const
-    {
-        return static_cast<const BarometerBase*>(sensors_->getByType(SensorBase::SensorType::Barometer));
-    }
-    const DistanceBase* getDistance() const
-    {
-        return static_cast<const DistanceBase*>(sensors_->getByType(SensorBase::SensorType::Distance));
-    }
-    const GpsBase* getGps() const
-    {
-        return static_cast<const GpsBase*>(sensors_->getByType(SensorBase::SensorType::Gps));
-    }
-
     void closeAllConnection()
     {
         close();