From f3f1e72b0bef123b48691bf2e5fbb1add995e35d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 22 Jul 2021 15:28:18 +1000 Subject: [PATCH] Copter: Fix guided yaw bug. # Conflicts: # ArduCopter/GCS_Mavlink.cpp --- ArduCopter/autoyaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/autoyaw.cpp b/ArduCopter/autoyaw.cpp index ec2d1d3c08..ae34ebc46b 100644 --- a/ArduCopter/autoyaw.cpp +++ b/ArduCopter/autoyaw.cpp @@ -198,7 +198,7 @@ float Mode::AutoYaw::yaw() // keep heading pointing in the direction held in fixed_yaw // with no pilot input allowed const uint32_t now_ms = millis(); - float dt = now_ms - _last_update_ms; + float dt = (now_ms - _last_update_ms) * 0.001; _last_update_ms = now_ms; float yaw_angle_step = constrain_float(_fixed_yaw_offset_cd, - dt * _fixed_yaw_slewrate_cds, dt * _fixed_yaw_slewrate_cds); _fixed_yaw_offset_cd -= yaw_angle_step; @@ -225,7 +225,7 @@ float Mode::AutoYaw::yaw() case AUTO_YAW_ANGLE_RATE:{ const uint32_t now_ms = millis(); - float dt = now_ms - _last_update_ms; + float dt = (now_ms - _last_update_ms) * 0.001; _last_update_ms = now_ms; _yaw_angle_cd += _yaw_rate_cds * dt; return _yaw_angle_cd;