added vtol state implementation

src/mavsdk_server/src/plugins/telemetry/telemetry_service_impl.h

@@ -259,6 +259,46 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
         }
     }
 
+    static rpc::telemetry::VtolState
+    translateToRpcVtolState(const mavsdk::Telemetry::VtolState& vtol_state)
+    {
+        switch (vtol_state) {
+            default:
+                LogErr() << "Unknown vtol_state enum value: " << static_cast<int>(vtol_state);
+            // FALLTHROUGH
+            case mavsdk::Telemetry::VtolState::Undefined:
+                return rpc::telemetry::VTOL_STATE_UNDEFINED;
+            case mavsdk::Telemetry::VtolState::TransitionToFw:
+                return rpc::telemetry::VTOL_STATE_TRANSITION_TO_FW;
+            case mavsdk::Telemetry::VtolState::TransitionToMc:
+                return rpc::telemetry::VTOL_STATE_TRANSITION_TO_MC;
+            case mavsdk::Telemetry::VtolState::Mc:
+                return rpc::telemetry::VTOL_STATE_MC;
+            case mavsdk::Telemetry::VtolState::Fw:
+                return rpc::telemetry::VTOL_STATE_FW;
+        }
+    }
+
+    static mavsdk::Telemetry::VtolState
+    translateFromRpcVtolState(const rpc::telemetry::VtolState vtol_state)
+    {
+        switch (vtol_state) {
+            default:
+                LogErr() << "Unknown vtol_state enum value: " << static_cast<int>(vtol_state);
+            // FALLTHROUGH
+            case rpc::telemetry::VTOL_STATE_UNDEFINED:
+                return mavsdk::Telemetry::VtolState::Undefined;
+            case rpc::telemetry::VTOL_STATE_TRANSITION_TO_FW:
+                return mavsdk::Telemetry::VtolState::TransitionToFw;
+            case rpc::telemetry::VTOL_STATE_TRANSITION_TO_MC:
+                return mavsdk::Telemetry::VtolState::TransitionToMc;
+            case rpc::telemetry::VTOL_STATE_MC:
+                return mavsdk::Telemetry::VtolState::Mc;
+            case rpc::telemetry::VTOL_STATE_FW:
+                return mavsdk::Telemetry::VtolState::Fw;
+        }
+    }
+
     static std::unique_ptr<rpc::telemetry::Position>
     translateToRpcPosition(const mavsdk::Telemetry::Position& position)
     {
@@ -1461,6 +1501,46 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
         return grpc::Status::OK;
     }
 
+    grpc::Status SubscribeVtolState(
+        grpc::ServerContext* /* context */,
+        const mavsdk::rpc::telemetry::SubscribeVtolStateRequest* /* request */,
+        grpc::ServerWriter<rpc::telemetry::VtolStateResponse>* writer) override
+    {
+        if (_lazy_plugin.maybe_plugin() == nullptr) {
+            return grpc::Status::OK;
+        }
+
+        auto stream_closed_promise = std::make_shared<std::promise<void>>();
+        auto stream_closed_future = stream_closed_promise->get_future();
+        register_stream_stop_promise(stream_closed_promise);
+
+        auto is_finished = std::make_shared<bool>(false);
+        auto subscribe_mutex = std::make_shared<std::mutex>();
+
+        _lazy_plugin.maybe_plugin()->subscribe_vtol_state(
+            [this, &writer, &stream_closed_promise, is_finished, subscribe_mutex](
+                const mavsdk::Telemetry::VtolState vtol_state) {
+                rpc::telemetry::VtolStateResponse rpc_response;
+
+                rpc_response.set_vtol_state(translateToRpcVtolState(vtol_state));
+
+                std::unique_lock<std::mutex> lock(*subscribe_mutex);
+                if (!*is_finished && !writer->Write(rpc_response)) {
+                    _lazy_plugin.maybe_plugin()->subscribe_vtol_state(nullptr);
+
+                    *is_finished = true;
+                    unregister_stream_stop_promise(stream_closed_promise);
+                    stream_closed_promise->set_value();
+                }
+            });
+
+        stream_closed_future.wait();
+        std::unique_lock<std::mutex> lock(*subscribe_mutex);
+        *is_finished = true;
+
+        return grpc::Status::OK;
+    }
+
     grpc::Status SubscribeAttitudeQuaternion(
         grpc::ServerContext* /* context */,
         const mavsdk::rpc::telemetry::SubscribeAttitudeQuaternionRequest* /* request */,
@@ -2667,6 +2747,34 @@ class TelemetryServiceImpl final : public rpc::telemetry::TelemetryService::Serv
         return grpc::Status::OK;
     }
 
