sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add base_q amount

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-03 16:40:59 +02:00
parent 2b287c561d
commit 78d5221633
1 changed files with 11 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/px4_connection/src/px4_controller.cpp
View File

@@ -82,6 +82,9 @@ private:
float last_angle = 0; float last_angle = 0;
float last_thrust = 0; float last_thrust = 0;
float base_q[4] = {0, 0, 0, 0};
int base_q_amount = 0;
// result quaternion // result quaternion
std::array<float, 4> q = {0, 0, 0, 0}; std::array<float, 4> q = {0, 0, 0, 0};
@@ -250,7 +253,14 @@ private:
void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg) void on_attitude_receive(const px4_msgs::msg::VehicleAttitude::SharedPtr msg)
{ {
if (!ready_to_fly)
{
base_q_amount++;
base_q[0] = (base_q[0] + msg->q[0])/base_q_amount;
base_q[1] = (base_q[1] + msg->q[1])/base_q_amount;
base_q[2] = (base_q[2] + msg->q[2])/base_q_amount;
base_q[3] = (base_q[3] + msg->q[3])/base_q_amount;
}
} }
/** /**