diff --git a/src/segway_RMP400.cpp b/src/segway_RMP400.cpp index 1ddb630f2a22184508cbdc7fbba70f837ae7c17d..de5f2456bf86613f38cf93e3b157c7727ba4ad9b 100644 --- a/src/segway_RMP400.cpp +++ b/src/segway_RMP400.cpp @@ -192,13 +192,13 @@ CSegwayRMP400::get_yaw_displacement() { need_to_be_connected(); - float yaw_displacement_mts = (segways_[0]->get_yaw_displacement() + + float yaw_displacement_tmp = (segways_[0]->get_yaw_displacement() + segways_[1]->get_yaw_displacement()) / 2; if (use_correction_wheel_factor_) - fix_with_wheel_factor(yaw_displacement_mts); + fix_with_wheel_factor(yaw_displacement_tmp); - yaw_displacement_ = yaw_displacement_mts * 2 * PI; // convert to radians + yaw_displacement_ = yaw_displacement_tmp * 2 * PI; // convert to radians return yaw_displacement_; }