Added SetAttitudeTarget message to the off board implementation (same as #621)

integration_tests/CMakeLists.txt

@@ -31,6 +31,7 @@ add_executable(integration_tests_runner
     follow_me.cpp
     gimbal.cpp
     info.cpp
+    offboard_attitude.cpp
     #logging.cpp # Not fully implemented
     log_files.cpp
     mission_cancellation.cpp

integration_tests/offboard_attitude.cpp

@@ -0,0 +1,197 @@
+#include <iostream>
+#include <cmath>
+#include "integration_test_helper.h"
+#include "dronecode_sdk.h"
+#include "plugins/action/action.h"
+#include "plugins/telemetry/telemetry.h"
+#include "plugins/offboard/offboard.h"
+
+struct Rates {
+    float roll;
+    float pitch;
+    float yaw;
+    float thrust;
+};
+using namespace dronecode_sdk;
+Rates close_loop(float, float, float, std::shared_ptr<Telemetry> telemetry);
+
+TEST_F(SitlTest, OffboardAttitudeRate)
+{
+    DronecodeSDK dc;
+
+    ConnectionResult ret =
+        dc.add_udp_connection("udp://:14540"); // For connecting with Jmavsim simulator.
+    ASSERT_EQ(ConnectionResult::SUCCESS, ret);
+
+    // Wait for system to connect via heartbeat.
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    System &system = dc.system();
+    auto telemetry = std::make_shared<Telemetry>(system);
+    auto action = std::make_shared<Action>(system);
+    auto offboard = std::make_shared<Offboard>(system);
+
+    while (!telemetry->health_all_ok()) {
+        std::cout << "waiting for system to be ready" << std::endl;
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    Action::Result action_ret = action->arm();
+    ASSERT_EQ(Action::Result::SUCCESS, action_ret);
+
+    action_ret = action->takeoff();
+    ASSERT_EQ(Action::Result::SUCCESS, action_ret);
+
+    // Taking off and reaching an altitude of 2.5mts
+    std::this_thread::sleep_for(std::chrono::seconds(3));
+
+    // Send it once before starting offboard, otherwise it will be rejected.
+    offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 0.0f});
+    // Printing Pitch, roll and yaw euler angles.
+    std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+              << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+              << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+
+    // Starting Offboard Mode
+    Offboard::Result offboard_result = offboard->start();
+    EXPECT_EQ(offboard_result, Offboard::Result::SUCCESS);
+
+    // Gaining altitude
+    offboard->set_attitude_rate({0.0f, 0.0f, 0.0f, 0.5f});
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    // Performing Acutal tests
+    // Testing Roll
+    std::cout << "Rolling at 45 degrees/seconds" << std::endl;
+    offboard->set_attitude_rate({45.0f, 0.0f, 0.0f, 0.5f});
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().roll_deg <= 45.0f) {
+            auto roll_deg = close_loop(45.0f, 0.0f, 0.0f, telemetry);
+            offboard->set_attitude_rate(
+                {roll_deg.roll, roll_deg.pitch, roll_deg.yaw, roll_deg.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        } else {
+            std::cout << "Roll Angle Reached" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+
+    // Balancing
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().roll_deg >= 0.2f) {
+            auto roll_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
+            offboard->set_attitude_rate(
+                {roll_bal.roll, roll_bal.pitch, roll_bal.yaw, roll_bal.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        } else {
+            std::cout << "Balancing Roll" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+
+    // Testing Pitch
+    std::cout << "Pitching at 45 degrees/second" << std::endl;
+    offboard->set_attitude_rate({0.0f, 45.0f, 0.0f, 0.5f});
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().pitch_deg <= 45.0f) {
+            auto pitch_deg = close_loop(0.0f, 45.0f, 0.0f, telemetry);
+            offboard->set_attitude_rate(
+                {pitch_deg.roll, pitch_deg.pitch, pitch_deg.yaw, pitch_deg.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(3));
+        } else {
+            std::cout << "Pitch angle achieved" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+
+    // Balancing
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().pitch_deg > 0.2f) {
+            auto pitch_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
+            offboard->set_attitude_rate(
+                {pitch_bal.roll, pitch_bal.pitch, pitch_bal.yaw, pitch_bal.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        } else {
+            std::cout << "Pitch Balanced" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+
+    // Testing Yaw
+    std::cout << "Yaw at -60 degrees/second" << std::endl;
+    offboard->set_attitude_rate({0.0f, 0.0f, -60.0f, 0.5f});
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().yaw_deg >=
+            -60.0f) // Flipping the comparison operator while turning anti-clock wise.
+        {
+            auto yaw_deg = close_loop(0.0f, 0.0f, -60.0f, telemetry);
+            offboard->set_attitude_rate({yaw_deg.roll, yaw_deg.pitch, yaw_deg.yaw, yaw_deg.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(5));
+        } else {
+            std::cout << "Yaw angle acheived" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+    std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+              << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+              << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+
+    // Balancing
+    for (int i = 0; i < 1000; i++) {
+        if (telemetry->attitude_euler_angle().yaw_deg <= 0.2f) {
+            auto yaw_bal = close_loop(0.0f, 0.0f, 0.0f, telemetry);
+            offboard->set_attitude_rate({yaw_bal.roll, yaw_bal.pitch, yaw_bal.yaw, yaw_bal.thrust});
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        } else {
+            std::cout << "Balanced Yaw" << std::endl;
+            std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+                      << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+                      << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+            break;
+        }
+    }
+
+    // Stopping offboard mode and Landing.
+    offboard_result = offboard->stop();
+    EXPECT_EQ(offboard_result, Offboard::Result::SUCCESS);
+    action_ret = action->land();
+    std::cout << "Landed! " << std::endl;
+
+    std::this_thread::sleep_for(std::chrono::seconds(3));
+    std::cout << "Landed" << std::endl;
+    std::cout << "Roll: " << telemetry->attitude_euler_angle().roll_deg
+              << " Pitch: " << telemetry->attitude_euler_angle().pitch_deg
+              << " Yaw: " << telemetry->attitude_euler_angle().yaw_deg << std::endl;
+    EXPECT_EQ(action_ret, Action::Result::SUCCESS);
+}
+
+// Creating a closing function:
+Rates close_loop(float roll_rate,
+                 float pitch_rate,
+                 float yaw_rate,
+                 std::shared_ptr<Telemetry> telemetry)
+{
+    double roll_control = 6.0 * (double)(roll_rate - telemetry->attitude_euler_angle().roll_deg);
+    double pitch_control = 7.2 * (double)(pitch_rate - telemetry->attitude_euler_angle().pitch_deg);
+    double yaw_control = 3.80 * (double)(yaw_rate - telemetry->attitude_euler_angle().yaw_deg);
+    double thrust_control = 0.1 * (double)(10.5f - telemetry->position().relative_altitude_m);
+    if (thrust_control < 0.1) {
+        thrust_control = 0.10;
+    }
+    return {(float)roll_control, (float)pitch_control, (float)yaw_control, (float)(thrust_control)};
+}

plugins/action/action_impl.cpp

@@ -56,7 +56,7 @@ Action::Result ActionImpl::arm() const
         return ret;
     }
 
-    // Go to LOITER mode first.
+    // Go to LOITER mode first. // For No GPS mode with vision system comment the next 5 lines.
     ret = action_result_from_command_result(_parent->set_flight_mode(SystemImpl::FlightMode::HOLD));
 
     if (ret != Action::Result::SUCCESS) {

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

@@ -93,6 +93,21 @@ class Offboard : public PluginBase {
                                  clock-wise looking from above). */
     };
 
+    /**
+     * @brief Type for Attitude rate commands in body coordinates (roll, pitch, yaw  angular
+     * rate and Thrust).
+     */
+    struct AttitudeRate {
+        float roll_deg_s; /**< @brief Roll Angular Rate in degrees/second (positive for clock-wise
+                             looking from front). */
+        float pitch_deg_s; /**< @brief Pitch Angular Rate in degrees/second (positive for head/front
+                              moving up). */
+        float yaw_deg_s; /**< @brief Yaw Angular Rate in degrees/second (positive for clock-wise
+                            looking from above). */
+        float thrust_value; /**< @brief Thrust in percentage ranging from 0 to 1 ( 0 to 100
+                               percent). */
+    };
+
     /**
      * @brief Start offboard control (synchronous).
      *
@@ -153,6 +168,14 @@ class Offboard : public PluginBase {
      */
     void set_velocity_body(VelocityBodyYawspeed velocity_body_yawspeed);
 
+    /**
+     * @brief Set the attitude rate in terms of pitch, roll and yaw angular rate along with thrust
+     * in percentage.
+     *
+     * @param attitude_rate roll, pitch and yaw angular rate along with thrust in percentage.
+     */
+    void set_attitude_rate(AttitudeRate attitude_rate);
+
     /**
      * @brief Copy constructor (object is not copyable).
      */

plugins/offboard/offboard.cpp

@@ -42,6 +42,11 @@ void Offboard::set_velocity_body(Offboard::VelocityBodyYawspeed velocity_body_ya
     return _impl->set_velocity_body(velocity_body_yawspeed);
 }
 
+void Offboard::set_attitude_rate(Offboard::AttitudeRate attitude_rate)
+{
+    return _impl->set_attitude_rate(attitude_rate);
+}
+
 const char *Offboard::result_str(Result result)
 {
     switch (result) {

plugins/offboard/offboard_impl.cpp

@@ -160,6 +160,33 @@ void OffboardImpl::set_velocity_body(Offboard::VelocityBodyYawspeed velocity_bod
     send_velocity_body();
 }
 
+void OffboardImpl::set_attitude_rate(Offboard::AttitudeRate attitude_rate)
+{
+    _mutex.lock();
+    _attitude_rate = attitude_rate;
+
+    if (_mode != Mode::ATTITUDE_RATE) {
+        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 body setpoints from now on.
+        _parent->add_call_every(
+            [this]() { send_attitude_rate(); }, SEND_INTERVAL_S, &_call_every_cookie);
+
+        _mode = Mode::ATTITUDE_RATE;
+    } else {
+        // We're already sending these kind of setpoints. Since the setpoint change, let's
+        // reschedule the next call, so we don't send setpoints too often.
+        _parent->reset_call_every(_call_every_cookie);
+    }
+    _mutex.unlock();
+
+    // also send it right now to reduce latency
+    send_attitude_rate();
+}
+
 void OffboardImpl::send_velocity_ned()
 {
     const static uint16_t IGNORE_X = (1 << 0);
@@ -266,6 +293,41 @@ void OffboardImpl::send_velocity_body()
     _parent->send_message(message);
 }
 
+void OffboardImpl::send_attitude_rate()
+{
+    // const static uint8_t IGNORE_BODY_ROLL_RATE = (1 << 0);
+    // const static uint8_t IGNORE_BODY_PITCH_RATE = (1 << 1);
+    // const static uint8_t IGNORE_BODY_YAW_RATE = (1 << 2);
+    // const static uint8_t IGNORE_4 = (1 << 3);
+    // const static uint8_t IGNORE_5 = (1 << 4);
+    // const static uint8_t IGNORE_6 = (1 << 5);
+    // const static uint8_t IGNORE_THRUST = (1 << 6);
+    const static uint8_t IGNORE_ATTITUDE = (1 << 7);
+
+    _mutex.lock();
+    const float thrust = _attitude_rate.thrust_value;
+    const float body_roll_rate = to_rad_from_deg(_attitude_rate.roll_deg_s);
+    const float body_pitch_rate = to_rad_from_deg(_attitude_rate.pitch_deg_s);
+    const float body_yaw_rate = to_rad_from_deg(_attitude_rate.yaw_deg_s);
+    _mutex.unlock();
+
+    mavlink_message_t message;
+    mavlink_msg_set_attitude_target_pack(
+        GCSClient::system_id,
+        GCSClient::component_id,
+        &message,
+        static_cast<uint32_t>(_parent->get_time().elapsed_s() * 1e3),
+        _parent->get_system_id(),
+        _parent->get_autopilot_id(),
+        IGNORE_ATTITUDE,
+        0,
+        body_roll_rate,
+        body_pitch_rate,
+        body_yaw_rate,
+        thrust);
+    _parent->send_message(message);
+}
+
 void OffboardImpl::process_heartbeat(const mavlink_message_t &message)
 {
     mavlink_heartbeat_t heartbeat;

plugins/offboard/offboard_impl.h

@@ -29,13 +29,15 @@ class OffboardImpl : public PluginImplBase {
 
     void set_velocity_ned(Offboard::VelocityNEDYaw velocity_ned_yaw);
     void set_velocity_body(Offboard::VelocityBodyYawspeed velocity_body_yawspeed);
+    void set_attitude_rate(Offboard::AttitudeRate attitude_rate);
 
     OffboardImpl(const OffboardImpl &) = delete;
     OffboardImpl &operator=(const OffboardImpl &) = delete;
 
 private:
     void send_velocity_ned();
     void send_velocity_body();
+    void send_attitude_rate();
 
     void process_heartbeat(const mavlink_message_t &message);
     void receive_command_result(MAVLinkCommands::Result result,
@@ -46,9 +48,15 @@ class OffboardImpl : public PluginImplBase {
     void stop_sending_setpoints();
 
     mutable std::mutex _mutex{};
-    enum class Mode { NOT_ACTIVE, VELOCITY_NED, VELOCITY_BODY } _mode = Mode::NOT_ACTIVE;
+    enum class Mode {
+        NOT_ACTIVE,
+        VELOCITY_NED,
+        VELOCITY_BODY,
+        ATTITUDE_RATE
+    } _mode = Mode::NOT_ACTIVE;
     Offboard::VelocityNEDYaw _velocity_ned_yaw{};
     Offboard::VelocityBodyYawspeed _velocity_body_yawspeed{};
+    Offboard::AttitudeRate _attitude_rate{};
 
     void *_call_every_cookie = nullptr;