From 4e95cf9a017802052cbf8fe6c5e352f5043bf09a Mon Sep 17 00:00:00 2001
From: Ali Tlisov <ali.tlisov@gmail.com>
Date: Mon, 10 Jun 2019 17:04:42 +0300
Subject: [PATCH 01/16] initial implementation of actuator control command

---
 .../include/plugins/offboard/offboard.h       | 15 ++++++
 src/plugins/offboard/offboard.cpp             |  5 ++
 src/plugins/offboard/offboard_impl.cpp        | 49 +++++++++++++++++++
 src/plugins/offboard/offboard_impl.h          |  6 ++-
 4 files changed, 74 insertions(+), 1 deletion(-)

diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index a94ea1bd3e..0c314aaf7a 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -131,6 +131,14 @@ class Offboard : public PluginBase {
                                percent). */
     };
 
+    /**
+     * @brief Type for actuator control commands
+     *
+     */
+    struct ActuatorControl {
+        std::array<float, 8> actuator_control;
+    };
+
     /**
      * @brief Start offboard control (synchronous).
      *
@@ -214,6 +222,13 @@ class Offboard : public PluginBase {
      */
     void set_attitude_rate(AttitudeRate attitude_rate);
 
+    /**
+     * @brief Set direct actuator control values
+     *
+     * @param array of 8 values
+     */
+    void set_actuator_control(ActuatorControl actuator_control);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index df3434d9e0..6878c985ea 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -57,6 +57,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) {
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 6adbdd1479..a26f772dc6 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -241,6 +241,32 @@ 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();
+
+    send_actuator_control();
+}
+
 void OffboardImpl::send_position_ned()
 {
     // const static uint16_t IGNORE_X = (1 << 0);
@@ -484,6 +510,29 @@ void OffboardImpl::send_attitude_rate()
     _parent->send_message(message);
 }
 
