From 49e83b6ae4d2d66c5cc2a2fc87444e1d20e10a30 Mon Sep 17 00:00:00 2001 From: SalimTerryLi Date: Wed, 12 Aug 2020 06:33:44 +0800 Subject: [PATCH] load_mon: add linux support --- boards/aerotenna/ocpoc/default.cmake | 2 +- boards/beaglebone/blue/default.cmake | 2 +- boards/emlid/navio2/default.cmake | 2 +- boards/px4/raspberrypi/default.cmake | 2 +- posix-configs/bbblue/px4.config | 2 ++ posix-configs/ocpoc/px4.config | 2 ++ posix-configs/rpi/px4.config | 2 ++ posix-configs/rpi/px4_fw.config | 2 ++ posix-configs/rpi/px4_hil.config | 2 ++ posix-configs/rpi/px4_test.config | 2 ++ src/modules/load_mon/LoadMon.cpp | 35 ++++++++++++++++++++++++++++ src/modules/load_mon/LoadMon.hpp | 20 +++++++++++++--- 12 files changed, 68 insertions(+), 7 deletions(-) diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index a7baaede74..abb95fff02 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -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 diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index a502acebe9..9feafcd7bc 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -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 diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index a8ea891e17..e403485752 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -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 diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index cc5f44fd32..53ccae512d 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -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 diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 18eca98c81..27e8161272 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -47,6 +47,8 @@ param set BAT_V_DIV 11.0 dataman start +load_mon start + bmp280 -I start mpu9250 -I start diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 248fc001c7..b26e6b2ea9 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -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 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 476558de88..620b2b5971 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -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 diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 039f037175..a3140f92e9 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -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 diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 003251d18b..64998099c2 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -20,6 +20,8 @@ param set MAV_TYPE 2 dataman start +load_mon start + rc_update start sensors start -hil commander start -hil diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index a54f737397..9b149d3b94 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -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 diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index 20ded39a41..d87c61d6d3 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -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) { diff --git a/src/modules/load_mon/LoadMon.hpp b/src/modules/load_mon/LoadMon.hpp index b426461f9f..b37215b8b2 100644 --- a/src/modules/load_mon/LoadMon.hpp +++ b/src/modules/load_mon/LoadMon.hpp @@ -45,6 +45,11 @@ #include #include +#if defined(__PX4_LINUX) +#include +#include +#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_pub{ORB_ID(task_stack_info)}; - uORB::Publication _cpuload_pub{ORB_ID(cpuload)}; - - hrt_abstime _last_idle_time{0}; +#endif + uORB::Publication _cpuload_pub {ORB_ID(cpuload)}; + +#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")};