add calculating quaternion

src/px4_connection/src/px4_controller.cpp

@@ -59,12 +59,15 @@ private:
 
         msg.thrust_body[0] = 0;  // north
         msg.thrust_body[1] = 0;  // east
-        msg.thrust_body[2] = -1; // down, 100% thrust up
+        msg.thrust_body[2] = 1; // down, 100% thrust up
 
-        msg.q_d[0] = 0;
+        auto array = std::make_unique<float[]>(4);
-        msg.q_d[1] = 0.1;
+        calculate_quaternion(array, 0, degrees_to_radians(20), 0);
-        msg.q_d[2] = 0.1;
+
-        msg.q_d[3] = 0.1;
+        msg.q_d[0] = array[0];
+        msg.q_d[1] = array[1];
+        msg.q_d[2] = array[2];
+        msg.q_d[3] = array[3];
 
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
         vehicle_setpoint_publisher_->publish(msg);
@@ -92,6 +95,52 @@ private:
         msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
         vehicle_command_publisher_->publish(msg);
     }
+
+
+    /**
+     * @brief calculates a quaternion based on the given input values
+     * 
+     * @param ptr pointer to result quaternion. Must be of size 4.
+     * @param heading desired heading (yaw) in radians.
+     * @param attitude desired attitude (pitch) in radians.
+     * @param bank desired bank (roll) in radians.
+     * @return -1 if the size of the quaternion was not correct, 1 if success.
+     */
+    static char calculate_quaternion(std::unique_ptr<float[]> ptr, float heading, float attitude, float bank)
+    {
+        if (sizeof(ptr.get()) / sizeof(float) != 4)
+        {
+            return -1;
+        }
+
+        float c1 = cos(heading / 2);
+        float c2 = cos(attitude / 2);
+        float c3 = cos(bank / 2);
+        float s1 = sin(heading / 2);
+        float s2 = sin(attitude / 2);
+        float s3 = sin(bank / 2);
+
+        float c1c2 = c1 * c2;
+        float s1s2 = s1 * s2;
+
+        ptr[0] = c1c2 * c3 - s1s2 * s3;       // w
+        ptr[1] = c1c2 * s3 + s1s2 * c3;       // x
+        ptr[2] = s1 * c2 * c3 + c1 * s2 * s3; // y
+        ptr[3] = c1 * s2 * c3 - s1 * c2 * s3; // z
+
+        return 1;
+    }
+
+    /**
+     * @brief converts degrees to radians
+     * 
+     * @param deg angle in degrees
+     * @return float new angle in radians
+     */
+    static float degrees_to_radians(float deg)
+    {
+        return deg * (M_PI / 180.0);
+    }
 };
 
 int main(int argc, char *argv[])