diff --git a/src/sleep.c b/src/sleep.c
index 03976a0be..e6b604ccd 100644
--- a/src/sleep.c
+++ b/src/sleep.c
@@ -38,7 +38,7 @@
 #include <unistd.h>
 #include <errno.h>
 #include <pthread.h>
-#include "config.h"
+#include "hamlib/config.h"
 #include "sleep.h"
 
 #ifdef  __cplusplus
@@ -49,28 +49,41 @@ extern double monotonic_seconds();
 
 int hl_usleep(rig_useconds_t usec)
 {
-    double sleep_time = usec / 1e9;
+    double sleep_time = usec / 1e6;
     struct timespec tv1, tv2;
     double start_at = monotonic_seconds();
     double end_at = start_at + sleep_time;
-    double delay = sleep_time - 0.010;
+    double delay = sleep_time;
     double lasterr = 0;
 
+    if (sleep_time > .001) { sleep_time -= .001; }
+    else if (sleep_time > .0001) { sleep_time -= .0005; }
+
     tv1.tv_sec = (time_t) delay;
-    tv1.tv_nsec = (long) ((delay - tv1.tv_sec) * 1e+9);
+    tv1.tv_nsec = (long)((delay - tv1.tv_sec) * 1e+9);
     tv2.tv_sec = 0;
     tv2.tv_nsec = 10;
+    rig_debug(RIG_DEBUG_VERBOSE,"usec=%ld, sleep_time=%f, tv1=%ld,%ld\n", usec, sleep_time, (long)tv1.tv_sec,
+           (long)tv1.tv_nsec);
 
 #ifdef __WIN32__
     timeBeginPeriod(1);
-    nanosleep (&tv1, NULL);
+    nanosleep(&tv1, NULL);
+
     while ((lasterr = end_at - monotonic_seconds()) > 0)
+    {
         nanosleep(&tv2, NULL);
+    }
+
     timeEndPeriod(1);
 #else
-    nanosleep (&tv1, NULL);
+    nanosleep(&tv1, NULL);
+
     while (((lasterr = end_at - monotonic_seconds()) > 0))
+    {
         nanosleep(&tv2, NULL);
+    }
+
 #endif
     return 0;
 }
@@ -83,7 +96,8 @@ int hl_usleep(rig_useconds_t usec)
 {
     int retval = 0;
     rig_debug(RIG_DEBUG_ERR, "%s: usec=%ld\n", __func__, usec);
-    if (usec <= 1000) return 0; // dont' sleep if only 1ms is requested -- speeds things up on Windows
+
+    if (usec <= 1000) { return 0; } // dont' sleep if only 1ms is requested -- speeds things up on Windows
 
     while (usec > 1000000)
     {
@@ -92,10 +106,11 @@ int hl_usleep(rig_useconds_t usec)
         retval = usleep(1000000);
         usec -= 1000000;
     }
+
 #ifdef HAVE_NANOSLEEP
     struct timespec t, tleft;
-    t.tv_sec = usec/1e6;
-    t.tv_nsec = (usec - (t.tv_sec*1e6)) * 1e3;
+    t.tv_sec = usec / 1e6;
+    t.tv_nsec = (usec - (t.tv_sec * 1e6)) * 1e3;
     return nanosleep(&t, &tleft);
 #else
     return usleep(usec);
@@ -155,3 +170,34 @@ int usleep(rig_useconds_t usec)
 }
 #endif
 /** @} */
+
+#ifdef TEST
+#include "misc.h"
+double get_elapsed_time(struct tm start, struct tm end) {
+    // Convert struct tm to time_t
+    time_t start_seconds = mktime(&start);
+    time_t end_seconds = mktime(&end);
+
+    double elapsed_time = difftime(end_seconds, start_seconds);
+    return elapsed_time;
+}
+
+
+int main()
+{
+    struct tm start_time, end_time;
+    time_t rawtime;
+
+    for (int i = 0; i < 11; ++i)
+    {
+        char buf[256];
+        time(&rawtime);
+        hl_usleep(1000000 * 1000); // test 1s sleep
+        date_strget(buf, sizeof(buf), 0);
+        printf("%s\n", buf);
+        time(&rawtime);
+    }
+
+    return 0;
+}
+#endif