PX4 Firmware
PX4 Autopilot Software http://px4.io
load_mon.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file load_mon.cpp
36  *
37  * @author Jonathan Challinger <jonathan@3drobotics.com>
38  * @author Julian Oes <julian@oes.ch
39  * @author Andreas Antener <andreas@uaventure.com>
40  */
41 
42 #include <drivers/drv_hrt.h>
43 #include <lib/perf/perf_counter.h>
44 #include <px4_platform_common/px4_config.h>
45 #include <px4_platform_common/defines.h>
46 #include <px4_platform_common/module.h>
47 #include <px4_platform_common/module_params.h>
48 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
49 #include <systemlib/cpuload.h>
50 #include <uORB/Publication.hpp>
52 #include <uORB/topics/cpuload.h>
54 
55 #if defined(__PX4_NUTTX) && !defined(CONFIG_SCHED_INSTRUMENTATION)
56 # error load_mon support requires CONFIG_SCHED_INSTRUMENTATION
57 #endif
58 
59 #define STACK_LOW_WARNING_THRESHOLD 300 ///< if free stack space falls below this, print a warning
60 #define FDS_LOW_WARNING_THRESHOLD 3 ///< if free file descriptors fall below this, print a warning
61 
62 namespace load_mon
63 {
64 
65 extern "C" __EXPORT int load_mon_main(int argc, char *argv[]);
66 
67 // Run it at 1 Hz.
68 const unsigned LOAD_MON_INTERVAL_US = 1000000;
69 
70 class LoadMon : public ModuleBase<LoadMon>, public ModuleParams, public px4::ScheduledWorkItem
71 {
72 public:
73  LoadMon();
74  ~LoadMon() override;
75 
76  static int task_spawn(int argc, char *argv[]);
77 
78  /** @see ModuleBase */
79  static int custom_command(int argc, char *argv[])
80  {
81  return print_usage("unknown command");
82  }
83 
84  /** @see ModuleBase */
85  static int print_usage(const char *reason = nullptr);
86 
87  /** @see ModuleBase::print_status() */
88  int print_status() override;
89 
90  void start();
91 
92 private:
93  /** Do a compute and schedule the next cycle. */
94  void Run() override;
95 
96  /** Do a calculation of the CPU load and publish it. */
97  void _cpuload();
98 
99  /** Calculate the memory usage */
100  float _ram_used();
101 
102 #ifdef __PX4_NUTTX
103  /* Calculate stack usage */
104  void _stack_usage();
105 
106  int _stack_task_index{0};
107  uORB::PublicationQueued<task_stack_info_s> _task_stack_info_pub{ORB_ID(task_stack_info)};
108 #endif
109 
110  DEFINE_PARAMETERS(
111  (ParamBool<px4::params::SYS_STCK_EN>) _param_sys_stck_en
112  )
113 
115 
118 
120 };
121 
123  ModuleParams(nullptr),
124  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
125  _stack_perf(perf_alloc(PC_ELAPSED, "stack_check"))
126 {
127 }
128 
130 {
131  ScheduleClear();
132 
134 }
135 
136 int LoadMon::task_spawn(int argc, char *argv[])
137 {
138  LoadMon *obj = new LoadMon();
139 
140  if (!obj) {
141  PX4_ERR("alloc failed");
142  return -1;
143  }
144 
145  _object.store(obj);
146  _task_id = task_id_is_work_queue;
147 
148  /* Schedule a cycle to start things. */
149  obj->start();
150 
151  return 0;
152 }
153 
154 void
156 {
157  ScheduleOnInterval(LOAD_MON_INTERVAL_US);
158 }
159 
161 {
162  _cpuload();
163 
164 #ifdef __PX4_NUTTX
165 
166  if (_param_sys_stck_en.get()) {
167  _stack_usage();
168  }
169 
170 #endif
171 
172  if (should_exit()) {
173  ScheduleClear();
174  exit_and_cleanup();
175  }
176 }
177 
179 {
180  if (_last_idle_time == 0) {
181  /* Just get the time in the first iteration */
182  _last_idle_time = system_load.tasks[0].total_runtime;
183  return;
184  }
185 
186  /* compute system load */
187  const hrt_abstime total_runtime = system_load.tasks[0].total_runtime;
189  const hrt_abstime interval_idletime = total_runtime - _last_idle_time;
190 
191  _last_idle_time = total_runtime;
193 
194  cpuload_s cpuload{};
195  cpuload.load = 1.0f - (float)interval_idletime / (float)interval;
198 
199  _cpuload_pub.publish(cpuload);
200 }
201 
203 {
204 #ifdef __PX4_NUTTX
205  struct mallinfo mem;
206 
207 #ifdef CONFIG_CAN_PASS_STRUCTS
208  mem = mallinfo();
209 #else
210  (void)mallinfo(&mem);
211 #endif /* CONFIG_CAN_PASS_STRUCTS */
212 
213  // mem.arena: total ram (bytes)
214  // mem.uordblks: used (bytes)
215  // mem.fordblks: free (bytes)
216  // mem.mxordblk: largest remaining block (bytes)
217 
218  return (float)mem.uordblks / mem.arena;
219 
220 #else
221  return 0.0f;
222 #endif
223 }
224 
225 #ifdef __PX4_NUTTX
226 void LoadMon::_stack_usage()
227 {
228  int task_index = 0;
229 
230  /* Scan maximum num_tasks_per_cycle tasks to reduce load. */
231  const int num_tasks_per_cycle = 2;
232 
233  for (int i = _stack_task_index; i < _stack_task_index + num_tasks_per_cycle; i++) {
234  task_index = i % CONFIG_MAX_TASKS;
235  unsigned stack_free = 0;
236  unsigned fds_free = FDS_LOW_WARNING_THRESHOLD + 1;
237  bool checked_task = false;
238 
240  sched_lock();
241 
242  task_stack_info_s task_stack_info = {};
243 
244  if (system_load.tasks[task_index].valid && (system_load.tasks[task_index].tcb->pid > 0)) {
245 
246  stack_free = up_check_tcbstack_remain(system_load.tasks[task_index].tcb);
247 
248  static_assert(sizeof(task_stack_info.task_name) == CONFIG_TASK_NAME_SIZE,
249  "task_stack_info.task_name must match NuttX CONFIG_TASK_NAME_SIZE");
250  strncpy((char *)task_stack_info.task_name, system_load.tasks[task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1);
251  task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0';
252 
253 #if CONFIG_NFILE_DESCRIPTORS > 0
254  FAR struct task_group_s *group = system_load.tasks[task_index].tcb->group;
255 
256  unsigned tcb_num_used_fds = 0;
257 
258  if (group) {
259  for (int fd_index = 0; fd_index < CONFIG_NFILE_DESCRIPTORS; ++fd_index) {
260  if (group->tg_filelist.fl_files[fd_index].f_inode) {
261  ++tcb_num_used_fds;
262  }
263  }
264 
265  fds_free = CONFIG_NFILE_DESCRIPTORS - tcb_num_used_fds;
266  }
267 
268 #endif // CONFIG_NFILE_DESCRIPTORS
269 
270  checked_task = true;
271  }
272 
273  sched_unlock();
275 
276  if (checked_task) {
277 
278  task_stack_info.stack_free = stack_free;
279  task_stack_info.timestamp = hrt_absolute_time();
280 
281  _task_stack_info_pub.publish(task_stack_info);
282 
283  /*
284  * Found task low on stack, report and exit. Continue here in next cycle.
285  */
286  if (stack_free < STACK_LOW_WARNING_THRESHOLD) {
287  PX4_WARN("%s low on stack! (%i bytes left)", task_stack_info.task_name, stack_free);
288  break;
289  }
290 
291  /*
292  * Found task low on file descriptors, report and exit. Continue here in next cycle.
293  */
294  if (fds_free < FDS_LOW_WARNING_THRESHOLD) {
295  PX4_WARN("%s low on FDs! (%i FDs left)", task_stack_info.task_name, fds_free);
296  break;
297  }
298 
299  } else {
300  /* No task here, check one more task in same cycle. */
301  _stack_task_index++;
302  }
303  }
304 
305  /* Continue after last checked task next cycle. */
306  _stack_task_index = task_index + 1;
307 }
308 #endif
309 
311 {
312  PX4_INFO("running");
314  return 0;
315 }
316 
317 int LoadMon::print_usage(const char *reason)
318 {
319  if (reason) {
320  PX4_ERR("%s\n", reason);
321  }
322 
323  PRINT_MODULE_DESCRIPTION(
324  R"DESCR_STR(
325 ### Description
326 Background process running periodically with 1 Hz on the LP work queue to calculate the CPU load and RAM
327 usage and publish the `cpuload` topic.
328 
329 On NuttX it also checks the stack usage of each process and if it falls below 300 bytes, a warning is output,
330 which will also appear in the log file.
331 )DESCR_STR");
332 
333  PRINT_MODULE_USAGE_NAME("load_mon", "system");
334  PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
335  PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
336  return 0;
337 }
338 
339 
340 int load_mon_main(int argc, char *argv[])
341 {
342  return LoadMon::main(argc, argv);
343 }
344 
345 } // namespace load_mon
Queued publication with queue length set as a message constant (ORB_QUEUE_LENGTH) ...
measure the time elapsed performing an event
Definition: perf_counter.h:56
uint64_t timestamp
Definition: cpuload.h:53
~LoadMon() override
Definition: load_mon.cpp:129
int main(int argc, char **argv)
Definition: main.cpp:3
const unsigned LOAD_MON_INTERVAL_US
Definition: load_mon.cpp:68
Definition: I2C.hpp:51
void Run() override
Do a compute and schedule the next cycle.
Definition: load_mon.cpp:160
High-resolution timer with callouts and timekeeping.
float load
Definition: cpuload.h:54
int print_status() override
Definition: load_mon.cpp:310
Base publication wrapper class.
Definition: Publication.hpp:52
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
static int custom_command(int argc, char *argv[])
Definition: load_mon.cpp:79
Header common to all counters.
DEFINE_PARAMETERS((ParamBool< px4::params::SYS_STCK_EN >) _param_sys_stck_en) uORB hrt_abstime _last_idle_time
Definition: load_mon.cpp:116
void perf_free(perf_counter_t handle)
Free a counter.
hrt_abstime _last_idle_time_sample
Definition: load_mon.cpp:117
static int task_spawn(int argc, char *argv[])
Definition: load_mon.cpp:136
#define perf_alloc(a, b)
Definition: px4io.h:59
#define CONFIG_MAX_TASKS
Definition: printload.h:49
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
void perf_end(perf_counter_t handle)
End a performance event.
float _ram_used()
Calculate the memory usage.
Definition: load_mon.cpp:202
__EXPORT int load_mon_main(int argc, char *argv[])
Definition: load_mon.cpp:340
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
#define FDS_LOW_WARNING_THRESHOLD
if free file descriptors fall below this, print a warning
Definition: load_mon.cpp:60
perf_counter_t _stack_perf
Definition: load_mon.cpp:119
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
static struct cpuload_s cpuload
Definition: Commander.cpp:151
Definition: bst.cpp:62
void _cpuload()
Do a calculation of the CPU load and publish it.
Definition: load_mon.cpp:178
static int print_usage(const char *reason=nullptr)
Definition: load_mon.cpp:317
float ram_usage
Definition: cpuload.h:55
#define STACK_LOW_WARNING_THRESHOLD
if free stack space falls below this, print a warning
Definition: load_mon.cpp:59
void perf_begin(perf_counter_t handle)
Begin a performance event.
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
Performance measuring tools.