Line data    Source code

       1             : #include "DataRateLimit.h"
       2             : 
       3             : #include <AP_HAL/AP_HAL.h>
       4             : 
       5             : // Return the max number of bytes that can be sent since the last call given byte/s rate limit
       6          38 : uint32_t DataRateLimit::max_bytes(const float bytes_per_sec)
       7             : {
       8             :     // Time since last call
       9          38 :     const uint32_t now_us = AP_HAL::micros();
      10          38 :     const float dt = (now_us - last_us) * 1.0e-6;
      11          38 :     last_us = now_us;
      12             : 
      13             :     // Maximum number of bytes that could be transferred in that time
      14          38 :     float max_bytes = bytes_per_sec * dt;
      15             : 
      16             :     // Add on the remainder from the last call, this prevents cumulative rounding errors
      17          38 :     max_bytes += remainder;
      18             : 
      19             :     // Get integer number of bytes and store the remainder
      20             :     float max_bytes_int;
      21          38 :     remainder = modf(max_bytes, &max_bytes_int);
      22             : 
      23             :     // Add 0.5 to make sure the float rounds to the correct int
      24          38 :     return uint32_t(max_bytes_int + 0.5);
      25             : }