diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 340cc019011..fb84d0426ac 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -216,6 +216,7 @@ if ! replay tryapplyparams then simulator start -c $simulator_tcp_port fi +load_mon start battery_simulator start tone_alarm start rc_update start diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index ea4664dcd70..dc58fac57b7 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -36,7 +36,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - #load_mon + load_mon local_position_estimator logger mavlink diff --git a/boards/px4/sitl/nolockstep.cmake b/boards/px4/sitl/nolockstep.cmake index 0f38f318d47..8c0c963b7ca 100644 --- a/boards/px4/sitl/nolockstep.cmake +++ b/boards/px4/sitl/nolockstep.cmake @@ -36,7 +36,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - #load_mon + load_mon local_position_estimator logger mavlink diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index f6ef8912670..fa97dbcf2af 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -36,7 +36,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - #load_mon + load_mon local_position_estimator logger mavlink diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index b33fed50e46..6d00a84e5ef 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -35,7 +35,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - #load_mon + load_mon local_position_estimator logger mavlink diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index d87c61d6d35..ea22b60f59a 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -86,6 +86,17 @@ void LoadMon::start() void LoadMon::Run() { +#if defined (__PX4_LINUX) + + if (_proc_fd == nullptr) { // init fd + _proc_fd = fopen("/proc/meminfo", "r"); + + if (_proc_fd == nullptr) { + PX4_ERR("Failed to open /proc/meminfo"); + } + } + +#endif perf_begin(_cycle_perf); cpuload(); @@ -100,6 +111,9 @@ void LoadMon::Run() if (should_exit()) { ScheduleClear(); +#if defined (__PX4_LINUX) + fclose(_proc_fd); +#endif exit_and_cleanup(); } @@ -142,17 +156,74 @@ void LoadMon::cpuload() const float interval_idletime = total_runtime - _last_idle_time; #endif - // get ram usage - struct mallinfo mem = mallinfo(); - float ram_usage = (float)mem.uordblks / mem.arena; - cpuload_s cpuload{}; #if defined(__PX4_LINUX) + /* following calculation is based on free(1) + * https://gitlab.com/procps-ng/procps/-/blob/master/proc/sysinfo.c */ + char line[256]; + int32_t kb_main_total = -1; + int32_t kb_main_free = -1; + int32_t kb_page_cache = -1; + int32_t kb_slab_reclaimable = -1; + int32_t kb_main_buffers = -1; + int parsedCount = 0; + + if (_proc_fd != nullptr) { + while (fgets(line, sizeof(line), _proc_fd)) { + if (sscanf(line, "MemTotal: %d kB", &kb_main_total) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "MemFree: %d kB", &kb_main_free) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "Cached: %d kB", &kb_page_cache) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "SReclaimable: %d kB", &kb_slab_reclaimable) == 1) { + ++parsedCount; + continue; + } + + if (sscanf(line, "Buffers: %d kB", &kb_main_buffers) == 1) { + ++parsedCount; + continue; + } + } + + fseek(_proc_fd, 0, SEEK_END); + + if (parsedCount == 5) { + int32_t kb_main_cached = kb_page_cache + kb_slab_reclaimable; + int32_t mem_used = kb_main_total - kb_main_free - kb_main_cached - kb_main_buffers; + + if (mem_used < 0) { + mem_used = kb_main_total - kb_main_free; + } + + cpuload.ram_usage = (float)mem_used / kb_main_total; + + } else { + PX4_ERR("Could not parse /proc/meminfo"); + cpuload.ram_usage = -1; + } + + } else { + cpuload.ram_usage = -1; + } + cpuload.load = interval_spent_time / interval; #elif defined(__PX4_NUTTX) + // get ram usage + struct mallinfo mem = mallinfo(); + cpuload.ram_usage = (float)mem.uordblks / mem.arena; cpuload.load = 1.f - interval_idletime / interval; #endif - cpuload.ram_usage = ram_usage; cpuload.timestamp = hrt_absolute_time(); _cpuload_pub.publish(cpuload); diff --git a/src/modules/load_mon/LoadMon.hpp b/src/modules/load_mon/LoadMon.hpp index d1d66153b73..afdbb23d2ba 100644 --- a/src/modules/load_mon/LoadMon.hpp +++ b/src/modules/load_mon/LoadMon.hpp @@ -33,7 +33,9 @@ #pragma once +#if defined(__PX4_NUTTX) #include +#endif #include #include #include @@ -48,7 +50,6 @@ #if defined(__PX4_LINUX) #include -#include #endif namespace load_mon @@ -92,6 +93,7 @@ private: uORB::Publication _cpuload_pub {ORB_ID(cpuload)}; #if defined(__PX4_LINUX) + FILE *_proc_fd = nullptr; /* calculate usage directly from clock ticks on Linux */ clock_t _last_total_time_stamp{}; clock_t _last_spent_time_stamp{};