forked from Archive/PX4-Autopilot
load_mon: add linux support
This commit is contained in:
parent
329ed6e273
commit
49e83b6ae4
|
@ -48,7 +48,7 @@ px4_add_board(
|
|||
fw_pos_control_l1
|
||||
land_detector
|
||||
landing_target_estimator
|
||||
#load_mon
|
||||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mavlink
|
||||
|
|
|
@ -43,7 +43,7 @@ px4_add_board(
|
|||
fw_pos_control_l1
|
||||
land_detector
|
||||
landing_target_estimator
|
||||
#load_mon
|
||||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mavlink
|
||||
|
|
|
@ -43,7 +43,7 @@ px4_add_board(
|
|||
fw_pos_control_l1
|
||||
land_detector
|
||||
landing_target_estimator
|
||||
#load_mon
|
||||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mavlink
|
||||
|
|
|
@ -42,7 +42,7 @@ px4_add_board(
|
|||
fw_pos_control_l1
|
||||
land_detector
|
||||
landing_target_estimator
|
||||
#load_mon
|
||||
load_mon
|
||||
local_position_estimator
|
||||
logger
|
||||
mavlink
|
||||
|
|
|
@ -47,6 +47,8 @@ param set BAT_V_DIV 11.0
|
|||
|
||||
dataman start
|
||||
|
||||
load_mon start
|
||||
|
||||
bmp280 -I start
|
||||
|
||||
mpu9250 -I start
|
||||
|
|
|
@ -16,6 +16,8 @@ param set MAV_BROADCAST 1
|
|||
param set MAV_TYPE 2
|
||||
param set MAV_SYS_ID 1
|
||||
|
||||
load_mon start
|
||||
|
||||
mpu9250 -s -R 4 start
|
||||
hmc5883 -I start
|
||||
ms5611 -s start
|
||||
|
|
|
@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
|
|||
|
||||
dataman start
|
||||
|
||||
load_mon start
|
||||
|
||||
mpu9250 -s -R 2 start
|
||||
lsm9ds1 -s -R 4 start
|
||||
lsm9ds1_mag -s -R 4 start
|
||||
|
|
|
@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
|
|||
|
||||
dataman start
|
||||
|
||||
load_mon start
|
||||
|
||||
mpu9250 -s -R 2 start
|
||||
lsm9ds1 -s -R 4 start
|
||||
lsm9ds1_mag -s -R 4 start
|
||||
|
|
|
@ -20,6 +20,8 @@ param set MAV_TYPE 2
|
|||
|
||||
dataman start
|
||||
|
||||
load_mon start
|
||||
|
||||
rc_update start
|
||||
sensors start -hil
|
||||
commander start -hil
|
||||
|
|
|
@ -19,6 +19,8 @@ param set IMU_GYRO_RATEMAX 400
|
|||
|
||||
dataman start
|
||||
|
||||
load_mon start
|
||||
|
||||
mpu9250 -s -R 2 start
|
||||
lsm9ds1 -s -R 4 start
|
||||
lsm9ds1_mag -s -R 4 start
|
||||
|
|
|
@ -33,6 +33,7 @@
|
|||
|
||||
#include "LoadMon.hpp"
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
// if free stack space falls below this, print a warning
|
||||
#if defined(CONFIG_ARMV7M_STACKCHECK)
|
||||
static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 100;
|
||||
|
@ -41,6 +42,7 @@ static constexpr unsigned STACK_LOW_WARNING_THRESHOLD = 300;
|
|||
#endif
|
||||
|
||||
static constexpr unsigned FDS_LOW_WARNING_THRESHOLD = 2; ///< if free file descriptors fall below this, print a warning
|
||||
#endif
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
@ -88,10 +90,14 @@ void LoadMon::Run()
|
|||
|
||||
cpuload();
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
|
||||
if (_param_sys_stck_en.get()) {
|
||||
stack_usage();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
|
@ -102,6 +108,23 @@ void LoadMon::Run()
|
|||
|
||||
void LoadMon::cpuload()
|
||||
{
|
||||
#if defined(__PX4_LINUX)
|
||||
tms spent_time_stamp_struct;
|
||||
clock_t total_time_stamp = times(&spent_time_stamp_struct);
|
||||
clock_t spent_time_stamp = spent_time_stamp_struct.tms_utime + spent_time_stamp_struct.tms_stime;
|
||||
|
||||
if (_last_total_time_stamp == 0 || _last_spent_time_stamp == 0) {
|
||||
// Just get the time in the first iteration */
|
||||
_last_total_time_stamp = total_time_stamp;
|
||||
_last_spent_time_stamp = spent_time_stamp;
|
||||
return;
|
||||
}
|
||||
|
||||
// compute system load
|
||||
const float interval = total_time_stamp - _last_total_time_stamp;
|
||||
const float interval_spent_time = spent_time_stamp - _last_spent_time_stamp;
|
||||
#elif defined(__PX4_NUTTX)
|
||||
|
||||
if (_last_idle_time == 0) {
|
||||
// Just get the time in the first iteration */
|
||||
_last_idle_time = system_load.tasks[0].total_runtime;
|
||||
|
@ -117,23 +140,34 @@ void LoadMon::cpuload()
|
|||
// compute system load
|
||||
const float interval = now - _last_idle_time_sample;
|
||||
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)
|
||||
cpuload.load = interval_spent_time / interval;
|
||||
#elif defined(__PX4_NUTTX)
|
||||
cpuload.load = 1.f - interval_idletime / interval;
|
||||
#endif
|
||||
cpuload.ram_usage = ram_usage;
|
||||
cpuload.timestamp = hrt_absolute_time();
|
||||
|
||||
_cpuload_pub.publish(cpuload);
|
||||
|
||||
// store for next iteration
|
||||
#if defined(__PX4_LINUX)
|
||||
_last_total_time_stamp = total_time_stamp;
|
||||
_last_spent_time_stamp = spent_time_stamp;
|
||||
#elif defined(__PX4_NUTTX)
|
||||
_last_idle_time = total_runtime;
|
||||
_last_idle_time_sample = now;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
void LoadMon::stack_usage()
|
||||
{
|
||||
unsigned stack_free = 0;
|
||||
|
@ -196,6 +230,7 @@ void LoadMon::stack_usage()
|
|||
// Continue after last checked task next cycle
|
||||
_stack_task_index = (_stack_task_index + 1) % CONFIG_MAX_TASKS;
|
||||
}
|
||||
#endif
|
||||
|
||||
int LoadMon::print_usage(const char *reason)
|
||||
{
|
||||
|
|
|
@ -45,6 +45,11 @@
|
|||
#include <uORB/topics/cpuload.h>
|
||||
#include <uORB/topics/task_stack_info.h>
|
||||
|
||||
#if defined(__PX4_LINUX)
|
||||
#include <sys/times.h>
|
||||
#include <malloc.h>
|
||||
#endif
|
||||
|
||||
namespace load_mon
|
||||
{
|
||||
|
||||
|
@ -74,16 +79,25 @@ private:
|
|||
/** Do a calculation of the CPU load and publish it. */
|
||||
void cpuload();
|
||||
|
||||
/* Stack check only available on Nuttx */
|
||||
#if defined(__PX4_NUTTX)
|
||||
/* Calculate stack usage */
|
||||
void stack_usage();
|
||||
|
||||
int _stack_task_index{0};
|
||||
|
||||
uORB::PublicationQueued<task_stack_info_s> _task_stack_info_pub{ORB_ID(task_stack_info)};
|
||||
uORB::Publication<cpuload_s> _cpuload_pub{ORB_ID(cpuload)};
|
||||
#endif
|
||||
uORB::Publication<cpuload_s> _cpuload_pub {ORB_ID(cpuload)};
|
||||
|
||||
hrt_abstime _last_idle_time{0};
|
||||
#if defined(__PX4_LINUX)
|
||||
/* calculate usage directly from clock ticks on Linux */
|
||||
clock_t _last_total_time_stamp{};
|
||||
clock_t _last_spent_time_stamp{};
|
||||
#elif defined(__PX4_NUTTX)
|
||||
hrt_abstime _last_idle_time {0};
|
||||
hrt_abstime _last_idle_time_sample{0};
|
||||
#endif
|
||||
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
|
||||
|
|
Loading…
Reference in New Issue