Merge pull request #782 from irsdkv/add-direct-control-with-group

Add offboard/set_actuator_control to offboard plugin without specifying group names

src/backend/src/plugins/offboard/offboard_service_impl.h

@@ -63,6 +63,38 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
         return grpc::Status::OK;
     }
 
+    grpc::Status SetActuatorControl(
+        grpc::ServerContext* /* context */,
+        const rpc::offboard::SetActuatorControlRequest* request,
+        rpc::offboard::SetActuatorControlResponse* /* response */) override
+    {
+        if (request != nullptr) {
+            auto requested_actuator_control =
+                translateRPCActuatorControl(request->actuator_control());
+            _offboard.set_actuator_control(requested_actuator_control);
+        }
+
+        return grpc::Status::OK;
+    }
+
+    static mavsdk::Offboard::ActuatorControl
+    translateRPCActuatorControl(const rpc::offboard::ActuatorControl& rpc_actuator_control)
+    {
+        mavsdk::Offboard::ActuatorControl actuator_control{std::numeric_limits<float>::quiet_NaN()};
+
+        int num_groups = std::min(2, rpc_actuator_control.groups_size());
+
+        for (int i = 0; i < num_groups; i++) {
+            int num_controls = std::min(8, rpc_actuator_control.groups(i).controls_size());
+            for (int j = 0; j < num_controls; j++) {
+                // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field#
+                actuator_control.groups[i].controls[j] = rpc_actuator_control.groups(i).controls(j);
+            }
+        }
+
+        return actuator_control;
+    }
+
     grpc::Status SetAttitude(
         grpc::ServerContext* /* context */,
         const rpc::offboard::SetAttitudeRequest* request,

src/backend/test/offboard_service_impl_test.cpp

@@ -16,6 +16,14 @@ using OffboardServiceImpl = mavsdk::backend::OffboardServiceImpl<MockOffboard>;
 using OffboardResult = mavsdk::rpc::offboard::OffboardResult;
 using InputPair = std::pair<std::string, mavsdk::Offboard::Result>;
 
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_0 = -0.42f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_1 = 0.15f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_2 = 0.56f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_3 = -0.95f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_4 = 0.34f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_5 = 0.98f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_6 = -0.15f;
+static constexpr float ARBITRARY_ACTUATOR_CONTROL_7 = 0.15f;
 static constexpr float ARBITRARY_ROLL = 25.0f;
 static constexpr float ARBITRARY_PITCH = 40.0f;
 static constexpr float ARBITRARY_YAW = 37.0f;
@@ -46,6 +54,8 @@ class OffboardServiceImplTest : public ::testing::TestWithParam<InputPair> {
     std::unique_ptr<mavsdk::rpc::offboard::VelocityBodyYawspeed>
     createArbitraryRPCVelocityBodyYawspeed() const;
     std::unique_ptr<mavsdk::rpc::offboard::VelocityNEDYaw> createArbitraryRPCVelocityNedYaw() const;
+    std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>
+    createArbitraryRPCActuatorControl() const;
 };
 
 TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly)
@@ -155,6 +165,73 @@ TEST_F(OffboardServiceImplTest, setAttitudeRateDoesNotFailWithAllNullParams)
     offboardService.SetAttitudeRate(nullptr, nullptr, nullptr);
 }
 
+TEST_F(OffboardServiceImplTest, setActuatorControlDoesNotFailWithAllNullParams)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+
+    offboardService.SetActuatorControl(nullptr, nullptr, nullptr);
+}
+
+TEST_F(OffboardServiceImplTest, setActuatorControlDoesNotFailWithNullResponse)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+    mavsdk::rpc::offboard::SetActuatorControlRequest request;
+
+    auto rpc_actuator_control = createArbitraryRPCActuatorControl();
+    request.set_allocated_actuator_control(rpc_actuator_control.release());
+
+    offboardService.SetActuatorControl(nullptr, &request, nullptr);
+}
+
+TEST_F(OffboardServiceImplTest, setsActuatorControlCorrectly)
+{
+    MockOffboard offboard;
+    OffboardServiceImpl offboardService(offboard);
+    mavsdk::rpc::offboard::SetActuatorControlRequest request;
+
+    auto rpc_actuator_control = createArbitraryRPCActuatorControl();
+    const auto expected_actuator_control =
+        OffboardServiceImpl::translateRPCActuatorControl(*rpc_actuator_control);
+    EXPECT_CALL(offboard, set_actuator_control(expected_actuator_control)).Times(1);
+
+    request.set_allocated_actuator_control(rpc_actuator_control.release());
+
+    offboardService.SetActuatorControl(nullptr, &request, nullptr);
+}
+
+std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>
+OffboardServiceImplTest::createArbitraryRPCActuatorControl() const
+{
+    auto rpc_actuator_control = std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>(
+        new mavsdk::rpc::offboard::ActuatorControl());
+
+    auto rpc_actuator_group_0 = rpc_actuator_control.get()->add_groups();
+
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_0);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_1);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_2);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_3);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_4);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_5);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_6);
+    rpc_actuator_group_0->add_controls(ARBITRARY_ACTUATOR_CONTROL_7);
+
+    auto rpc_actuator_group_1 = rpc_actuator_control.get()->add_groups();
+
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_0 + 0.01f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_1 + 0.02f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_2 + 0.03f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_3 + 0.04f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_4 + 0.05f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_5 + 0.06f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_6 + 0.07f);
+    rpc_actuator_group_1->add_controls(ARBITRARY_ACTUATOR_CONTROL_7 + 0.08f);
+
+    return rpc_actuator_control;
+}
+
 TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithNullResponse)
 {
     MockOffboard offboard;

src/plugins/offboard/include/plugins/offboard/offboard.h

@@ -131,6 +131,34 @@ class Offboard : public PluginBase {
                                percent). */
     };
 
