Customizing distance sensor to add orientation for multiple sensor support

examples/distance_sensor/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.10.2)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+project(distance_sensor)
+
+add_executable(distance_sensor
+    distance_sensor.cpp
+)
+
+find_package(MAVSDK REQUIRED)
+
+target_link_libraries(distance_sensor
+    MAVSDK::mavsdk
+)
+
+if(NOT MSVC)
+    add_compile_options(distance_sensor PRIVATE -Wall -Wextra)
+else()
+    add_compile_options(distance_sensor PRIVATE -WX -W2)
+endif()

examples/distance_sensor/distance_sensor.cpp

@@ -0,0 +1,74 @@
+//
+// Simple example to demonstrate how to imitate that all distance sensor
+// readings are read from all orientations.
+//
+// Author: Julian Oes <julian@oes.ch>
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/mavlink_passthrough/mavlink_passthrough.h>
+#include <mavsdk/plugins/telemetry/telemetry.h>
+#include <chrono>
+#include <cstdint>
+#include <iostream>
+#include <future>
+#include <memory>
+#include <thread>
+
+using namespace mavsdk;
+using std::chrono::seconds;
+
+static void subscribe_distance_sensor(Telemetry& telemetry);
+
+void usage(const std::string& bin_name)
+{
+    std::cerr << "Usage : " << bin_name << " <connection_url>\n"
+              << "Connection URL format should be :\n"
+              << " For TCP : tcp://[server_host][:server_port]\n"
+              << " For UDP : udp://[bind_host][:bind_port]\n"
+              << " For Serial : serial:///path/to/serial/dev[:baudrate]\n"
+              << "For example, to connect to the simulator use URL: udp://:14540\n";
+}
+
+int main(int argc, char** argv)
+{
+    if (argc != 2) {
+        usage(argv[0]);
+        return 1;
+    }
+
+    Mavsdk mavsdk;
+    ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);
+
+    if (connection_result != ConnectionResult::Success) {
+        std::cerr << "Connection failed: " << connection_result << '\n';
+        return 1;
+    }
+
+    auto system = mavsdk.first_autopilot(3.0);
+    if (!system) {
+        std::cerr << "Timed out waiting for system\n";
+        return 1;
+    }
+
+    // Instantiate plugins.
+    auto telemetry = Telemetry{system.value()};
+
+    // subscribe to distance sensor readings
+    subscribe_distance_sensor(telemetry);
+
+    // endless loop so that the sensor readings will keep comming
+    // will close only with kill or Ctrl+C
+    while (true) {
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    return 0;
+}
+
+void subscribe_distance_sensor(Telemetry& telemetry)
+{
+    telemetry.subscribe_distance_sensor([](Telemetry::DistanceSensor dist_sensor) {
+        std::cout << "Distance sensor dist: " << dist_sensor.current_distance_m
+                  << " - orient: " << dist_sensor.orientation << '\n';
+    });
+}

src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h

@@ -664,6 +664,7 @@ class Telemetry : public PluginBase {
             float(NAN)}; /**< @brief Maximum distance the sensor can measure, NaN if unknown. */
         float current_distance_m{
             float(NAN)}; /**< @brief Current distance reading, NaN if unknown. */
