From ab912b71368458f87f93306fd9fea98221255d5e Mon Sep 17 00:00:00 2001 From: KlemenDEV Date: Mon, 22 Nov 2021 22:26:31 +0100 Subject: [PATCH] FIx dead reckoning --- src/localizers/dead_reckoning/src/dead_reckoning.cc | 10 +++++++--- src/velocity_integrator/src/PoseManager.cc | 4 +--- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/localizers/dead_reckoning/src/dead_reckoning.cc b/src/localizers/dead_reckoning/src/dead_reckoning.cc index 986d20d7..b7128874 100644 --- a/src/localizers/dead_reckoning/src/dead_reckoning.cc +++ b/src/localizers/dead_reckoning/src/dead_reckoning.cc @@ -29,6 +29,7 @@ int counter = 0; double vx = 0, vy = 0; double height_last = 0; +int hcount = 0; void heightCallback(const std_msgs::Float64::ConstPtr &msg) { height_last = msg->data; @@ -43,6 +44,9 @@ void imuDataCallback(const sensor_msgs::Imu::ConstPtr &imu_msg) { if (t_last == -1) { if (height_last > 6) + hcount++; + + if (hcount > 3000) t_last = imu_msg->header.stamp.toSec(); return; } @@ -53,7 +57,7 @@ void imuDataCallback(const sensor_msgs::Imu::ConstPtr &imu_msg) { double ay = imu_msg->linear_acceleration.y - 0.163474; double az = imu_msg->linear_acceleration.z; - double axl = ax * cos(pitch) + ay * sin (roll) * sin(pitch) - az * cos(roll) * sin(pitch); + double axl = ax * cos(pitch) + ay * sin(roll) * sin(pitch) - az * cos(roll) * sin(pitch); double ayl = ax * 0 + ay * cos(roll) + az * sin(roll); if (std::abs(ax) > 0.3) @@ -66,8 +70,8 @@ void imuDataCallback(const sensor_msgs::Imu::ConstPtr &imu_msg) { geometry_msgs::TwistWithCovarianceStamped velocitymsg; velocitymsg.header.frame_id = "uav_velocity"; velocitymsg.header.stamp = imu_msg->header.stamp; - velocitymsg.twist.twist.linear.x = vx; - velocitymsg.twist.twist.linear.y = vy; + velocitymsg.twist.twist.linear.x = -vy; + velocitymsg.twist.twist.linear.y = -vx; publisher_velocity.publish(velocitymsg); } diff --git a/src/velocity_integrator/src/PoseManager.cc b/src/velocity_integrator/src/PoseManager.cc index c51606cd..0c37db52 100644 --- a/src/velocity_integrator/src/PoseManager.cc +++ b/src/velocity_integrator/src/PoseManager.cc @@ -18,8 +18,6 @@ PoseManager::PoseManager(ros::NodeHandle *nh) { set_datum_client_sim = nh->serviceClient("/datum_sim"); } -#define ALPHA 1.0 - void PoseManager::imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg) { orientation_last = msg->orientation; @@ -29,7 +27,7 @@ void PoseManager::imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg) { double r, p, yaw; gps_tf2_rot.getRPY(r, p, yaw, 1); - yaw_mag_curr = (yaw * ALPHA) + (1.0 - ALPHA) * yaw_mag_curr; + yaw_mag_curr = yaw; if (datum_set) yaw_last = yaw_mag_init - yaw_mag_curr; }