Line data    Source code

       1             : #include <stdarg.h>
       2             : #include <stdio.h>
       3             : #include <sys/time.h>
       4             : #include <unistd.h>
       5             : 
       6             : #include <AP_Common/AP_Common.h>
       7             : #include <AP_HAL/AP_HAL.h>
       8             : #include <AP_HAL/system.h>
       9             : #include <AP_HAL_Linux/Scheduler.h>
      10             : #include <AP_Math/div1000.h>
      11             : 
      12             : extern const AP_HAL::HAL& hal;
      13             : 
      14             : namespace AP_HAL {
      15             : 
      16             : static struct {
      17             :     uint64_t start_time_ns;
      18             : } state;
      19             : 
      20        2357 : static uint64_t ts_to_nsec(struct timespec &ts)
      21             : {
      22        2357 :     return ts.tv_sec*1000000000ULL + ts.tv_nsec;
      23             : }
      24             : 
      25          48 : void init()
      26             : {
      27          48 :     struct timespec ts {};
      28          48 :     clock_gettime(CLOCK_MONOTONIC, &ts);
      29          48 :     state.start_time_ns = ts_to_nsec(ts);
      30          48 : }
      31             : 
      32           0 : void WEAK panic(const char *errormsg, ...)
      33             : {
      34             :     va_list ap;
      35             : 
      36           0 :     va_start(ap, errormsg);
      37           0 :     vdprintf(1, errormsg, ap);
      38           0 :     va_end(ap);
      39           0 :     UNUSED_RESULT(write(1, "\n", 1));
      40             : 
      41           0 :     if (hal.rcin != nullptr) {
      42           0 :         hal.rcin->teardown();
      43             :     }
      44           0 :     if (hal.scheduler != nullptr) {
      45           0 :         hal.scheduler->delay_microseconds(10000);
      46             :     }
      47           0 :     exit(1);
      48             : }
      49             : 
      50          19 : uint32_t micros()
      51             : {
      52          19 :     return micros64() & 0xFFFFFFFF;
      53             : }
      54             : 
      55          22 : uint32_t millis()
      56             : {
      57          22 :     return millis64() & 0xFFFFFFFF;
      58             : }
      59             : 
      60        2328 : uint64_t micros64()
      61             : {
      62        2328 :     const Linux::Scheduler* scheduler = Linux::Scheduler::from(hal.scheduler);
      63        2328 :     uint64_t stopped_usec = scheduler->stopped_clock_usec();
      64        2328 :     if (stopped_usec) {
      65          19 :         return stopped_usec;
      66             :     }
      67             : 
      68             :     struct timespec ts;
      69        2309 :     clock_gettime(CLOCK_MONOTONIC, &ts);
      70        2309 :     return uint64_div1000(ts_to_nsec(ts) - state.start_time_ns);
      71             : }
      72             : 
      73          22 : uint64_t millis64()
      74             : {
      75          22 :     return uint64_div1000(micros64());
      76             : }
      77             : 
      78             : } // namespace AP_HAL