+    grpc::Status SetRateVtolState(
+        grpc::ServerContext* /* context */,
+        const rpc::telemetry::SetRateVtolStateRequest* request,
+        rpc::telemetry::SetRateVtolStateResponse* response) override
+    {
+        if (_lazy_plugin.maybe_plugin() == nullptr) {
+            if (response != nullptr) {
+                auto result = mavsdk::Telemetry::Result::NoSystem;
+                fillResponseWithResult(response, result);
+            }
+
+            return grpc::Status::OK;
+        }
+
+        if (request == nullptr) {
+            LogWarn() << "SetRateVtolState sent with a null request! Ignoring...";
+            return grpc::Status::OK;
+        }
+
+        auto result = _lazy_plugin.maybe_plugin()->set_rate_vtol_state(request->rate_hz());
+
+        if (response != nullptr) {
+            fillResponseWithResult(response, result);
+        }
+
+        return grpc::Status::OK;
+    }
+
     grpc::Status SetRateAttitude(
         grpc::ServerContext* /* context */,
         const rpc::telemetry::SetRateAttitudeRequest* request,

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

@@ -148,6 +148,24 @@ class Telemetry : public PluginBase {
      */
     friend std::ostream& operator<<(std::ostream& str, Telemetry::LandedState const& landed_state);
 
+    /**
+     * @brief VTOL State enumeration
+     */
+    enum class VtolState {
+        Undefined, /**< @brief MAV is not configured as VTOL. */
+        TransitionToFw, /**< @brief VTOL is in transition from multicopter to fixed-wing. */
+        TransitionToMc, /**< @brief VTOL is in transition from fixed-wing to multicopter. */
+        Mc, /**< @brief VTOL is in multicopter state. */
+        Fw, /**< @brief VTOL is in fixed-wing state. */
+    };
+
+    /**
+     * @brief Stream operator to print information about a `Telemetry::VtolState`.
+     *
+     * @return A reference to the stream.
+     */
+    friend std::ostream& operator<<(std::ostream& str, Telemetry::VtolState const& vtol_state);
+
     /**
      * @brief Position type in global coordinates.
      */
@@ -1053,6 +1071,24 @@ class Telemetry : public PluginBase {
      */
     bool armed() const;
 
+    /**
+     * @brief Callback type for subscribe_vtol_state.
+     */
+
+    using VtolStateCallback = std::function<void(VtolState)>;
+
+    /**
+     * @brief subscribe to vtol state Updates
+     */
+    void subscribe_vtol_state(VtolStateCallback callback);
+
+    /**
+     * @brief Poll for 'VtolState' (blocking).
+     *
+     * @return One VtolState update.
+     */
+    VtolState vtol_state() const;
+
     /**
      * @brief Callback type for subscribe_attitude_quaternion.
      */
@@ -1604,6 +1640,22 @@ class Telemetry : public PluginBase {
      */
     Result set_rate_landed_state(double rate_hz) const;
 
+    /**
+     * @brief Set rate to VTOL state updates
+     *
+     * This function is non-blocking. See 'set_rate_vtol_state' for the blocking counterpart.
+     */
+    void set_rate_vtol_state_async(double rate_hz, const ResultCallback callback);
+
+    /**
+     * @brief Set rate to VTOL state updates
+     *
+     * This function is blocking. See 'set_rate_vtol_state_async' for the non-blocking counterpart.
+     *
+     * @return Result of request.
+     */
+    Result set_rate_vtol_state(double rate_hz) const;
+
     /**
      * @brief Set rate to 'attitude' updates.
      *

src/plugins/telemetry/telemetry.cpp

@@ -99,6 +99,16 @@ bool Telemetry::armed() const
     return _impl->armed();
 }
 
+void Telemetry::subscribe_vtol_state(VtolStateCallback callback)
+{
+    _impl->subscribe_vtol_state(callback);
+}
+
+Telemetry::VtolState Telemetry::vtol_state() const
+{
+    return _impl->vtol_state();
+}
+
 void Telemetry::subscribe_attitude_quaternion(AttitudeQuaternionCallback callback)
 {
     _impl->subscribe_attitude_quaternion(callback);
@@ -410,6 +420,16 @@ Telemetry::Result Telemetry::set_rate_landed_state(double rate_hz) const
     return _impl->set_rate_landed_state(rate_hz);
 }
 
+void Telemetry::set_rate_vtol_state_async(double rate_hz, const ResultCallback callback)
+{
+    _impl->set_rate_vtol_state_async(rate_hz, callback);
+}
+
+Telemetry::Result Telemetry::set_rate_vtol_state(double rate_hz) const
+{
+    return _impl->set_rate_vtol_state(rate_hz);
+}
+
 void Telemetry::set_rate_attitude_async(double rate_hz, const ResultCallback callback)
 {
     _impl->set_rate_attitude_async(rate_hz, callback);
@@ -1369,4 +1389,22 @@ std::ostream& operator<<(std::ostream& str, Telemetry::LandedState const& landed
     }
 }
 
+std::ostream& operator<<(std::ostream& str, Telemetry::VtolState const& vtol_state)
+{
+    switch (vtol_state) {
+        case Telemetry::VtolState::Undefined:
+            return str << "Undefined";
+        case Telemetry::VtolState::TransitionToFw:
+            return str << "Transition To Fw";
+        case Telemetry::VtolState::TransitionToMc:
+            return str << "Transition To Mc";
+        case Telemetry::VtolState::Mc:
+            return str << "Mc";
+        case Telemetry::VtolState::Fw:
+            return str << "Fw";
+        default:
+            return str << "Unknown";
+    }
+}
+
 } // namespace mavsdk

src/plugins/telemetry/telemetry_impl.cpp

@@ -201,6 +201,11 @@ Telemetry::Result TelemetryImpl::set_rate_in_air(double rate_hz)
     return set_rate_landed_state(rate_hz);
 }
 
+Telemetry::Result TelemetryImpl::set_rate_vtol_state(double rate_hz)
+{
+    return set_rate_landed_state(rate_hz);
+}
+
 Telemetry::Result TelemetryImpl::set_rate_landed_state(double rate_hz)
 {
     return telemetry_result_from_command_result(
@@ -346,6 +351,11 @@ void TelemetryImpl::set_rate_in_air_async(double rate_hz, Telemetry::ResultCallb
     set_rate_landed_state_async(rate_hz, callback);
 }
 
+void TelemetryImpl::set_rate_vtol_state_async(double rate_hz, Telemetry::ResultCallback callback)
+{
+    set_rate_landed_state_async(rate_hz, callback);
+}
+
 void TelemetryImpl::set_rate_landed_state_async(double rate_hz, Telemetry::ResultCallback callback)
 {
     _parent->set_msg_rate_async(
@@ -956,6 +966,9 @@ void TelemetryImpl::process_extended_sys_state(const mavlink_message_t& message)
     {
         Telemetry::LandedState landed_state = to_landed_state(extended_sys_state);
         set_landed_state(landed_state);
+
+        Telemetry::VtolState vtol_state = to_vtol_state(extended_sys_state);
+        set_vtol_state(vtol_state);
     }
 
     std::lock_guard<std::mutex> lock(_subscription_mutex);
@@ -965,6 +978,12 @@ void TelemetryImpl::process_extended_sys_state(const mavlink_message_t& message)
         _parent->call_user_callback([callback, arg]() { callback(arg); });
     }
 
+    if (_votl_state_subscription) {
+        auto callback = _votl_state_subscription;
+        auto arg = vtol_state();
+        _parent->call_user_callback([callback, arg]() { callback(arg); });
+    }
+
     if (extended_sys_state.landed_state == MAV_LANDED_STATE_IN_AIR ||
         extended_sys_state.landed_state == MAV_LANDED_STATE_TAKEOFF ||
         extended_sys_state.landed_state == MAV_LANDED_STATE_LANDING) {
@@ -1366,6 +1385,23 @@ TelemetryImpl::to_landed_state(mavlink_extended_sys_state_t extended_sys_state)
     }
 }
 
+Telemetry::VtolState
+TelemetryImpl::to_vtol_state(mavlink_extended_sys_state_t extended_sys_state)
+{
+    switch (extended_sys_state.vtol_state) {
+        case MAV_VTOL_STATE::MAV_VTOL_STATE_TRANSITION_TO_FW:
+            return Telemetry::VtolState::TransitionToFw;
+        case MAV_VTOL_STATE_TRANSITION_TO_MC:
+            return Telemetry::VtolState::TransitionToMc;
+        case MAV_VTOL_STATE_MC:
+            return Telemetry::VtolState::Mc;
+        case MAV_VTOL_STATE_FW:
+            return Telemetry::VtolState::Fw;
+        default:
+            return Telemetry::VtolState::Undefined;
+    }
+}
+
 Telemetry::FlightMode
 TelemetryImpl::telemetry_flight_mode_from_flight_mode(SystemImpl::FlightMode flight_mode)
 {
@@ -1819,6 +1855,18 @@ void TelemetryImpl::set_health_armable(bool ok)
     _health.is_armable = ok;
 }
 
+Telemetry::VtolState TelemetryImpl::vtol_state() const 
+{
+    std::lock_guard<std::mutex> lock(_vtol_state_mutex);
+    return _vtol_state;
+}
+
+void TelemetryImpl::set_vtol_state(Telemetry::VtolState vtol_state) 
+{
+    std::lock_guard<std::mutex> lock(_vtol_state_mutex);
+    _vtol_state = vtol_state;
+}
+
 Telemetry::LandedState TelemetryImpl::landed_state() const
 {
     std::lock_guard<std::mutex> lock(_landed_state_mutex);
@@ -2027,6 +2075,12 @@ void TelemetryImpl::subscribe_health_all_ok(Telemetry::HealthAllOkCallback& call
     _health_all_ok_subscription = callback;
 }
 
+void TelemetryImpl::subscribe_vtol_state(Telemetry::VtolStateCallback& callback)
+{
+    std::lock_guard<std::mutex> lock(_subscription_mutex);
+    _votl_state_subscription = callback;
+}
+
 void TelemetryImpl::subscribe_landed_state(Telemetry::LandedStateCallback& callback)
 {
     std::lock_guard<std::mutex> lock(_subscription_mutex);