From 1f5323ebf2c00bbf6791f4c85124eadb77358a35 Mon Sep 17 00:00:00 2001 From: Eric Miller Date: Wed, 29 Nov 2017 22:21:51 -0500 Subject: [PATCH] Fixed error with odometry caused by clockwise angles reported from AR Drone --- src/ardrone_driver.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/ardrone_driver.cpp b/src/ardrone_driver.cpp index 57d15049..62292cd3 100644 --- a/src/ardrone_driver.cpp +++ b/src/ardrone_driver.cpp @@ -686,11 +686,12 @@ void ARDroneDriver::PublishOdometry(const navdata_unpacked_t &navdata_raw, const if (last_receive_time.isValid()) { double delta_t = (navdata_receive_time - last_receive_time).toSec(); - odometry[0] += ((cos((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) * - navdata_raw.navdata_demo.vx - sin((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) * + double yaw_angle = -(navdata_raw.navdata_demo.psi / 180000.0) * M_PI; + odometry[0] += ((cos(yaw_angle) * + navdata_raw.navdata_demo.vx - sin(yaw_angle) * -navdata_raw.navdata_demo.vy) * delta_t) / 1000.0; - odometry[1] += ((sin((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) * - navdata_raw.navdata_demo.vx + cos((navdata_raw.navdata_demo.psi / 180000.0) * M_PI) * + odometry[1] += ((sin(yaw_angle) * + navdata_raw.navdata_demo.vx + cos(yaw_angle) * -navdata_raw.navdata_demo.vy) * delta_t) / 1000.0; } last_receive_time = navdata_receive_time;