Line data    Source code

       1             : #include "Util.h"
       2             : #include <sys/time.h>
       3             : #include <AP_Param/AP_Param.h>
       4             : #include "RCOutput.h"
       5             : #include <sys/types.h>
       6             : #include <sys/stat.h>
       7             : #include <fcntl.h>
       8             : #include <AP_Common/ExpandingString.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12             : #ifdef WITH_SITL_TONEALARM
      13             : HALSITL::ToneAlarm_SF HALSITL::Util::_toneAlarm;
      14             : #endif
      15             : 
      16        2094 : uint64_t HALSITL::Util::get_hw_rtc() const
      17             : {
      18             : #ifndef CLOCK_REALTIME
      19             :     struct timeval ts;
      20             :     gettimeofday(&ts, nullptr);
      21             :     return ((long long)((ts.tv_sec * 1000000) + ts.tv_usec));
      22             : #else
      23             :     struct timespec ts;
      24        2094 :     clock_gettime(CLOCK_REALTIME, &ts);
      25        2094 :     const uint64_t seconds = ts.tv_sec;
      26        2094 :     const uint64_t nanoseconds = ts.tv_nsec;
      27        2094 :     return (seconds * 1000000ULL + nanoseconds/1000ULL);
      28             : #endif
      29             : }
      30             : 
      31             : /*
      32             :   get a (hopefully unique) machine ID
      33             :  */
      34        3070 : bool HALSITL::Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len)
      35             : {
      36        3070 :     char *cbuf = (char *)buf;
      37             : 
      38             :     // try first to use machine-id file. Most systems will have this
      39        3070 :     const char *paths[] = { "/etc/machine-id", "/var/lib/dbus/machine-id" };
      40        3070 :     for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
      41        3070 :         int fd = open(paths[i], O_RDONLY);
      42        3070 :         if (fd == -1) {
      43           0 :             continue;
      44             :         }
      45        3070 :         ssize_t ret = read(fd, buf, len);
      46        3070 :         close(fd);
      47        3070 :         if (ret <= 0) {
      48           0 :             continue;
      49             :         }
      50        3070 :         if (ret == len) {
      51          48 :             cbuf[len-1] = '\0';
      52             :         } else {
      53        3022 :             cbuf[ret] = '\0';
      54             :         }
      55        3070 :         len = ret;
      56        3070 :         char *p = strchr(cbuf, '\n');
      57        3070 :         if (p) {
      58        3022 :             *p = 0;
      59             :         }
      60        3070 :         len = strnlen(cbuf, len);
      61        3070 :         buf[0] += sitlState->get_instance();
      62        3070 :         return true;
      63             :     }
      64             : 
      65             :     // fallback to hostname
      66           0 :     if (gethostname(cbuf, len) != 0) {
      67             :         // use a default name so this always succeeds. Without it we can't
      68             :         // implement some features (such as UAVCAN)
      69           0 :         snprintf(cbuf, len, "sitl-unknown-%d", sitlState->get_instance());
      70             :     } else {
      71             :         // To ensure separate ids for each instance
      72           0 :         cbuf[0] += sitlState->get_instance();
      73             :     }
      74           0 :     len = strnlen(cbuf, len);
      75           0 :     return true;
      76             : }
      77             : 
      78             : /*
      79             :   as get_system_id_unformatted will already be ascii, we use the same
      80             :   ID here
      81             :  */
      82        3022 : bool HALSITL::Util::get_system_id(char buf[50])
      83             : {
      84        3022 :     uint8_t len = 40;
      85        3022 :     return get_system_id_unformatted((uint8_t *)buf, len);
      86             : }
      87             : 
      88             : #if !defined(HAL_BUILD_AP_PERIPH)
      89    41698406 : enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void)
      90             : {
      91             : #define HAL_USE_PWM 1
      92             : #if HAL_USE_PWM
      93    41698406 :     return ((RCOutput *)hal.rcout)->_safety_switch_state();
      94             : #else
      95             :     return SAFETY_NONE;
      96             : #endif
      97             : }
      98             : 
      99        3863 : void HALSITL::Util::set_cmdline_parameters()
     100             : {
     101        3863 :     for (uint16_t i=0; i<sitlState->cmdline_param.available(); i++) {
     102           0 :         const auto param = sitlState->cmdline_param[i];
     103           0 :         if (param != nullptr) {
     104           0 :             AP_Param::set_default_by_name(param->name, param->value);
     105             :         }
     106             :     }
     107        3863 : }
     108             : #endif
     109             : 
     110             : /**
     111             :    return commandline arguments, if available
     112             : */
     113           0 : void HALSITL::Util::commandline_arguments(uint8_t &argc, char * const *&argv)
     114             : {
     115           0 :     argc = saved_argc;
     116           0 :     argv = saved_argv;
     117           0 : }
     118             : 
     119             : /**
     120             :  * This method will read random values with set size.
     121             :  */
     122     1000000 : bool HALSITL::Util::get_random_vals(uint8_t* data, size_t size)
     123             : {
     124     1000000 :     int dev_random = open("/dev/urandom", O_RDONLY);
     125     1000000 :     if (dev_random < 0) {
     126           0 :         return false;
     127             :     }
     128     1000000 :     ssize_t result = read(dev_random, data, size);
     129     1000000 :     close(dev_random);
     130     1000000 :     return result >= 0 && (size_t)result == size;
     131             : }
     132             : 
     133             : #if HAL_UART_STATS_ENABLED
     134             : // request information on uart I/O
     135        1276 : void HALSITL::Util::uart_info(ExpandingString &str)
     136             : {
     137             :     // Calculate time since last call
     138        1276 :     const uint32_t now_ms = AP_HAL::millis();
     139        1276 :     const uint32_t dt_ms = now_ms - sys_uart_stats.last_ms;
     140        1276 :     sys_uart_stats.last_ms = now_ms;
     141             : 
     142             :     // a header to allow for machine parsers to determine format
     143        1276 :     str.printf("UARTV1\n");
     144       14036 :     for (uint8_t i = 0; i < hal.num_serial; i++) {
     145       12760 :         if (i >= ARRAY_SIZE(sitlState->_serial_path)) {
     146        1276 :             continue;
     147             :         }
     148       11484 :         auto *uart = hal.serial(i);
     149       11484 :         if (uart) {
     150       11484 :             str.printf("SERIAL%u ", i);
     151       11484 :             uart->uart_info(str, sys_uart_stats.serial[i], dt_ms);
     152             :         }
     153             :     }
     154        1276 : }
     155             : 
     156             : #if HAL_LOGGING_ENABLED
     157             : // Log UART message for each serial port
     158      149217 : void HALSITL::Util::uart_log()
     159             : {
     160             :     // Calculate time since last call
     161      149217 :     const uint32_t now_ms = AP_HAL::millis();
     162      149217 :     const uint32_t dt_ms = now_ms - log_uart_stats.last_ms;
     163      149217 :     log_uart_stats.last_ms = now_ms;
     164             : 
     165             :     // Loop over all ports
     166     1641387 :     for (uint8_t i = 0; i < hal.num_serial; i++) {
     167     1492170 :         auto *uart = hal.serial(i);
     168     1492170 :         if (uart) {
     169     1492170 :             uart->log_stats(i, log_uart_stats.serial[i], dt_ms);
     170             :         }
     171             :     }
     172      149217 : }
     173             : #endif // HAL_LOGGING_ENABLED
     174             : #endif // HAL_UART_STATS_ENABLED