+    /**
+     * @brief Type for actuator control command.
+     * controls members should be normed to -1..+1 where 0 is neutral position.
+     * Throttle for single rotation direction motors is 0..1, negative range for reverse direction.
+     *
+     * One group support eight controls.
+     *
+     * Up to 16 actuator controls can be set. To ignore an output group, set all it conrols to NaN.
+     * If one or more controls in group is not NaN, then all NaN controls will sent as zero.
+     * The first 8 actuator controls internally map to control group 0, the latter 8 actuator
+     * controls map to control group 1. Depending on what controls are set (instead of NaN) 1 or 2
+     * MAVLink messages are actually sent.
+     *
+     * In PX4 v1.9.0 Only first four Control Groups are supported
+     * (https://github.com/PX4/Firmware/blob/v1.9.0/src/modules/mavlink/mavlink_receiver.cpp#L980).
+     */
+    struct ActuatorControl {
+        /**
+         * @brief Eight controls that will be given to the group. Each control is a normalized
+         * (-1..+1) command value, which will be mapped and scaled through the mixer.
+         */
+        struct Group {
+            float controls[8]; /**< @brief Controls in the group. */
+        };
+        Group groups[2]; /**< @brief Control Groups. In order not to send a group, set all its
+                            values to NaN. */
+    };
+
     /**
      * @brief Start offboard control (synchronous).
      *
@@ -214,6 +242,16 @@ class Offboard : public PluginBase {
      */
     void set_attitude_rate(AttitudeRate attitude_rate);
 
+    /**
+     * @brief Set direct actuator control values to groups #0 and #1.
+     * First 8 controls will go to control group 0, the following 8 controls to control group 1 (if
+     * actuator_control.num_controls more than 8).
+     *
+     * @param actuator_control actuators control values
+     */
+
+    void set_actuator_control(const ActuatorControl actuator_control);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */
@@ -228,6 +266,13 @@ class Offboard : public PluginBase {
     std::unique_ptr<OffboardImpl> _impl;
 };
 
+/**
+ * @brief Equal operator to compare two `Offboard::ActuatorControl` objects.
+ *
+ * @return `true` if items are equal.
+ */
+bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs);
+
 /**
  * @brief Equal operator to compare two `Offboard::Attitude` objects.
  *
@@ -242,6 +287,13 @@ bool operator==(const Offboard::Attitude& lhs, const Offboard::Attitude& rhs);
  */
 bool operator==(const Offboard::AttitudeRate& lhs, const Offboard::AttitudeRate& rhs);
 