+        EulerAngle orientation{}; /**< @brief Sensor Orientation reading. */
     };
 
     /**

src/mavsdk/plugins/telemetry/telemetry.cpp

@@ -1236,7 +1236,8 @@ bool operator==(const Telemetry::DistanceSensor& lhs, const Telemetry::DistanceS
            ((std::isnan(rhs.maximum_distance_m) && std::isnan(lhs.maximum_distance_m)) ||
             rhs.maximum_distance_m == lhs.maximum_distance_m) &&
            ((std::isnan(rhs.current_distance_m) && std::isnan(lhs.current_distance_m)) ||
-            rhs.current_distance_m == lhs.current_distance_m);
+            rhs.current_distance_m == lhs.current_distance_m) &&
+           (rhs.orientation == lhs.orientation);
 }
 
 std::ostream& operator<<(std::ostream& str, Telemetry::DistanceSensor const& distance_sensor)
@@ -1246,6 +1247,7 @@ std::ostream& operator<<(std::ostream& str, Telemetry::DistanceSensor const& dis
     str << "    minimum_distance_m: " << distance_sensor.minimum_distance_m << '\n';
     str << "    maximum_distance_m: " << distance_sensor.maximum_distance_m << '\n';
     str << "    current_distance_m: " << distance_sensor.current_distance_m << '\n';
+    str << "    orientation: " << distance_sensor.orientation << '\n';
     str << '}';
     return str;
 }

src/mavsdk/plugins/telemetry/telemetry_impl.cpp

@@ -1464,6 +1464,7 @@ void TelemetryImpl::process_distance_sensor(const mavlink_message_t& message)
         static_cast<float>(distance_sensor_msg.max_distance) * 1e-2f; // cm to m
     distance_sensor_struct.current_distance_m =
         static_cast<float>(distance_sensor_msg.current_distance) * 1e-2f; // cm to m
+    distance_sensor_struct.orientation = extractOrientation(distance_sensor_msg);
 
     set_distance_sensor(distance_sensor_struct);
 
@@ -1472,6 +1473,216 @@ void TelemetryImpl::process_distance_sensor(const mavlink_message_t& message)
         distance_sensor(), [this](const auto& func) { _system_impl->call_user_callback(func); });
 }
 
+Telemetry::EulerAngle
+TelemetryImpl::extractOrientation(mavlink_distance_sensor_t distance_sensor_msg)
+{
+    MavSensorOrientation orientation =
+        static_cast<MavSensorOrientation>(distance_sensor_msg.orientation);
+
+    Telemetry::EulerAngle euler_angle;
+    euler_angle.roll_deg = 0;
+    euler_angle.pitch_deg = 0;
+    euler_angle.yaw_deg = 0;
+
+    switch (orientation) {
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_45: {
+            euler_angle.yaw_deg = 45;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_90: {
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_135: {
+            euler_angle.yaw_deg = 135;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_180: {
+            euler_angle.yaw_deg = 180;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_225: {
+            euler_angle.yaw_deg = 225;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_270: {
+            euler_angle.yaw_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_YAW_315: {
+            euler_angle.yaw_deg = 315;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180: {
+            euler_angle.roll_deg = 180;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_45: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 45;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_90: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_135: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 135;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_180: {
+            euler_angle.pitch_deg = 180;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_225: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 225;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_270: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_YAW_315: {
+            euler_angle.roll_deg = 180;
+            euler_angle.yaw_deg = 315;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90: {
+            euler_angle.roll_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_YAW_45: {
+            euler_angle.roll_deg = 90;
+            euler_angle.yaw_deg = 45;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_YAW_90: {
+            euler_angle.roll_deg = 90;
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_YAW_135: {
+            euler_angle.roll_deg = 90;
+            euler_angle.yaw_deg = 135;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270: {
+            euler_angle.roll_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_YAW_45: {
+            euler_angle.roll_deg = 270;
+            euler_angle.yaw_deg = 45;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_YAW_90: {
+            euler_angle.roll_deg = 270;
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_YAW_135: {
+            euler_angle.roll_deg = 270;
+            euler_angle.yaw_deg = 135;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_90: {
+            euler_angle.pitch_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_270: {
+            euler_angle.pitch_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_180_YAW_90: {
+            euler_angle.pitch_deg = 180;
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_180_YAW_270: {
+            euler_angle.pitch_deg = 180;
+            euler_angle.yaw_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_90: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_PITCH_90: {
+            euler_angle.roll_deg = 180;
+            euler_angle.pitch_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_PITCH_90: {
+            euler_angle.roll_deg = 270;
+            euler_angle.pitch_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_180: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 180;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_PITCH_180: {
+            euler_angle.roll_deg = 270;
+            euler_angle.pitch_deg = 180;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_270: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_180_PITCH_270: {
+            euler_angle.roll_deg = 180;
+            euler_angle.pitch_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_270_PITCH_270: {
+            euler_angle.roll_deg = 270;
+            euler_angle.pitch_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_180_YAW_90: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 180;
+            euler_angle.yaw_deg = 90;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_YAW_270: {
+            euler_angle.roll_deg = 90;
+            euler_angle.yaw_deg = 270;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 68;
+            euler_angle.yaw_deg = 293;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_PITCH_315: {
+            euler_angle.pitch_deg = 315;
+            break;
+        }
+        case MavSensorOrientation::MAV_SENSOR_ROTATION_ROLL_90_PITCH_315: {
+            euler_angle.roll_deg = 90;
+            euler_angle.pitch_deg = 315;
+            break;
+        }
+        default: {
+            euler_angle.roll_deg = 0;
+            euler_angle.pitch_deg = 0;
+            euler_angle.yaw_deg = 0;
+        }
+    }
+
+    return euler_angle;
+}
+
 void TelemetryImpl::process_scaled_pressure(const mavlink_message_t& message)
 {
     mavlink_scaled_pressure_t scaled_pressure_msg;