+void OffboardImpl::send_actuator_control()
+{
+    _mutex.lock();
+    float actuator_control[8] = {};
+
+    std::copy(_actuator_control.actuator_control.begin(),
+              _actuator_control.actuator_control.end(),
+              actuator_control);
+    _mutex.unlock();
+
+    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),
+        0,
+        _parent->get_system_id(),
+        _parent->get_autopilot_id(),
+        actuator_control);
+    _parent->send_message(message);
+}
+
 void OffboardImpl::process_heartbeat(const mavlink_message_t &message)
 {
     mavlink_heartbeat_t heartbeat;
diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h
index 4f5e2afb15..c4d1a45595 100644
--- a/src/plugins/offboard/offboard_impl.h
+++ b/src/plugins/offboard/offboard_impl.h
@@ -33,6 +33,7 @@ class OffboardImpl : public PluginImplBase {
     void set_velocity_body(Offboard::VelocityBodyYawspeed velocity_body_yawspeed);
     void set_attitude(Offboard::Attitude attitude);
     void set_attitude_rate(Offboard::AttitudeRate attitude_rate);
+    void set_actuator_control(Offboard::ActuatorControl actuator_control);
 
     OffboardImpl(const OffboardImpl &) = delete;
     OffboardImpl &operator=(const OffboardImpl &) = delete;
@@ -43,6 +44,7 @@ class OffboardImpl : public PluginImplBase {
     void send_velocity_body();
     void send_attitude_rate();
     void send_attitude();
+    void send_actuator_control();
 
     void process_heartbeat(const mavlink_message_t &message);
     void receive_command_result(MAVLinkCommands::Result result,
@@ -59,13 +61,15 @@ class OffboardImpl : public PluginImplBase {
         VELOCITY_NED,
         VELOCITY_BODY,
         ATTITUDE,
-        ATTITUDE_RATE
+        ATTITUDE_RATE,
+        ACTUATOR_CONTROL
     } _mode = Mode::NOT_ACTIVE;
     Offboard::PositionNEDYaw _position_ned_yaw{};
     Offboard::VelocityNEDYaw _velocity_ned_yaw{};
     Offboard::VelocityBodyYawspeed _velocity_body_yawspeed{};
     Offboard::Attitude _attitude{};
     Offboard::AttitudeRate _attitude_rate{};
+    Offboard::ActuatorControl _actuator_control{};
 
     void *_call_every_cookie = nullptr;
 

From 6429cb4b40edeb6ca7ea62fac1ff6c81d600d725 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Tue, 11 Jun 2019 20:45:05 +0300
Subject: [PATCH 02/16] Type changed, tests and some additional functions
 implemented.

Corresponding commit in MAVSDK-Proto:
https://github.com/irsdkv/MAVSDK-Proto/commit/d2718757bfc5376dd42c1159b9b52309a7000f33
---
 .../plugins/offboard/offboard_service_impl.h  | 29 ++++++++
 .../test/offboard_service_impl_test.cpp       | 67 ++++++++++++++++++-
 .../include/plugins/offboard/offboard.h       | 30 ++++++++-
 src/plugins/offboard/mocks/offboard_mock.h    |  1 +
 src/plugins/offboard/offboard.cpp             | 23 +++++++
 src/plugins/offboard/offboard_impl.cpp        | 11 ++-
 6 files changed, 151 insertions(+), 10 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index c136d1b765..e79fc733b8 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -60,6 +60,35 @@ 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 = {};
+
+        actuator_control.actuator_group =
+            static_cast<mavsdk::Offboard::ActuatorControl::ActuatorGroup>(rpc_actuator_control.actuator_group());
+
+        int len = std::min(8, rpc_actuator_control.actuator_values_size());
+        for (int i = 0; i < len; i++) {
+            // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field#
+            actuator_control.actuator_values[i] = rpc_actuator_control.actuator_values(i);
+        }
+
+        return actuator_control;
+    }
+
     grpc::Status SetAttitude(grpc::ServerContext * /* context */,
                              const rpc::offboard::SetAttitudeRequest *request,
                              rpc::offboard::SetAttitudeResponse * /* response */) override
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index ba1590f2db..2661f3467e 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/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;
@@ -45,7 +53,9 @@ class OffboardServiceImplTest : public ::testing::TestWithParam<InputPair> {
     std::unique_ptr<mavsdk::rpc::offboard::PositionNEDYaw> createArbitraryRPCPositionNEDYaw() const;
     std::unique_ptr<mavsdk::rpc::offboard::VelocityBodyYawspeed>
     createArbitraryRPCVelocityBodyYawspeed() const;
-    std::unique_ptr<mavsdk::rpc::offboard::VelocityNEDYaw> createArbitraryRPCVelocityNedYaw() 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,61 @@ 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());
+
+    rpc_actuator_control.get()->set_actuator_group(mavsdk::rpc::offboard::ActuatorControl::FLIGHT_CONTROL);
+
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_0);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_1);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_2);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_3);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_4);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_5);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_6);
+    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_7);
+
+    return rpc_actuator_control;
+}
+
 TEST_F(OffboardServiceImplTest, setAttitudeDoesNotFailWithNullResponse)
 {
     MockOffboard offboard;
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 0c314aaf7a..698983b16e 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -132,11 +132,23 @@ class Offboard : public PluginBase {
     };
 
     /**
-     * @brief Type for actuator control commands
+     * @brief Type for actuator control commands.
+     * ActuatorValues 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.
      *
+     * 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 {
-        std::array<float, 8> actuator_control;
+        enum ActuatorGroup {
+            FLIGHT_CONTROL = 0, /**< @brief Control Group #0 (Flight Control). */
+            FLIGHT_CONTROL_VTOL = 1, /**< @brief Control Group #1 (Flight Control VTOL/Alternate). */
+            GIMBAL = 2, /**< @brief Control Group #2 (Gimbal). */
+            MANUAL_PASSTHROUGH = 3, /**< @brief Control Group #3 (Manual Passthrough). */
+        };
+
+        ActuatorGroup actuator_group; /**< @brief Actuator group. */
+        float actuator_values[8]; /**< @brief Actuator values array. */
     };
 
     /**
@@ -243,6 +255,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.
  *
@@ -257,6 +276,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`.
  *
diff --git a/src/plugins/offboard/mocks/offboard_mock.h b/src/plugins/offboard/mocks/offboard_mock.h
index d520b824a2..ef05bec54e 100644
--- a/src/plugins/offboard/mocks/offboard_mock.h
+++ b/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
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 6878c985ea..3b8bab5c5d 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -85,6 +85,29 @@ const char *Offboard::result_str(Result result)
     }
 }
 
+bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs)
+{
+    if (lhs.actuator_group != rhs.actuator_group)
+        return false;
+    for (int i = 0; i < 8; i++)
+        if (lhs.actuator_values[i] != rhs.actuator_values[i])
+            return false;
+    return true;
+}
+
+std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control)
+{
+    return str << "[group: " << actuator_control.actuator_group
+               << ", Command port 0: " << actuator_control.actuator_values[0]
+               << ", Command port 1: " << actuator_control.actuator_values[1]
+               << ", Command port 2: " << actuator_control.actuator_values[2]
+               << ", Command port 3: " << actuator_control.actuator_values[3]
+               << ", Command port 4: " << actuator_control.actuator_values[4]
+               << ", Command port 5: " << actuator_control.actuator_values[5]
+               << ", Command port 6: " << actuator_control.actuator_values[6]
+               << ", Command port 7: " << actuator_control.actuator_values[7] << "]";
+}
+
 bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs)
 {
     return lhs.roll_deg == rhs.roll_deg && lhs.pitch_deg == rhs.pitch_deg &&
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index a26f772dc6..2c56b467b8 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -264,6 +264,7 @@ void OffboardImpl::set_actuator_control(Offboard::ActuatorControl actuator_contr
     }
     _mutex.unlock();
 
+    // also send it right now to reduce latency
     send_actuator_control();
 }
 
@@ -513,11 +514,7 @@ void OffboardImpl::send_attitude_rate()
 void OffboardImpl::send_actuator_control()
 {
     _mutex.lock();
-    float actuator_control[8] = {};
-
-    std::copy(_actuator_control.actuator_control.begin(),
-              _actuator_control.actuator_control.end(),
-              actuator_control);
+    const Offboard::ActuatorControl actuator_control = _actuator_control;
     _mutex.unlock();
 
     mavlink_message_t message;
@@ -526,10 +523,10 @@ void OffboardImpl::send_actuator_control()
         _parent->get_own_component_id(),
         &message,
         static_cast<uint32_t>(_parent->get_time().elapsed_s() * 1e3),
-        0,
+        actuator_control.actuator_group,
         _parent->get_system_id(),
         _parent->get_autopilot_id(),
-        actuator_control);
+        actuator_control.actuator_values);
     _parent->send_message(message);
 }
 

From 9adc834be9dcddfbe9bdfa88930a9199ddaa2e94 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Mon, 1 Jul 2019 21:01:55 +0300
Subject: [PATCH 03/16] Group enum removed

---
 .../src/plugins/offboard/offboard_service_impl.h      |  6 ++++--
 src/backend/test/offboard_service_impl_test.cpp       |  2 --
 .../offboard/include/plugins/offboard/offboard.h      | 11 ++---------
 3 files changed, 6 insertions(+), 13 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index e79fc733b8..d7642a1f3c 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -77,8 +77,10 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     {
         mavsdk::Offboard::ActuatorControl actuator_control = {};
 
-        actuator_control.actuator_group =
-            static_cast<mavsdk::Offboard::ActuatorControl::ActuatorGroup>(rpc_actuator_control.actuator_group());
+        if (rpc_actuator_control.actuator_group() > UINT8_MAX)
+            return actuator_control;
+
+        actuator_control.actuator_group = rpc_actuator_control.actuator_group();
 
         int len = std::min(8, rpc_actuator_control.actuator_values_size());
         for (int i = 0; i < len; i++) {
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index 2661f3467e..bb5ed0e8ee 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/src/backend/test/offboard_service_impl_test.cpp
@@ -206,8 +206,6 @@ OffboardServiceImplTest::createArbitraryRPCActuatorControl() const
     auto rpc_actuator_control = std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>(
             new mavsdk::rpc::offboard::ActuatorControl());
 
-    rpc_actuator_control.get()->set_actuator_group(mavsdk::rpc::offboard::ActuatorControl::FLIGHT_CONTROL);
-
     rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_0);
     rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_1);
     rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_2);
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 698983b16e..feb2a7e5e6 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -140,15 +140,8 @@ class Offboard : public PluginBase {
      * (https://github.com/PX4/Firmware/blob/v1.9.0/src/modules/mavlink/mavlink_receiver.cpp#L980).
      */
     struct ActuatorControl {
-        enum ActuatorGroup {
-            FLIGHT_CONTROL = 0, /**< @brief Control Group #0 (Flight Control). */
-            FLIGHT_CONTROL_VTOL = 1, /**< @brief Control Group #1 (Flight Control VTOL/Alternate). */
-            GIMBAL = 2, /**< @brief Control Group #2 (Gimbal). */
-            MANUAL_PASSTHROUGH = 3, /**< @brief Control Group #3 (Manual Passthrough). */
-        };
-
-        ActuatorGroup actuator_group; /**< @brief Actuator group. */
         float actuator_values[8]; /**< @brief Actuator values array. */
+        uint8_t actuator_group; /**< @brief Actuator group. */
     };
 
     /**
@@ -237,7 +230,7 @@ class Offboard : public PluginBase {
     /**
      * @brief Set direct actuator control values
      *
-     * @param array of 8 values
+     * @param actuator_control actuators control values
      */
     void set_actuator_control(ActuatorControl actuator_control);
 