+/**
+ * @brief Stream operator to print information about a `Offboard::ActuatorControl`.
+ *
+ * @return A reference to the stream.
+ */
+std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control);
+
 /**
  * @brief Stream operator to print information about a `Offboard::Attitude`.
  *

src/plugins/offboard/mocks/offboard_mock.h

@@ -15,6 +15,7 @@ class MockOffboard {
     MOCK_CONST_METHOD1(set_position_ned, void(Offboard::PositionNEDYaw)){};
     MOCK_CONST_METHOD1(set_velocity_body, void(Offboard::VelocityBodyYawspeed)){};
     MOCK_CONST_METHOD1(set_velocity_ned, void(Offboard::VelocityNEDYaw)){};
+    MOCK_CONST_METHOD1(set_actuator_control, void(Offboard::ActuatorControl)){};
 };
 
 } // namespace testing

src/plugins/offboard/offboard.cpp

@@ -1,5 +1,6 @@
 #include "plugins/offboard/offboard.h"
 #include "offboard_impl.h"
+#include <cmath>
 
 namespace mavsdk {
 
@@ -57,6 +58,11 @@ void Offboard::set_attitude_rate(Offboard::AttitudeRate attitude_rate)
     return _impl->set_attitude_rate(attitude_rate);
 }
 
+void Offboard::set_actuator_control(Offboard::ActuatorControl actuator_control)
+{
+    return _impl->set_actuator_control(actuator_control);
+}
+
 const char* Offboard::result_str(Result result)
 {
     switch (result) {
@@ -80,6 +86,41 @@ const char* Offboard::result_str(Result result)
     }
 }
 
+bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs)
+{
+    for (int i = 0; i < 2; i++) {
+        for (int j = 0; j < 8; j++) {
+            if ((std::isnan(lhs.groups[i].controls[j]) && std::isnan(rhs.groups[i].controls[j])) ||
+                lhs.groups[i].controls[j] == rhs.groups[i].controls[j]) {
+                continue;
+            }
+            return false;
+        }
+    }
+
+    return true;
+}
+
+std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control)
+{
+    return str << "[group: " << 0 << ", Command port 0: " << actuator_control.groups[0].controls[0]
+               << ", Command port 1: " << actuator_control.groups[0].controls[1]
+               << ", Command port 2: " << actuator_control.groups[0].controls[2]
+               << ", Command port 3: " << actuator_control.groups[0].controls[3]
+               << ", Command port 4: " << actuator_control.groups[0].controls[4]
+               << ", Command port 5: " << actuator_control.groups[0].controls[5]
+               << ", Command port 6: " << actuator_control.groups[0].controls[6]
+               << ", Command port 7: " << actuator_control.groups[0].controls[7] << "], "
+               << "[group: " << 1 << ", Command port 0: " << actuator_control.groups[1].controls[0]
+               << ", Command port 1: " << actuator_control.groups[1].controls[1]
+               << ", Command port 2: " << actuator_control.groups[1].controls[2]
+               << ", Command port 3: " << actuator_control.groups[1].controls[3]
+               << ", Command port 4: " << actuator_control.groups[1].controls[4]
+               << ", Command port 5: " << actuator_control.groups[1].controls[5]
+               << ", Command port 6: " << actuator_control.groups[1].controls[6]
+               << ", Command port 7: " << actuator_control.groups[1].controls[7] << "]";
+}
+
 bool operator==(const Offboard::Attitude& lhs, const Offboard::Attitude& rhs)
 {
     return lhs.roll_deg == rhs.roll_deg && lhs.pitch_deg == rhs.pitch_deg &&

src/plugins/offboard/offboard_impl.cpp

@@ -1,3 +1,4 @@
+#include <cmath>
 #include "global_include.h"
 #include "offboard_impl.h"
 #include "mavsdk_impl.h"
@@ -241,6 +242,33 @@ void OffboardImpl::set_attitude_rate(Offboard::AttitudeRate attitude_rate)
     send_attitude_rate();
 }
 
+void OffboardImpl::set_actuator_control(Offboard::ActuatorControl actuator_control)
+{
+    _mutex.lock();
+    _actuator_control = actuator_control;
+
+    if (_mode != Mode::ACTUATOR_CONTROL) {
+        if (_call_every_cookie) {
+            // If we're already sending other setpoints, stop that now.
+            _parent->remove_call_every(_call_every_cookie);
+            _call_every_cookie = nullptr;
+        }
+        // We automatically send motor rate values from now on.
+        _parent->add_call_every(
+            [this]() { send_actuator_control(); }, SEND_INTERVAL_S, &_call_every_cookie);
+
+        _mode = Mode::ACTUATOR_CONTROL;
+    } else {
+        // We're already sending these kind of values. Since the value changes, let's
+        // reschedule the next call, so we don't send values too often.
+        _parent->reset_call_every(_call_every_cookie);
+    }
+    _mutex.unlock();
+
+    // also send it right now to reduce latency
+    send_actuator_control();
+}
+
 void OffboardImpl::send_position_ned()
 {
     // const static uint16_t IGNORE_X = (1 << 0);
@@ -484,6 +512,41 @@ void OffboardImpl::send_attitude_rate()
     _parent->send_message(message);
 }
 
+void OffboardImpl::send_actuator_control_message(const float* controls, uint8_t group_number)
+{
+    mavlink_message_t message;
+    mavlink_msg_set_actuator_control_target_pack(
+        _parent->get_own_system_id(),
+        _parent->get_own_component_id(),
+        &message,
+        static_cast<uint32_t>(_parent->get_time().elapsed_s() * 1e3),
+        group_number,
+        _parent->get_system_id(),
+        _parent->get_autopilot_id(),
+        controls);
+    _parent->send_message(message);
+}
+
+void OffboardImpl::send_actuator_control()
+{
+    _mutex.lock();
+    Offboard::ActuatorControl actuator_control = _actuator_control;
+    _mutex.unlock();
+
+    for (int i = 0; i < 2; i++) {
+        int nan_count = 0;
+        for (int j = 0; j < 8; j++) {
+            if (std::isnan(actuator_control.groups[i].controls[j])) {
+                nan_count++;
+                actuator_control.groups[i].controls[j] = 0.0f;
+            }
+        }
+        if (nan_count < 8) {
+            send_actuator_control_message(&actuator_control.groups[i].controls[0], i);
+        }
+    }
+}
+
 void OffboardImpl::process_heartbeat(const mavlink_message_t& message)
 {
     mavlink_heartbeat_t heartbeat;