add calculating quaternion

src/px4_connection/src/px4_controller.cpp

@@ -28,13 +28,13 @@ public:
         // create a publisher on the vehicle attitude setpoint topic
         vehicle_setpoint_publisher_ = this->create_publisher<px4_msgs::msg::VehicleAttitudeSetpoint>("/fmu/in/vehicle_attitude_setpoint", 10);
         vehicle_command_publisher_ = this->create_publisher<px4_msgs::msg::VehicleCommand>("/fmu/in/vehicle_command", 10);
-        trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint",10);
+        trajectory_setpoint_publisher = this->create_publisher<px4_msgs::msg::TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);
 
         // set to offboard mode
         this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_DO_SET_MODE, 1, 6);
         RCLCPP_INFO(this->get_logger(), "Set to offboard mode");
         // arm the drone
-        this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0,0);
+        this->publish_vehicle_command(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.0, 0);
 
         RCLCPP_INFO(this->get_logger(), "Arm command sent");
 
@@ -57,14 +57,17 @@ private:
         // set message to enable attitude
         auto msg = px4_msgs::msg::VehicleAttitudeSetpoint();
 
-        msg.thrust_body[0] = 0; // north
-        msg.thrust_body[1] = 0; // east
-        msg.thrust_body[2] = -1; // down, 100% thrust up
+        msg.thrust_body[0] = 0;  // north
+        msg.thrust_body[1] = 0;  // east
+        msg.thrust_body[2] = 1; // down, 100% thrust up
 
-        msg.q_d[0] = 0;
-        msg.q_d[1] = 0.1;
-        msg.q_d[2] = 0.1;
-        msg.q_d[3] = 0.1;
+        auto array = std::make_unique<float[]>(4);
+        calculate_quaternion(array, 0, degrees_to_radians(20), 0);
+
+        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[])