pbio/util: Move time comparison util.

This is not only useful for motor control code.

Author: laurensvalk

lib/pbio/include/pbio/control_settings.h

@@ -135,7 +135,6 @@ int32_t pbio_control_settings_app_to_ctl(const pbio_control_settings_t *s, int32
 void pbio_control_settings_app_to_ctl_long(const pbio_control_settings_t *s, int32_t input, pbio_angle_t *output);
 int32_t pbio_control_settings_actuation_ctl_to_app(int32_t input);
 int32_t pbio_control_settings_actuation_app_to_ctl(int32_t input);
-bool pbio_control_settings_time_is_later(uint32_t sample, uint32_t base);
 
 // Scale values by given constants:
 

lib/pbio/include/pbio/util.h

@@ -130,6 +130,8 @@ void pbio_uuid128_reverse_copy(uint8_t *dst, const uint8_t *src);
 
 bool pbio_oneshot(bool value, bool *state);
 
+bool pbio_util_time_has_passed(uint32_t sample, uint32_t base);
+
 #endif // _PBIO_UTIL_H_
 
 /** @} */

lib/pbio/src/control.c

@@ -14,6 +14,7 @@
 #include <pbio/int_math.h>
 #include <pbio/trajectory.h>
 #include <pbio/integrator.h>
+#include <pbio/util.h>
 
 /**
  * Gets the wall time in control unit time ticks (1e-4 seconds).
@@ -108,7 +109,7 @@ static bool pbio_control_check_completion(const pbio_control_t *ctl, uint32_t ti
     }
 
     // Check if we are passed the nominal maneuver time.
-    bool time_completed = pbio_control_settings_time_is_later(time, end->time);
+    bool time_completed = pbio_util_time_has_passed(time, end->time);
 
     if (pbio_control_type_is_time(ctl)) {
         // Infinite maneuvers are always done (should never block).
@@ -354,7 +355,7 @@ void pbio_control_update(
         // without resetting any controllers. This avoids accumulating errors
         // in sequential relative maneuvers.
         (pbio_control_on_completion_is_passive_smart(ctl->on_completion) &&
-         !pbio_control_settings_time_is_later(ref->time, ref_end.time + ctl->settings.smart_passive_hold_time))) {
+         !pbio_util_time_has_passed(ref->time, ref_end.time + ctl->settings.smart_passive_hold_time))) {
         // Keep actuating, so apply calculated PID torque value.
         *actuation = PBIO_DCMOTOR_ACTUATION_TORQUE;
         *control = torque;

lib/pbio/src/control_settings.c

@@ -165,17 +165,6 @@ int32_t pbio_control_settings_mul_by_loop_time(int32_t input) {
     return input / (1000 / PBIO_CONFIG_CONTROL_LOOP_TIME_MS);
 }
 
-/**
- * Checks if a time sample is equal to or newer than a given base time stamp.
- *
- * @param [in] sample         Sample time.
- * @param [in] base           Base time to compare to.
- * @return                    True if sample time is equal to or newer than base time, else false.
- */
-bool pbio_control_settings_time_is_later(uint32_t sample, uint32_t base) {
-    return sample - base < UINT32_MAX / 2;
-}
-
 /**
  * Gets the control limits for movement and actuation, in application units.
  *

lib/pbio/src/integrator.c

@@ -11,6 +11,7 @@
 #include <pbio/control_settings.h>
 #include <pbio/integrator.h>
 #include <pbio/int_math.h>
+#include <pbio/util.h>
 
 /**
  * Pauses the speed integrator at the current position error.
@@ -124,7 +125,7 @@ bool pbio_speed_integrator_stalled(const pbio_speed_integrator_t *itg, uint32_t
     }
 
     // If the integrator is paused for less than the stall time, we're still not stalled for now.
-    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
+    if (!pbio_util_time_has_passed(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
         return false;
     }
 
@@ -299,7 +300,7 @@ bool pbio_position_integrator_stalled(const pbio_position_integrator_t *itg, uin
     }
 
     // If the integrator is paused for less than the stall time, we're still not stalled for now.
-    if (!pbio_control_settings_time_is_later(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
+    if (!pbio_util_time_has_passed(time_now, itg->time_pause_begin + itg->settings->stall_time)) {
         return false;
     }
 

lib/pbio/src/util.c

@@ -80,3 +80,14 @@ bool pbio_oneshot(bool value, bool *state) {
 
     return ret;
 }
+
+/**
+ * Checks if a time sample is equal to or newer than a given base time stamp.
+ *
+ * @param [in] sample         Sample time.
+ * @param [in] base           Base time to compare to.
+ * @return                    True if sample time is equal to or newer than base time, else false.
+ */
+bool pbio_util_time_has_passed(uint32_t sample, uint32_t base) {
+    return sample - base < UINT32_MAX / 2;
+}