From 295fbe44328bbc5282283cc43c29cd8caeb22e17 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Tue, 2 Jul 2019 19:13:49 +0300
Subject: [PATCH 04/16] Added function to set actuator controls from array

---
 .../offboard/include/plugins/offboard/offboard.h      | 10 ++++++++++
 src/plugins/offboard/offboard.cpp                     | 11 +++++++++++
 2 files changed, 21 insertions(+)

diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index feb2a7e5e6..cdee107183 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -234,6 +234,16 @@ class Offboard : public PluginBase {
      */
     void set_actuator_control(ActuatorControl actuator_control);
 
+    /**
+     * @brief Set direct actuator control values.
+     * Default group_number is zero.
+     *
+     * @param controls array of actuators control values
+     *
+     * @param group_number group number to control
+     */
+    void set_actuator_control(const float (&controls)[8], uint8_t group_number = 0);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 3b8bab5c5d..d314f4d447 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -62,6 +62,17 @@ void Offboard::set_actuator_control(Offboard::ActuatorControl actuator_control)
     return _impl->set_actuator_control(actuator_control);
 }
 
+void Offboard::set_actuator_control(const float (&controls)[8], uint8_t group_number)
+{
+    Offboard::ActuatorControl actuator_control {};
+
+    std::copy(std::begin(controls), std::end(controls), actuator_control.actuator_values);
+
+    actuator_control.actuator_group = group_number;
+
+    set_actuator_control(actuator_control);
+}
+
 const char *Offboard::result_str(Result result)
 {
     switch (result) {

From 64d34f84ec1ee17e3f9d68b8ede801c967c62939 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Mon, 8 Jul 2019 16:21:47 +0300
Subject: [PATCH 05/16] Group number in internal types deleted

---
 .../plugins/offboard/offboard_service_impl.h  |  8 ++--
 .../test/offboard_service_impl_test.cpp       | 18 +++----
 .../include/plugins/offboard/offboard.h       | 31 ++++++------
 src/plugins/offboard/offboard.cpp             | 47 ++++++++++---------
 src/plugins/offboard/offboard_impl.cpp        | 31 +++++++-----
 src/plugins/offboard/offboard_impl.h          |  1 +
 6 files changed, 74 insertions(+), 62 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index d7642a1f3c..2e544fb9ca 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -77,15 +77,13 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     {
         mavsdk::Offboard::ActuatorControl actuator_control = {};
 
-        if (rpc_actuator_control.actuator_group() > UINT8_MAX)
-            return actuator_control;
+        int len = std::min(16, rpc_actuator_control.controls_size());
 
-        actuator_control.actuator_group = rpc_actuator_control.actuator_group();
+        actuator_control.num_controls = len;
 
-        int len = std::min(8, rpc_actuator_control.actuator_values_size());
         for (int i = 0; i < len; i++) {
             // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field#
-            actuator_control.actuator_values[i] = rpc_actuator_control.actuator_values(i);
+            actuator_control.controls[i] = rpc_actuator_control.controls(i);
         }
 
         return actuator_control;
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index bb5ed0e8ee..c834cbd5b5 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/src/backend/test/offboard_service_impl_test.cpp
@@ -24,6 +24,7 @@ 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_ACTUATOR_CONTROL_8 = 0.65f;
 static constexpr float ARBITRARY_ROLL = 25.0f;
 static constexpr float ARBITRARY_PITCH = 40.0f;
 static constexpr float ARBITRARY_YAW = 37.0f;
@@ -206,14 +207,15 @@ OffboardServiceImplTest::createArbitraryRPCActuatorControl() const
     auto rpc_actuator_control = std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>(
             new mavsdk::rpc::offboard::ActuatorControl());
 
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_0);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_1);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_2);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_3);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_4);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_5);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_6);
-    rpc_actuator_control.get()->add_actuator_values(ARBITRARY_ACTUATOR_CONTROL_7);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_0);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_1);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_2);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_3);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_4);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_5);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_6);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_7);
+    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_8);
 
     return rpc_actuator_control;
 }
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index cdee107183..5450e4e3ac 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -132,16 +132,24 @@ class Offboard : public PluginBase {
     };
 
     /**
-     * @brief Type for actuator control commands.
-     * ActuatorValues members should be normed to -1..+1 where 0 is neutral position.
+     * @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.
+     *
      * 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 {
-        float actuator_values[8]; /**< @brief Actuator values array. */
-        uint8_t actuator_group; /**< @brief Actuator group. */
+        float controls[16]; /**< @brief Actuator controls array.
+                               First 8 will go to control group 0, the following 8 to control group 1.*/
+        uint8_t num_controls; /**< @brief Number of valid actuator control values in array.
+                               If num_controls <= 8 then only one message for Group 0 will be sent. In this case
+                                controls in controls array for channels from num_controls to 7 will also be sent.
+                               If number > 8 then two messages (to Group 0 and Group 1) will be sent. In this case
+                                controls in controls array from num_controls to 15 will also be sent in the second
+                                message.*/
     };
 
     /**
@@ -228,21 +236,14 @@ class Offboard : public PluginBase {
     void set_attitude_rate(AttitudeRate attitude_rate);
 
     /**
-     * @brief Set direct actuator control values
+     * @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(ActuatorControl actuator_control);
 
-    /**
-     * @brief Set direct actuator control values.
-     * Default group_number is zero.
-     *
-     * @param controls array of actuators control values
-     *
-     * @param group_number group number to control
-     */
-    void set_actuator_control(const float (&controls)[8], uint8_t group_number = 0);
+    void set_actuator_control(const ActuatorControl actuator_control);
 
     /**
      * @brief Copy constructor (object is not copyable).
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index d314f4d447..7a951ce14f 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -62,17 +62,6 @@ void Offboard::set_actuator_control(Offboard::ActuatorControl actuator_control)
     return _impl->set_actuator_control(actuator_control);
 }
 
-void Offboard::set_actuator_control(const float (&controls)[8], uint8_t group_number)
-{
-    Offboard::ActuatorControl actuator_control {};
-
-    std::copy(std::begin(controls), std::end(controls), actuator_control.actuator_values);
-
-    actuator_control.actuator_group = group_number;
-
-    set_actuator_control(actuator_control);
-}
-
 const char *Offboard::result_str(Result result)
 {
     switch (result) {
@@ -98,25 +87,37 @@ const char *Offboard::result_str(Result result)
 
 bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs)
 {
-    if (lhs.actuator_group != rhs.actuator_group)
+    if (lhs.num_controls!= rhs.num_controls)
         return false;
-    for (int i = 0; i < 8; i++)
-        if (lhs.actuator_values[i] != rhs.actuator_values[i])
+    for (int i = 0; i < lhs.num_controls; i++)
+        if (lhs.controls[i] != rhs.controls[i])
             return false;
     return true;
 }
 
 std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control)
 {
-    return str << "[group: " << actuator_control.actuator_group
-               << ", Command port 0: " << actuator_control.actuator_values[0]
-               << ", Command port 1: " << actuator_control.actuator_values[1]
-               << ", Command port 2: " << actuator_control.actuator_values[2]
-               << ", Command port 3: " << actuator_control.actuator_values[3]
-               << ", Command port 4: " << actuator_control.actuator_values[4]
-               << ", Command port 5: " << actuator_control.actuator_values[5]
-               << ", Command port 6: " << actuator_control.actuator_values[6]
-               << ", Command port 7: " << actuator_control.actuator_values[7] << "]";
+    str << "[group: " << 0
+               << ", Command port 0: " << actuator_control.controls[0]
+               << ", Command port 1: " << actuator_control.controls[1]
+               << ", Command port 2: " << actuator_control.controls[2]
+               << ", Command port 3: " << actuator_control.controls[3]
+               << ", Command port 4: " << actuator_control.controls[4]
+               << ", Command port 5: " << actuator_control.controls[5]
+               << ", Command port 6: " << actuator_control.controls[6]
+               << ", Command port 7: " << actuator_control.controls[7];
+    if (actuator_control.num_controls > 8) {
+        str << "], [group: " << 1
+            << ", Command port 0: " << actuator_control.controls[8]
+            << ", Command port 1: " << actuator_control.controls[9]
+            << ", Command port 2: " << actuator_control.controls[10]
+            << ", Command port 3: " << actuator_control.controls[11]
+            << ", Command port 4: " << actuator_control.controls[12]
+            << ", Command port 5: " << actuator_control.controls[13]
+            << ", Command port 6: " << actuator_control.controls[14]
+            << ", Command port 7: " << actuator_control.controls[15];
+    }
+    return str << "]";
 }
 
 bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs)
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 2c56b467b8..81791158a7 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -511,23 +511,32 @@ 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();
     const Offboard::ActuatorControl actuator_control = _actuator_control;
     _mutex.unlock();
 
-    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),
-        actuator_control.actuator_group,
-        _parent->get_system_id(),
-        _parent->get_autopilot_id(),
-        actuator_control.actuator_values);
-    _parent->send_message(message);
+    send_actuator_control_message(actuator_control.controls);
+
+    if (actuator_control.num_controls > 8) {
+        send_actuator_control_message(&actuator_control.controls[8], 1);
+    }
 }
 
 void OffboardImpl::process_heartbeat(const mavlink_message_t &message)
diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h
index c4d1a45595..d41d7d2380 100644
--- a/src/plugins/offboard/offboard_impl.h
+++ b/src/plugins/offboard/offboard_impl.h
@@ -45,6 +45,7 @@ class OffboardImpl : public PluginImplBase {
     void send_attitude_rate();
     void send_attitude();
     void send_actuator_control();
+    void send_actuator_control_message(const float * controls, uint8_t group_number = 0);
 
     void process_heartbeat(const mavlink_message_t &message);
     void receive_command_result(MAVLinkCommands::Result result,

From ea7a971f103c53d84ff03244116b0f542aeba226 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Mon, 8 Jul 2019 21:15:46 +0300
Subject: [PATCH 06/16] NaN as the end of valid controls

---
 .../plugins/offboard/offboard_service_impl.h  |  8 +--
 .../test/offboard_service_impl_test.cpp       |  3 +-
 .../include/plugins/offboard/offboard.h       | 16 +++---
 src/plugins/offboard/offboard.cpp             | 53 +++++++++++--------
 src/plugins/offboard/offboard_impl.cpp        | 16 ++++--
 5 files changed, 60 insertions(+), 36 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index 2e544fb9ca..193aa22dd1 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -75,17 +75,19 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     static mavsdk::Offboard::ActuatorControl
     translateRPCActuatorControl(const rpc::offboard::ActuatorControl &rpc_actuator_control)
     {
-        mavsdk::Offboard::ActuatorControl actuator_control = {};
+        mavsdk::Offboard::ActuatorControl actuator_control;
 
         int len = std::min(16, rpc_actuator_control.controls_size());
 
-        actuator_control.num_controls = len;
-
         for (int i = 0; i < len; i++) {
             // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field#
             actuator_control.controls[i] = rpc_actuator_control.controls(i);
         }
 
+        if (len < 16) {
+            actuator_control.controls[len] = std::numeric_limits<float>::quiet_NaN();
+        }
+
         return actuator_control;
     }
 
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index c834cbd5b5..f389db9445 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/src/backend/test/offboard_service_impl_test.cpp
@@ -54,8 +54,7 @@ class OffboardServiceImplTest : public ::testing::TestWithParam<InputPair> {
     std::unique_ptr<mavsdk::rpc::offboard::PositionNEDYaw> createArbitraryRPCPositionNEDYaw() const;
     std::unique_ptr<mavsdk::rpc::offboard::VelocityBodyYawspeed>
     createArbitraryRPCVelocityBodyYawspeed() const;
-    std::unique_ptr<mavsdk::rpc::offboard::VelocityNEDYaw>
-    createArbitraryRPCVelocityNedYaw() const;
+    std::unique_ptr<mavsdk::rpc::offboard::VelocityNEDYaw> createArbitraryRPCVelocityNedYaw() const;
     std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl> createArbitraryRPCActuatorControl() const;
 };
 
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 5450e4e3ac..341e0d2e50 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -143,13 +143,15 @@ class Offboard : public PluginBase {
      */
     struct ActuatorControl {
         float controls[16]; /**< @brief Actuator controls array.
-                               First 8 will go to control group 0, the following 8 to control group 1.*/
-        uint8_t num_controls; /**< @brief Number of valid actuator control values in array.
-                               If num_controls <= 8 then only one message for Group 0 will be sent. In this case
-                                controls in controls array for channels from num_controls to 7 will also be sent.
-                               If number > 8 then two messages (to Group 0 and Group 1) will be sent. In this case
-                                controls in controls array from num_controls to 15 will also be sent in the second
-                                message.*/
+                               First 8 will go to control group 0, the following 8 to control group 1.
+                               If you want to send less than 16 controls you should to set first non-specified
+                               control to NAN.
+                               If index of first NAN value will be less or equal than 8 then only one
+                               message for Group 0 will be sent. All controls from first NAN value
+                               to 7 in Group 0 will be set to zero.
+                               If index of first NAN value will be great than 8 then two messages (to
+                               Group 0 and Group 1) will be sent. In this case all controls from first NAN value
+                               to 7 in Group 1 will be set to zero. */
     };
 
     /**
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 7a951ce14f..1924e5dfc7 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -87,35 +87,46 @@ const char *Offboard::result_str(Result result)
 
 bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs)
 {
-    if (lhs.num_controls!= rhs.num_controls)
-        return false;
-    for (int i = 0; i < lhs.num_controls; i++)
-        if (lhs.controls[i] != rhs.controls[i])
+    for (int i = 0; i < 16; i++) {
+        if (std::isnan(lhs.controls[i]) && std::isnan(rhs.controls[i]))
+            return true;
+        if (lhs.controls[i] != rhs.controls[i]) {
             return false;
+        }
+    }
+
     return true;
 }
 
 std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control)
 {
+    bool was_nan = false;
+
+    const auto get_value_to_send = [&was_nan](float value) -> float {
+        if (was_nan)
+            return 0.0f;
+        return (std::isnan(value) ? was_nan = true, 0.0f : value);
+    };
+
     str << "[group: " << 0
-               << ", Command port 0: " << actuator_control.controls[0]
-               << ", Command port 1: " << actuator_control.controls[1]
-               << ", Command port 2: " << actuator_control.controls[2]
-               << ", Command port 3: " << actuator_control.controls[3]
-               << ", Command port 4: " << actuator_control.controls[4]
-               << ", Command port 5: " << actuator_control.controls[5]
-               << ", Command port 6: " << actuator_control.controls[6]
-               << ", Command port 7: " << actuator_control.controls[7];
-    if (actuator_control.num_controls > 8) {
+               << ", Command port 0: " << get_value_to_send(actuator_control.controls[0])
+               << ", Command port 1: " << get_value_to_send(actuator_control.controls[1])
+               << ", Command port 2: " << get_value_to_send(actuator_control.controls[2])
+               << ", Command port 3: " << get_value_to_send(actuator_control.controls[3])
+               << ", Command port 4: " << get_value_to_send(actuator_control.controls[4])
+               << ", Command port 5: " << get_value_to_send(actuator_control.controls[5])
+               << ", Command port 6: " << get_value_to_send(actuator_control.controls[6])
+               << ", Command port 7: " << get_value_to_send(actuator_control.controls[7]);
+    if (!was_nan && !std::isnan(actuator_control.controls[8])) {
         str << "], [group: " << 1
-            << ", Command port 0: " << actuator_control.controls[8]
-            << ", Command port 1: " << actuator_control.controls[9]
-            << ", Command port 2: " << actuator_control.controls[10]
-            << ", Command port 3: " << actuator_control.controls[11]
-            << ", Command port 4: " << actuator_control.controls[12]
-            << ", Command port 5: " << actuator_control.controls[13]
-            << ", Command port 6: " << actuator_control.controls[14]
-            << ", Command port 7: " << actuator_control.controls[15];
+            << ", Command port 0: " << get_value_to_send(actuator_control.controls[8])
+            << ", Command port 1: " << get_value_to_send(actuator_control.controls[9])
+            << ", Command port 2: " << get_value_to_send(actuator_control.controls[10])
+            << ", Command port 3: " << get_value_to_send(actuator_control.controls[11])
+            << ", Command port 4: " << get_value_to_send(actuator_control.controls[12])
+            << ", Command port 5: " << get_value_to_send(actuator_control.controls[13])
+            << ", Command port 6: " << get_value_to_send(actuator_control.controls[14])
+            << ", Command port 7: " << get_value_to_send(actuator_control.controls[15]);
     }
     return str << "]";
 }
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 81791158a7..ed75368ecf 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -529,12 +529,22 @@ void OffboardImpl::send_actuator_control_message(const float * controls, uint8_t
 void OffboardImpl::send_actuator_control()
 {
     _mutex.lock();
-    const Offboard::ActuatorControl actuator_control = _actuator_control;
+    Offboard::ActuatorControl actuator_control = _actuator_control;
     _mutex.unlock();
 
-    send_actuator_control_message(actuator_control.controls);
+    int first_nan_index = 16;
 
-    if (actuator_control.num_controls > 8) {
+    for (int i = 0; i < 16; i++) {
+        if (std::isnan(actuator_control.controls[i])) {
+            first_nan_index = i;
+            std::fill(&actuator_control.controls[i], &actuator_control.controls[16], 0.0f);
+            break;
+        }
+    }
+
+    send_actuator_control_message(&actuator_control.controls[0]);
+
+    if (first_nan_index > 8) {
         send_actuator_control_message(&actuator_control.controls[8], 1);
     }
 }

From 05c60283aa35a0e5f14212a231d82926c151ce9e Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Tue, 9 Jul 2019 17:53:31 +0300
Subject: [PATCH 07/16] ActuatorControl data type changed

---
 .../plugins/offboard/offboard_service_impl.h  | 17 +++---
 .../test/offboard_service_impl_test.cpp       | 31 ++++++----
 .../include/plugins/offboard/offboard.h       | 20 +++----
 src/plugins/offboard/offboard.cpp             | 57 ++++++++-----------
 src/plugins/offboard/offboard_impl.cpp        | 23 ++++----
 5 files changed, 73 insertions(+), 75 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index 193aa22dd1..e573d89861 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -75,17 +75,16 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     static mavsdk::Offboard::ActuatorControl
     translateRPCActuatorControl(const rpc::offboard::ActuatorControl &rpc_actuator_control)
     {
-        mavsdk::Offboard::ActuatorControl actuator_control;
+        mavsdk::Offboard::ActuatorControl actuator_control {std::numeric_limits<float>::quiet_NaN()};
 
-        int len = std::min(16, rpc_actuator_control.controls_size());
+        int num_groups = std::min(2, rpc_actuator_control.groups_size());
 
-        for (int i = 0; i < len; i++) {
-            // https://developers.google.com/protocol-buffers/docs/reference/cpp/google.protobuf.repeated_field#
-            actuator_control.controls[i] = rpc_actuator_control.controls(i);
-        }
-
-        if (len < 16) {
-            actuator_control.controls[len] = std::numeric_limits<float>::quiet_NaN();
+        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;
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index f389db9445..832180a2d1 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/src/backend/test/offboard_service_impl_test.cpp
@@ -24,7 +24,6 @@ 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_ACTUATOR_CONTROL_8 = 0.65f;
 static constexpr float ARBITRARY_ROLL = 25.0f;
 static constexpr float ARBITRARY_PITCH = 40.0f;
 static constexpr float ARBITRARY_YAW = 37.0f;
@@ -206,15 +205,27 @@ OffboardServiceImplTest::createArbitraryRPCActuatorControl() const
     auto rpc_actuator_control = std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>(
             new mavsdk::rpc::offboard::ActuatorControl());
 
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_0);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_1);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_2);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_3);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_4);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_5);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_6);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_7);
-    rpc_actuator_control.get()->add_controls(ARBITRARY_ACTUATOR_CONTROL_8);
+    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;
 }
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 341e0d2e50..1537d29257 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -138,20 +138,20 @@ class Offboard : public PluginBase {
      *
      * 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 {
-        float controls[16]; /**< @brief Actuator controls array.
-                               First 8 will go to control group 0, the following 8 to control group 1.
-                               If you want to send less than 16 controls you should to set first non-specified
-                               control to NAN.
-                               If index of first NAN value will be less or equal than 8 then only one
-                               message for Group 0 will be sent. All controls from first NAN value
-                               to 7 in Group 0 will be set to zero.
-                               If index of first NAN value will be great than 8 then two messages (to
-                               Group 0 and Group 1) will be sent. In this case all controls from first NAN value
-                               to 7 in Group 1 will be set to zero. */
+        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. */
     };
 
     /**
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 1924e5dfc7..da2961d59c 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -87,10 +87,12 @@ const char *Offboard::result_str(Result result)
 
 bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs)
 {
-    for (int i = 0; i < 16; i++) {
-        if (std::isnan(lhs.controls[i]) && std::isnan(rhs.controls[i]))
-            return true;
-        if (lhs.controls[i] != rhs.controls[i]) {
+    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;
         }
     }
@@ -100,35 +102,24 @@ bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorCo
 
 std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control)
 {
-    bool was_nan = false;
-
-    const auto get_value_to_send = [&was_nan](float value) -> float {
-        if (was_nan)
-            return 0.0f;
-        return (std::isnan(value) ? was_nan = true, 0.0f : value);
-    };
-
-    str << "[group: " << 0
-               << ", Command port 0: " << get_value_to_send(actuator_control.controls[0])
-               << ", Command port 1: " << get_value_to_send(actuator_control.controls[1])
-               << ", Command port 2: " << get_value_to_send(actuator_control.controls[2])
-               << ", Command port 3: " << get_value_to_send(actuator_control.controls[3])
-               << ", Command port 4: " << get_value_to_send(actuator_control.controls[4])
-               << ", Command port 5: " << get_value_to_send(actuator_control.controls[5])
-               << ", Command port 6: " << get_value_to_send(actuator_control.controls[6])
-               << ", Command port 7: " << get_value_to_send(actuator_control.controls[7]);
-    if (!was_nan && !std::isnan(actuator_control.controls[8])) {
-        str << "], [group: " << 1
-            << ", Command port 0: " << get_value_to_send(actuator_control.controls[8])
-            << ", Command port 1: " << get_value_to_send(actuator_control.controls[9])
-            << ", Command port 2: " << get_value_to_send(actuator_control.controls[10])
-            << ", Command port 3: " << get_value_to_send(actuator_control.controls[11])
-            << ", Command port 4: " << get_value_to_send(actuator_control.controls[12])
-            << ", Command port 5: " << get_value_to_send(actuator_control.controls[13])
-            << ", Command port 6: " << get_value_to_send(actuator_control.controls[14])
-            << ", Command port 7: " << get_value_to_send(actuator_control.controls[15]);
-    }
-    return str << "]";
+    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[8]
+               << ", Command port 1: " << actuator_control.groups[1].controls[9]
+               << ", Command port 2: " << actuator_control.groups[1].controls[10]
+               << ", Command port 3: " << actuator_control.groups[1].controls[11]
+               << ", Command port 4: " << actuator_control.groups[1].controls[12]
+               << ", Command port 5: " << actuator_control.groups[1].controls[13]
+               << ", Command port 6: " << actuator_control.groups[1].controls[14]
+               << ", Command port 7: " << actuator_control.groups[1].controls[15] << "]";
 }
 
 bool operator==(const Offboard::Attitude &lhs, const Offboard::Attitude &rhs)
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index ed75368ecf..a178ead1e8 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -532,20 +532,17 @@ void OffboardImpl::send_actuator_control()
     Offboard::ActuatorControl actuator_control = _actuator_control;
     _mutex.unlock();
 
-    int first_nan_index = 16;
-
-    for (int i = 0; i < 16; i++) {
-        if (std::isnan(actuator_control.controls[i])) {
-            first_nan_index = i;
-            std::fill(&actuator_control.controls[i], &actuator_control.controls[16], 0.0f);
-            break;
+    for (int i = 0; i < 2; i++) {
+        int nan_count = 0;
+        for (int j = 0; i < 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);
         }
-    }
-
-    send_actuator_control_message(&actuator_control.controls[0]);
-
-    if (first_nan_index > 8) {
-        send_actuator_control_message(&actuator_control.controls[8], 1);
     }
 }
 

From 399bc2b404006fb33582a85eed192c52f044e0d2 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 10 Jul 2019 10:54:05 +0300
Subject: [PATCH 08/16] Out of bounds fix

---
 src/plugins/offboard/offboard.cpp | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index da2961d59c..8db65a6625 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -112,14 +112,14 @@ std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &act
                << ", 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[8]
-               << ", Command port 1: " << actuator_control.groups[1].controls[9]
-               << ", Command port 2: " << actuator_control.groups[1].controls[10]
-               << ", Command port 3: " << actuator_control.groups[1].controls[11]
-               << ", Command port 4: " << actuator_control.groups[1].controls[12]
-               << ", Command port 5: " << actuator_control.groups[1].controls[13]
-               << ", Command port 6: " << actuator_control.groups[1].controls[14]
-               << ", Command port 7: " << actuator_control.groups[1].controls[15] << "]";
+               << ", 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)

From 5a8968303a82a931cf316bb4f495a310946d14f4 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 10 Jul 2019 14:23:20 +0300
Subject: [PATCH 09/16] cmath header include added

---
 src/plugins/offboard/offboard.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 8db65a6625..1b9ab52470 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -1,5 +1,6 @@
 #include "plugins/offboard/offboard.h"
 #include "offboard_impl.h"
+#include <cmath>
 
 namespace mavsdk {
 

From dab5e05cbd6e8526a5c7852fa1e7888e54e06a3f Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 10 Jul 2019 16:04:30 +0300
Subject: [PATCH 10/16] Proto submodule rev updated.

---
 src/backend/proto | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/backend/proto b/src/backend/proto
index d2042e2dc1..12f26d3b60 160000
--- a/src/backend/proto
+++ b/src/backend/proto
@@ -1 +1 @@
-Subproject commit d2042e2dc116529db30d0b0c31aa7121cdc7e4b7
+Subproject commit 12f26d3b60fd718f0e6199a453f0d25dbfbbb8cc

From d21fe35b1251ad224ef2783a953b8659dcb81771 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 10 Jul 2019 16:44:08 +0300
Subject: [PATCH 11/16] cmath header included again

---
 src/plugins/offboard/offboard_impl.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index a178ead1e8..53ff17e489 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -1,3 +1,4 @@
+#include <cmath>
 #include "global_include.h"
 #include "offboard_impl.h"
 #include "mavsdk_impl.h"

From 6ef3af93d028bec92a73692ddf1b43d7f1c6dce7 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 10 Jul 2019 17:26:29 +0300
Subject: [PATCH 12/16] Glaring mistake fix

---
 src/plugins/offboard/offboard_impl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 53ff17e489..50b5709921 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -535,7 +535,7 @@ void OffboardImpl::send_actuator_control()
 
     for (int i = 0; i < 2; i++) {
         int nan_count = 0;
-        for (int j = 0; i < 8; j++) {
+        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;

From 4ed2e377c3937256990dfb6e62a9d72e2e920fac Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Thu, 11 Jul 2019 15:59:34 +0300
Subject: [PATCH 13/16] Style fix

---
 .../plugins/offboard/offboard_service_impl.h   | 12 +++++++-----
 .../test/offboard_service_impl_test.cpp        |  8 +++++---
 .../include/plugins/offboard/offboard.h        |  9 +++++----
 src/plugins/offboard/offboard.cpp              |  2 +-
 src/plugins/offboard/offboard_impl.cpp         | 18 +++++++++---------
 src/plugins/offboard/offboard_impl.h           |  2 +-
 6 files changed, 28 insertions(+), 23 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index e573d89861..29dfd09a39 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -60,12 +60,14 @@ 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
+    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());
+            auto requested_actuator_control =
+                translateRPCActuatorControl(request->actuator_control());
             _offboard.set_actuator_control(requested_actuator_control);
         }
 
@@ -75,7 +77,7 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     static mavsdk::Offboard::ActuatorControl
     translateRPCActuatorControl(const rpc::offboard::ActuatorControl &rpc_actuator_control)
     {
-        mavsdk::Offboard::ActuatorControl actuator_control {std::numeric_limits<float>::quiet_NaN()};
+        mavsdk::Offboard::ActuatorControl actuator_control{std::numeric_limits<float>::quiet_NaN()};
 
         int num_groups = std::min(2, rpc_actuator_control.groups_size());
 
diff --git a/src/backend/test/offboard_service_impl_test.cpp b/src/backend/test/offboard_service_impl_test.cpp
index 832180a2d1..36fe92921a 100644
--- a/src/backend/test/offboard_service_impl_test.cpp
+++ b/src/backend/test/offboard_service_impl_test.cpp
@@ -54,7 +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;
+    std::unique_ptr<mavsdk::rpc::offboard::ActuatorControl>
+    createArbitraryRPCActuatorControl() const;
 };
 
 TEST_P(OffboardServiceImplTest, startResultIsTranslatedCorrectly)
@@ -191,7 +192,8 @@ TEST_F(OffboardServiceImplTest, setsActuatorControlCorrectly)
     mavsdk::rpc::offboard::SetActuatorControlRequest request;
 
     auto rpc_actuator_control = createArbitraryRPCActuatorControl();
-    const auto expected_actuator_control = OffboardServiceImpl::translateRPCActuatorControl(*rpc_actuator_control);
+    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());
@@ -203,7 +205,7 @@ 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());
+        new mavsdk::rpc::offboard::ActuatorControl());
 
     auto rpc_actuator_group_0 = rpc_actuator_control.get()->add_groups();
 
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 1537d29257..7c39f94dd8 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -140,9 +140,9 @@ class Offboard : public PluginBase {
      *
      * 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.
+     * 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).
@@ -151,7 +151,8 @@ class Offboard : public PluginBase {
         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. */
