2018-04-24 09:25:25 -03:00
|
|
|
#include "Util.h"
|
2018-10-14 01:23:17 -03:00
|
|
|
#include <sys/time.h>
|
2020-08-23 03:24:22 -03:00
|
|
|
#include <AP_Param/AP_Param.h>
|
2023-07-07 00:27:26 -03:00
|
|
|
#include "RCOutput.h"
|
2023-12-12 04:04:09 -04:00
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/stat.h>
|
|
|
|
#include <fcntl.h>
|
2024-03-15 11:25:18 -03:00
|
|
|
#include <AP_Common/ExpandingString.h>
|
2023-07-07 00:27:26 -03:00
|
|
|
|
|
|
|
extern const AP_HAL::HAL& hal;
|
2018-04-24 09:25:25 -03:00
|
|
|
|
2019-03-19 23:19:41 -03:00
|
|
|
#ifdef WITH_SITL_TONEALARM
|
|
|
|
HALSITL::ToneAlarm_SF HALSITL::Util::_toneAlarm;
|
|
|
|
#endif
|
|
|
|
|
2018-04-24 09:25:25 -03:00
|
|
|
uint64_t HALSITL::Util::get_hw_rtc() const
|
|
|
|
{
|
2018-10-14 01:23:17 -03:00
|
|
|
#ifndef CLOCK_REALTIME
|
|
|
|
struct timeval ts;
|
|
|
|
gettimeofday(&ts, nullptr);
|
|
|
|
return ((long long)((ts.tv_sec * 1000000) + ts.tv_usec));
|
|
|
|
#else
|
2018-04-24 09:25:25 -03:00
|
|
|
struct timespec ts;
|
|
|
|
clock_gettime(CLOCK_REALTIME, &ts);
|
|
|
|
const uint64_t seconds = ts.tv_sec;
|
|
|
|
const uint64_t nanoseconds = ts.tv_nsec;
|
|
|
|
return (seconds * 1000000ULL + nanoseconds/1000ULL);
|
2018-10-14 01:23:17 -03:00
|
|
|
#endif
|
2018-04-24 09:25:25 -03:00
|
|
|
}
|
2018-11-16 17:32:38 -04:00
|
|
|
|
|
|
|
/*
|
|
|
|
get a (hopefully unique) machine ID
|
|
|
|
*/
|
|
|
|
bool HALSITL::Util::get_system_id_unformatted(uint8_t buf[], uint8_t &len)
|
|
|
|
{
|
|
|
|
char *cbuf = (char *)buf;
|
|
|
|
|
|
|
|
// try first to use machine-id file. Most systems will have this
|
|
|
|
const char *paths[] = { "/etc/machine-id", "/var/lib/dbus/machine-id" };
|
|
|
|
for (uint8_t i=0; i<ARRAY_SIZE(paths); i++) {
|
|
|
|
int fd = open(paths[i], O_RDONLY);
|
|
|
|
if (fd == -1) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
ssize_t ret = read(fd, buf, len);
|
|
|
|
close(fd);
|
|
|
|
if (ret <= 0) {
|
|
|
|
continue;
|
|
|
|
}
|
2020-04-03 08:20:02 -03:00
|
|
|
if (ret == len) {
|
|
|
|
cbuf[len-1] = '\0';
|
|
|
|
} else {
|
|
|
|
cbuf[ret] = '\0';
|
|
|
|
}
|
2018-11-16 17:32:38 -04:00
|
|
|
len = ret;
|
|
|
|
char *p = strchr(cbuf, '\n');
|
|
|
|
if (p) {
|
|
|
|
*p = 0;
|
|
|
|
}
|
|
|
|
len = strnlen(cbuf, len);
|
2020-12-28 10:33:56 -04:00
|
|
|
buf[0] += sitlState->get_instance();
|
2018-11-16 17:32:38 -04:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// fallback to hostname
|
|
|
|
if (gethostname(cbuf, len) != 0) {
|
|
|
|
// use a default name so this always succeeds. Without it we can't
|
|
|
|
// implement some features (such as UAVCAN)
|
2020-12-28 10:33:56 -04:00
|
|
|
snprintf(cbuf, len, "sitl-unknown-%d", sitlState->get_instance());
|
|
|
|
} else {
|
|
|
|
// To ensure separate ids for each instance
|
|
|
|
cbuf[0] += sitlState->get_instance();
|
2018-11-16 17:32:38 -04:00
|
|
|
}
|
|
|
|
len = strnlen(cbuf, len);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
as get_system_id_unformatted will already be ascii, we use the same
|
|
|
|
ID here
|
|
|
|
*/
|
2022-09-18 14:04:23 -03:00
|
|
|
bool HALSITL::Util::get_system_id(char buf[50])
|
2018-11-16 17:32:38 -04:00
|
|
|
{
|
|
|
|
uint8_t len = 40;
|
|
|
|
return get_system_id_unformatted((uint8_t *)buf, len);
|
|
|
|
}
|
2018-12-08 22:34:54 -04:00
|
|
|
|
|
|
|
#ifdef ENABLE_HEAP
|
|
|
|
void *HALSITL::Util::allocate_heap_memory(size_t size)
|
|
|
|
{
|
|
|
|
struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
|
|
|
|
if (new_heap != nullptr) {
|
|
|
|
new_heap->scripting_max_heap_size = size;
|
|
|
|
new_heap->current_heap_usage = 0;
|
|
|
|
}
|
|
|
|
return (void *)new_heap;
|
|
|
|
}
|
|
|
|
|
2022-12-22 18:50:37 -04:00
|
|
|
void *HALSITL::Util::heap_realloc(void *heap_ptr, void *ptr, size_t old_size, size_t new_size)
|
2018-12-08 22:34:54 -04:00
|
|
|
{
|
2019-04-08 09:03:45 -03:00
|
|
|
if (heap_ptr == nullptr) {
|
2018-12-08 22:34:54 -04:00
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
2019-04-08 09:03:45 -03:00
|
|
|
struct heap *heapp = (struct heap*)heap_ptr;
|
2018-12-08 22:34:54 -04:00
|
|
|
|
|
|
|
// extract appropriate headers
|
2022-12-22 18:50:37 -04:00
|
|
|
size_t old_size_header = 0;
|
2018-12-08 22:34:54 -04:00
|
|
|
heap_allocation_header *old_header = nullptr;
|
|
|
|
if (ptr != nullptr) {
|
|
|
|
old_header = ((heap_allocation_header *)ptr) - 1;
|
2022-12-22 18:50:37 -04:00
|
|
|
old_size_header = old_header->allocation_size;
|
|
|
|
#if !defined(HAL_BUILD_AP_PERIPH)
|
|
|
|
if (old_size_header != old_size && new_size != 0) {
|
|
|
|
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
|
|
|
|
}
|
|
|
|
#endif
|
2018-12-08 22:34:54 -04:00
|
|
|
}
|
|
|
|
|
2019-08-26 21:22:11 -03:00
|
|
|
if ((heapp->current_heap_usage + new_size - old_size) > heapp->scripting_max_heap_size) {
|
|
|
|
// fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
2022-12-22 18:50:37 -04:00
|
|
|
heapp->current_heap_usage -= old_size_header;
|
2018-12-08 22:34:54 -04:00
|
|
|
if (new_size == 0) {
|
|
|
|
free(old_header);
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
|
|
|
|
heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
|
|
|
|
if (new_header == nullptr) {
|
|
|
|
// total failure to allocate, this is very surprising in SITL
|
|
|
|
return nullptr;
|
|
|
|
}
|
|
|
|
heapp->current_heap_usage += new_size;
|
|
|
|
new_header->allocation_size = new_size;
|
|
|
|
void *new_mem = new_header + 1;
|
|
|
|
|
|
|
|
if (ptr == nullptr) {
|
|
|
|
return new_mem;
|
|
|
|
}
|
|
|
|
memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
|
|
|
|
free(old_header);
|
|
|
|
return new_mem;
|
|
|
|
}
|
|
|
|
|
|
|
|
#endif // ENABLE_HEAP
|
2019-11-25 03:02:11 -04:00
|
|
|
|
2020-09-12 16:04:38 -03:00
|
|
|
#if !defined(HAL_BUILD_AP_PERIPH)
|
2019-11-25 03:02:11 -04:00
|
|
|
enum AP_HAL::Util::safety_state HALSITL::Util::safety_switch_state(void)
|
|
|
|
{
|
2023-07-07 00:27:26 -03:00
|
|
|
#define HAL_USE_PWM 1
|
|
|
|
#if HAL_USE_PWM
|
|
|
|
return ((RCOutput *)hal.rcout)->_safety_switch_state();
|
|
|
|
#else
|
|
|
|
return SAFETY_NONE;
|
|
|
|
#endif
|
2019-11-25 03:02:11 -04:00
|
|
|
}
|
2020-08-23 03:24:22 -03:00
|
|
|
|
|
|
|
void HALSITL::Util::set_cmdline_parameters()
|
|
|
|
{
|
|
|
|
for (auto param: sitlState->cmdline_param) {
|
|
|
|
AP_Param::set_default_by_name(param.name, param.value);
|
|
|
|
}
|
|
|
|
}
|
2020-11-08 22:04:44 -04:00
|
|
|
#endif
|
2020-11-08 17:48:25 -04:00
|
|
|
|
|
|
|
/**
|
|
|
|
return commandline arguments, if available
|
|
|
|
*/
|
|
|
|
void HALSITL::Util::commandline_arguments(uint8_t &argc, char * const *&argv)
|
|
|
|
{
|
|
|
|
argc = saved_argc;
|
|
|
|
argv = saved_argv;
|
|
|
|
}
|
2020-11-08 22:04:44 -04:00
|
|
|
|
2021-06-24 13:50:23 -03:00
|
|
|
/**
|
|
|
|
* This method will read random values with set size.
|
|
|
|
*/
|
|
|
|
bool HALSITL::Util::get_random_vals(uint8_t* data, size_t size)
|
|
|
|
{
|
|
|
|
int dev_random = open("/dev/urandom", O_RDONLY);
|
|
|
|
if (dev_random < 0) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
ssize_t result = read(dev_random, data, size);
|
|
|
|
if (result < 0) {
|
|
|
|
close(dev_random);
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
close(dev_random);
|
|
|
|
return true;
|
|
|
|
}
|
2024-03-15 11:25:18 -03:00
|
|
|
|
|
|
|
#if HAL_UART_STATS_ENABLED
|
|
|
|
// request information on uart I/O
|
|
|
|
void HALSITL::Util::uart_info(ExpandingString &str)
|
|
|
|
{
|
|
|
|
// Calculate time since last call
|
|
|
|
const uint32_t now_ms = AP_HAL::millis();
|
|
|
|
const uint32_t dt_ms = now_ms - uart_stats.last_ms;
|
|
|
|
uart_stats.last_ms = now_ms;
|
|
|
|
|
|
|
|
// a header to allow for machine parsers to determine format
|
|
|
|
str.printf("UARTV1\n");
|
|
|
|
for (uint8_t i = 0; i < hal.num_serial; i++) {
|
|
|
|
if (i >= ARRAY_SIZE(sitlState->_serial_path)) {
|
|
|
|
continue;
|
|
|
|
}
|
|
|
|
auto *uart = hal.serial(i);
|
|
|
|
if (uart) {
|
|
|
|
str.printf("SERIAL%u ", i);
|
|
|
|
uart->uart_info(str, uart_stats.serial[i], dt_ms);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|