diff --git a/Rover/mode.cpp b/Rover/mode.cpp index ae858691ea3..20fa99906a6 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -175,7 +175,7 @@ void Mode::get_pilot_desired_heading_and_speed(float &heading_out, float &speed_ } // calculate angle of input stick vector - heading_out = wrap_360_cd(atan2f(desired_steering, desired_throttle) * DEGX100); + heading_out = wrap_360_cd(rad_to_cd(atan2f(desired_steering, desired_throttle))); // calculate throttle using magnitude of input stick vector const float throttle = MIN(safe_sqrt(sq(desired_throttle) + sq(desired_steering)), 1.0f); diff --git a/Rover/mode_follow.cpp b/Rover/mode_follow.cpp index bf36049b5ac..f63b5a49be7 100644 --- a/Rover/mode_follow.cpp +++ b/Rover/mode_follow.cpp @@ -67,7 +67,7 @@ void ModeFollow::update() } // calculate vehicle heading - const float desired_yaw_cd = wrap_180_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x) * DEGX100); + const float desired_yaw_cd = wrap_180_cd(rad_to_cd(atan2f(desired_velocity_ne.y, desired_velocity_ne.x))); // run steering and throttle controllers calc_steering_to_heading(desired_yaw_cd);