+        Group groups[2]; /**< @brief Control Groups. In order not to send a group, set all its
+                            values to NaN. */
     };
 
     /**
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 1b9ab52470..8c426045e3 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -91,7 +91,7 @@ bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorCo
     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]) {
+                lhs.groups[i].controls[j] == rhs.groups[i].controls[j]) {
                 continue;
             }
             return false;
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 50b5709921..2c3d03c186 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -512,18 +512,18 @@ void OffboardImpl::send_attitude_rate()
     _parent->send_message(message);
 }
 
-void OffboardImpl::send_actuator_control_message(const float * controls, uint8_t group_number)
+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->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);
 }
 
diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h
index d41d7d2380..e5f28da76f 100644
--- a/src/plugins/offboard/offboard_impl.h
+++ b/src/plugins/offboard/offboard_impl.h
@@ -45,7 +45,7 @@ class OffboardImpl : public PluginImplBase {
     void send_attitude_rate();
     void send_attitude();
     void send_actuator_control();
-    void send_actuator_control_message(const float * controls, uint8_t group_number = 0);
+    void send_actuator_control_message(const float *controls, uint8_t group_number = 0);
 
     void process_heartbeat(const mavlink_message_t &message);
     void receive_command_result(MAVLinkCommands::Result result,

From 7d63daf940b5558dc449ce084a317a3e5468efc1 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Tue, 16 Jul 2019 17:10:50 +0300
Subject: [PATCH 14/16] fix_style from latest develop applied

---
 .../src/plugins/offboard/offboard_service_impl.h     | 12 ++++++------
 .../offboard/include/plugins/offboard/offboard.h     |  4 ++--
 src/plugins/offboard/offboard.cpp                    |  4 ++--
 src/plugins/offboard/offboard_impl.cpp               |  2 +-
 src/plugins/offboard/offboard_impl.h                 |  2 +-
 5 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/src/backend/src/plugins/offboard/offboard_service_impl.h b/src/backend/src/plugins/offboard/offboard_service_impl.h
index 590e05ecbe..5618d5cc04 100644
--- a/src/backend/src/plugins/offboard/offboard_service_impl.h
+++ b/src/backend/src/plugins/offboard/offboard_service_impl.h
@@ -63,10 +63,10 @@ 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
+    grpc::Status SetActuatorControl(
+        grpc::ServerContext* /* context */,
+        const rpc::offboard::SetActuatorControlRequest* request,
+        rpc::offboard::SetActuatorControlResponse* /* response */) override
     {
         if (request != nullptr) {
             auto requested_actuator_control =
@@ -78,7 +78,7 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
     }
 
     static mavsdk::Offboard::ActuatorControl
-    translateRPCActuatorControl(const rpc::offboard::ActuatorControl &rpc_actuator_control)
+    translateRPCActuatorControl(const rpc::offboard::ActuatorControl& rpc_actuator_control)
     {
         mavsdk::Offboard::ActuatorControl actuator_control{std::numeric_limits<float>::quiet_NaN()};
 
@@ -94,7 +94,7 @@ class OffboardServiceImpl final : public rpc::offboard::OffboardService::Service
 
         return actuator_control;
     }
-    
+
     grpc::Status SetAttitude(
         grpc::ServerContext* /* context */,
         const rpc::offboard::SetAttitudeRequest* request,
diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 9f41c62cfb..17c1c5dc3a 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -267,7 +267,7 @@ class Offboard : public PluginBase {
  *
  * @return `true` if items are equal.
  */
-bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs);
+bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs);
 
 /**
  * @brief Equal operator to compare two `Offboard::Attitude` objects.
@@ -288,7 +288,7 @@ bool operator==(const Offboard::AttitudeRate& lhs, const Offboard::AttitudeRate&
  *
  * @return A reference to the stream.
  */
-std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control);
+std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control);
 
 /**
  * @brief Stream operator to print information about a `Offboard::Attitude`.
diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index c7894f2ec0..876af05d3f 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -86,7 +86,7 @@ const char* Offboard::result_str(Result result)
     }
 }
 
-bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorControl &rhs)
+bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorControl& rhs)
 {
     for (int i = 0; i < 2; i++) {
         for (int j = 0; j < 8; j++) {
@@ -101,7 +101,7 @@ bool operator==(const Offboard::ActuatorControl &lhs, const Offboard::ActuatorCo
     return true;
 }
 
-std::ostream &operator<<(std::ostream &str, Offboard::ActuatorControl const &actuator_control)
+std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control)
 {
     return str << "[group: " << 0
                << ", Command port 0: " << actuator_control.groups[0].controls[0]
diff --git a/src/plugins/offboard/offboard_impl.cpp b/src/plugins/offboard/offboard_impl.cpp
index 1576929d26..bcaf531aaa 100644
--- a/src/plugins/offboard/offboard_impl.cpp
+++ b/src/plugins/offboard/offboard_impl.cpp
@@ -512,7 +512,7 @@ void OffboardImpl::send_attitude_rate()
     _parent->send_message(message);
 }
 
-void OffboardImpl::send_actuator_control_message(const float *controls, uint8_t group_number)
+void OffboardImpl::send_actuator_control_message(const float* controls, uint8_t group_number)
 {
     mavlink_message_t message;
     mavlink_msg_set_actuator_control_target_pack(
diff --git a/src/plugins/offboard/offboard_impl.h b/src/plugins/offboard/offboard_impl.h
index e0af169249..3e16d55835 100644
--- a/src/plugins/offboard/offboard_impl.h
+++ b/src/plugins/offboard/offboard_impl.h
@@ -45,7 +45,7 @@ class OffboardImpl : public PluginImplBase {
     void send_attitude_rate();
     void send_attitude();
     void send_actuator_control();
-    void send_actuator_control_message(const float *controls, uint8_t group_number = 0);
+    void send_actuator_control_message(const float* controls, uint8_t group_number = 0);
 
     void process_heartbeat(const mavlink_message_t& message);
     void receive_command_result(

From bd4370f0489904d0b551fb544cb87a758e429c53 Mon Sep 17 00:00:00 2001
From: Jonas Vautherin <jonas.vautherin@gmail.com>
Date: Tue, 16 Jul 2019 19:25:32 +0200
Subject: [PATCH 15/16] fix style

---
 src/plugins/offboard/offboard.cpp | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/plugins/offboard/offboard.cpp b/src/plugins/offboard/offboard.cpp
index 876af05d3f..1a77187029 100644
--- a/src/plugins/offboard/offboard.cpp
+++ b/src/plugins/offboard/offboard.cpp
@@ -103,8 +103,7 @@ bool operator==(const Offboard::ActuatorControl& lhs, const Offboard::ActuatorCo
 
 std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& actuator_control)
 {
-    return str << "[group: " << 0
-               << ", Command port 0: " << actuator_control.groups[0].controls[0]
+    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]
@@ -112,8 +111,7 @@ std::ostream& operator<<(std::ostream& str, Offboard::ActuatorControl const& act
                << ", 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]
+               << "[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]

From 63d2704367cc45eac93955b629a68ccef2c0ece2 Mon Sep 17 00:00:00 2001
From: Ildar Sadykov <irsdkv@gmail.com>
Date: Wed, 17 Jul 2019 11:44:23 +0300
Subject: [PATCH 16/16] Group struct docs

---
 src/plugins/offboard/include/plugins/offboard/offboard.h | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/plugins/offboard/include/plugins/offboard/offboard.h b/src/plugins/offboard/include/plugins/offboard/offboard.h
index 17c1c5dc3a..7aa3e62041 100644
--- a/src/plugins/offboard/include/plugins/offboard/offboard.h
+++ b/src/plugins/offboard/include/plugins/offboard/offboard.h
@@ -148,6 +148,10 @@ class Offboard : public PluginBase {
      * (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. */
         };