FIx dead reckoning

KlemenDEV · Nov 22, 2021 · ab912b7 · ab912b7
1 parent 5f3d1e4
commit ab912b7
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 6 deletions.
diff --git 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
@@ -18,8 +18,6 @@ PoseManager::PoseManager(ros::NodeHandle *nh) {
     set_datum_client_sim = nh->serviceClient<velocity_integrator::SetDatum>("/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;
 }