FHSS WS: time convert to support negative values

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -43,23 +43,23 @@ void (*fhss_uc_switch)(void) = NULL;
 void (*fhss_bc_switch)(void) = NULL;
 #endif /*FHSS_CHANNEL_DEBUG_CBS*/
 // Seconds to milliseconds
-#define S_TO_MS(x) (((uint32_t)x)*1000)
+#define S_TO_MS(x) (((int32_t)x)*1000)
 // Milliseconds to seconds
 #define MS_TO_S(x) divide_integer(x, 1000)
 // Seconds to microseconds
-#define S_TO_US(x) (((uint32_t)x)*1000000)
+#define S_TO_US(x) (((int32_t)x)*1000000)
 // Microseconds to seconds
 #define US_TO_S(x) divide_integer(x, 1000000)
 // Milliseconds to microseconds
-#define MS_TO_US(x) (((uint32_t)x)*1000)
+#define MS_TO_US(x) (((int32_t)x)*1000)
 // Microseconds to milliseconds
 #define US_TO_MS(x) divide_integer(x, 1000)
 // Milliseconds to nanoseconds
-#define MS_TO_NS(x) (((uint32_t)x)*1000000)
+#define MS_TO_NS(x) (((int32_t)x)*1000000)
 // Nanoseconds to milliseconds
 #define NS_TO_MS(x) divide_integer(x, 1000000)
 // Microseconds to nanoseconds
-#define US_TO_NS(x) (((uint32_t)x)*1000)
+#define US_TO_NS(x) (((int32_t)x)*1000)
 // Nanoseconds to microseconds
 #define NS_TO_US(x) divide_integer(x, 1000)
 #define DEF_2E24 0x1000000
@@ -85,8 +85,11 @@ static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure);
 static uint8_t fhss_set_txrx_slot_length(fhss_structure_t *fhss_structure);
 
 // This function supports rounding up
-static uint32_t divide_integer(uint32_t dividend, uint32_t divisor)
+static int32_t divide_integer(int32_t dividend, int32_t divisor)
 {
+    if (dividend < 0) {
+        return (dividend - divisor / 2) / divisor;
+    }
     return (dividend + divisor / 2) / divisor;
 }
 
@@ -778,14 +781,8 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     fhss_structure->ws->parent_bc_info = bc_timing_info;
     if (prev_synchronization_time) {
         if (SYNCH_COMPENSATION_MIN_INTERVAL <= US_TO_S(time_since_last_synch_us)) {
-            // Compensate clock drift
-            if (true_bc_interval_offset > own_bc_interval_offset) {
-                // Our clock runs too slow compared to parent, mark as positive drift.
-                fhss_structure->ws->drift_per_millisecond_ns += divide_integer(MS_TO_NS(true_bc_interval_offset - own_bc_interval_offset), US_TO_MS(time_since_last_synch_us));
-            } else {
-                // Our clock runs too fast compared to parent, mark as negative drift.
-                fhss_structure->ws->drift_per_millisecond_ns -= divide_integer(MS_TO_NS(own_bc_interval_offset - true_bc_interval_offset), US_TO_MS(time_since_last_synch_us));
-            }
+            // Update clock drift
+            fhss_structure->ws->drift_per_millisecond_ns += divide_integer(MS_TO_NS(true_bc_interval_offset - own_bc_interval_offset), US_TO_MS(time_since_last_synch_us));
         }
         tr_debug("synch to parent: %s, drift: %ims in %u seconds, compensation: %ins per ms", trace_array(eui64, 8), true_bc_interval_offset - own_bc_interval_offset, US_TO_S(time_since_last_synch_us), fhss_structure->ws->drift_per_millisecond_ns);
     }