yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

px4_work_queue: command line status output and shutdown empty queues

Browse Source 
* adds a work_queue systemcmd that will bring a tree view of all active work queues and work items
 * WorkQueues now track attached WorkItems and will shutdown when the last WorkItem is detached
This commit is contained in:
Daniel Agar
2019-10-02 12:23:17 -04:00
committed by GitHub
parent cc1d86bc0b
commit 26364d44c9
167 changed files with 582 additions and 274 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
.ci/Jenkinsfile-hardware
View File

@@ -291,6 +291,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// ensure buzzer is disabled for the next test
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set CBRK_BUZZER 782097"' // disable buzzer
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "param set LED_RGB_MAXBRT 0"' // disable rgbled
@@ -351,6 +352,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
// ensure buzzer is disabled for the next test
@@ -420,6 +422,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
// ensure buzzer is disabled for the next test
@@ -490,6 +493,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
// ensure buzzer is disabled for the next test
@@ -561,6 +565,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
// ensure buzzer is disabled for the next test
@@ -632,6 +637,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// critmon
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "critmon_start; sleep 5; critmon_stop"'
// run tests
@@ -709,6 +715,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// irqinfo
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "irqinfo"'
// run tests
@@ -771,6 +778,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "top once"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
// sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`' // TODO: currently unusably slow
// ensure buzzer is disabled for the next test
@@ -847,6 +855,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-SEGGER_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-SEGGER_*`'
// ensure buzzer is disabled for the next test
@@ -916,6 +925,7 @@ pipeline {
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb status"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "uorb top -1"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "ver all"'
sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-FTDI_*` --cmd "work_queue status"'
// run tests
sh './Tools/HIL/run_tests.py --device `find /dev/serial -name *usb-FTDI_*`'
// ensure buzzer is disabled for the next test

1
boards/aerotenna/ocpoc/ubuntu.cmake
View File

@@ -70,6 +70,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/airmind/mindpx-v2/default.cmake
View File

@@ -97,6 +97,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/atlflight/eagle/default.cmake
View File

@@ -111,6 +111,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/atlflight/excelsior/default.cmake
View File

@@ -111,6 +111,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/auav/esc35-v1/default.cmake
View File

@@ -55,5 +55,6 @@ px4_add_board(
param
top
ver
work_queue
)

1
boards/auav/x21/default.cmake
View File

@@ -102,6 +102,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/av/x-v1/default.cmake
View File

@@ -102,6 +102,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/beaglebone/blue/cross.cmake
View File

@@ -67,6 +67,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/beaglebone/blue/native.cmake
View File

@@ -65,6 +65,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/bitcraze/crazyflie/default.cmake
View File

@@ -66,5 +66,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

1
boards/emlid/navio2/cross.cmake
View File

@@ -74,6 +74,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/emlid/navio2/native.cmake
View File

@@ -72,6 +72,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/holybro/kakutef7/default.cmake
View File

@@ -69,5 +69,6 @@ px4_add_board(
topic_listener
usb_connected
ver
work_queue
)

1
boards/intel/aerofc-v1/default.cmake
View File

@@ -81,6 +81,7 @@ px4_add_board(
#topic_listener
tune_control
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/intel/aerofc-v1/rtps.cmake
View File

@@ -85,6 +85,7 @@ px4_add_board(
#topic_listener
tune_control
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/mro/ctrl-zero-f7/default.cmake
View File

@@ -106,6 +106,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/nxp/fmuk66-v3/default.cmake
View File

@@ -101,6 +101,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/omnibus/f4sd/default.cmake
View File

@@ -93,6 +93,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

2
boards/parrot/bebop/default.cmake
View File

@@ -59,4 +59,6 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
)

1
boards/px4/cannode-v1/default.cmake
View File

@@ -52,5 +52,6 @@ px4_add_board(
reboot
top
ver
work_queue
)

1
boards/px4/esc-v1/default.cmake
View File

@@ -55,5 +55,6 @@ px4_add_board(
param
top
ver
work_queue
)

1
boards/px4/fmu-v2/default.cmake
View File

@@ -109,6 +109,7 @@ px4_add_board(
tune_control
#usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

2
boards/px4/fmu-v2/fixedwing.cmake
View File

@@ -81,4 +81,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

1
boards/px4/fmu-v2/lpe.cmake
View File

@@ -104,6 +104,7 @@ px4_add_board(
#topic_listener
tune_control
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

2
boards/px4/fmu-v2/multicopter.cmake
View File

@@ -80,4 +80,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

2
boards/px4/fmu-v2/rover.cmake
View File

@@ -72,4 +72,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

1
boards/px4/fmu-v2/test.cmake
View File

@@ -104,6 +104,7 @@ px4_add_board(
#topic_listener
tune_control
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/px4/fmu-v3/default.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v3/rtps.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v3/stackcheck.cmake
View File

@@ -109,6 +109,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/px4/fmu-v4/default.cmake
View File

@@ -96,6 +96,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v4/rtps.cmake
View File

@@ -99,6 +99,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v4/stackcheck.cmake
View File

@@ -96,6 +96,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/px4/fmu-v4pro/default.cmake
View File

@@ -108,6 +108,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v4pro/rtps.cmake
View File

@@ -108,6 +108,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v5/critmonitor.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v5/default.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

2
boards/px4/fmu-v5/fixedwing.cmake
View File

@@ -84,4 +84,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

1
boards/px4/fmu-v5/irqmonitor.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

2
boards/px4/fmu-v5/multicopter.cmake
View File

@@ -92,4 +92,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

2
boards/px4/fmu-v5/rover.cmake
View File

@@ -85,4 +85,6 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
)

1
boards/px4/fmu-v5/rtps.cmake
View File

@@ -108,6 +108,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/fmu-v5/stackcheck.cmake
View File

@@ -109,6 +109,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

1
boards/px4/fmu-v5x/default.cmake
View File

@@ -110,6 +110,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/raspberrypi/cross.cmake
View File

@@ -66,6 +66,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/raspberrypi/native.cmake
View File

@@ -65,6 +65,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/sitl/default.cmake
View File

@@ -69,6 +69,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/sitl/rtps.cmake
View File

@@ -70,6 +70,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/px4/sitl/test.cmake
View File

@@ -69,6 +69,7 @@ px4_add_board(
topic_listener
tune_control
ver
work_queue
EXAMPLES
bottle_drop # OBC challenge

1
boards/thiemar/s2740vc-v1/default.cmake
View File

@@ -45,5 +45,6 @@ px4_add_board(
reboot
top
ver
work_queue
)

1
boards/uvify/core/default.cmake
View File

@@ -103,6 +103,7 @@ px4_add_board(
tune_control
usb_connected
ver
work_queue
EXAMPLES
#bottle_drop # OBC challenge

13
platforms/common/include/px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp
View File

@@ -44,9 +44,6 @@ class ScheduledWorkItem : public WorkItem
{
public:
ScheduledWorkItem(const wq_config_t &config) : WorkItem(config) {}
virtual ~ScheduledWorkItem() override;
/**
* Schedule next run with a delay in microseconds.
*
@@ -67,14 +64,20 @@ public:
*/
void ScheduleClear();
virtual void Run() override = 0;
protected:
ScheduledWorkItem(const char *name, const wq_config_t &config) : WorkItem(name, config) {}
virtual ~ScheduledWorkItem() override;
virtual void print_run_status() const override;
private:
virtual void Run() override = 0;
static void schedule_trampoline(void *arg);
hrt_call _call{};
};
} // namespace px4

33
platforms/common/include/px4_platform_common/px4_work_queue/WorkItem.hpp
View File

@@ -33,7 +33,6 @@
#pragma once
#include "WorkQueueManager.hpp"
#include "WorkQueue.hpp"
@@ -41,21 +40,18 @@
#include <px4_defines.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
namespace px4
{
class WorkItem : public IntrusiveQueueNode<WorkItem *>
class WorkItem : public ListNode<WorkItem *>, public IntrusiveQueueNode<WorkItem *>
{
public:
explicit WorkItem(const wq_config_t &config);
WorkItem() = delete;
virtual ~WorkItem();
inline void ScheduleNow() { if (_wq != nullptr) _wq->Add(this); }
virtual void Run() = 0;
virtual void print_run_status() const;
/**
* Switch to a different WorkQueue.
@@ -68,6 +64,18 @@ public:
protected:
explicit WorkItem(const char *name, const wq_config_t &config);
WorkItem() = delete;
virtual ~WorkItem();
protected:
void RunPreamble() { _run_count++; }
friend void WorkQueue::Run();
virtual void Run() = 0;
/**
* Initialize WorkItem given a WorkQueue config. This call
* can also be used to switch to a different WorkQueue.
@@ -79,6 +87,15 @@ protected:
bool Init(const wq_config_t &config);
void Deinit();
float elapsed_time() const;
float average_rate() const;
float average_interval() const;
hrt_abstime _start_time{0};
unsigned _run_count{0};
const char *_item_name;
private:
WorkQueue *_wq{nullptr};

10
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp
View File

@@ -35,6 +35,7 @@
#include "WorkQueueManager.hpp"
#include <containers/BlockingList.hpp>
#include <containers/List.hpp>
#include <containers/IntrusiveQueue.hpp>
#include <px4_atomic.h>
@@ -57,6 +58,9 @@ public:
const char *get_name() { return _config.name; }
bool Attach(WorkItem *item);
void Detach(WorkItem *item);
void Add(WorkItem *item);
void Remove(WorkItem *item);
@@ -66,7 +70,7 @@ public:
void request_stop() { _should_exit.store(true); }
void print_status();
void print_status(bool last = false);
private:
@@ -85,9 +89,9 @@ private:
IntrusiveQueue<WorkItem *> _q;
px4_sem_t _process_lock;
px4::atomic_bool _should_exit{false};
const wq_config_t &_config;
BlockingList<WorkItem *> _work_items;
px4::atomic_bool _should_exit{false};
};

5
platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
View File

@@ -82,6 +82,11 @@ int WorkQueueManagerStart();
*/
int WorkQueueManagerStop();
/**
* Work queue manager status.
*/
int WorkQueueManagerStatus();
/**
* Create (or find) a work queue with a particular configuration.
*

27
platforms/common/px4_work_queue/ScheduledWorkItem.cpp
View File

@@ -41,25 +41,44 @@ ScheduledWorkItem::~ScheduledWorkItem()
ScheduleClear();
}
void ScheduledWorkItem::schedule_trampoline(void *arg)
void
ScheduledWorkItem::schedule_trampoline(void *arg)
{
ScheduledWorkItem *dev = reinterpret_cast<ScheduledWorkItem *>(arg);
dev->ScheduleNow();
}
void ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us)
void
ScheduledWorkItem::ScheduleDelayed(uint32_t delay_us)
{
hrt_call_after(&_call, delay_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this);
}
void ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us)
void
ScheduledWorkItem::ScheduleOnInterval(uint32_t interval_us, uint32_t delay_us)
{
// reset start time to first deadline (approximately)
_start_time = hrt_absolute_time() + interval_us + delay_us;
hrt_call_every(&_call, delay_us, interval_us, (hrt_callout)&ScheduledWorkItem::schedule_trampoline, this);
}
void ScheduledWorkItem::ScheduleClear()
void
ScheduledWorkItem::ScheduleClear()
{
hrt_cancel(&_call);
}
void
ScheduledWorkItem::print_run_status() const
{
if (_call.period > 0) {
PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us (%" PRId64 " us)\n", _item_name, (double)average_rate(),
(double)average_interval(), _call.period);
} else {
WorkItem::print_run_status();
}
}
} // namespace px4

55
platforms/common/px4_work_queue/WorkItem.cpp
View File

@@ -42,7 +42,8 @@
namespace px4
{
WorkItem::WorkItem(const wq_config_t &config)
WorkItem::WorkItem(const char *name, const wq_config_t &config) :
_item_name(name)
{
if (!Init(config)) {
PX4_ERR("init failed");
@@ -54,25 +55,26 @@ WorkItem::~WorkItem()
Deinit();
}
bool WorkItem::Init(const wq_config_t &config)
bool
WorkItem::Init(const wq_config_t &config)
{
// clear any existing first
Deinit();
px4::WorkQueue *wq = WorkQueueFindOrCreate(config);
if (wq == nullptr) {
PX4_ERR("%s not available", config.name);
} else {
if ((wq != nullptr) && wq->Attach(this)) {
_wq = wq;
_start_time = hrt_absolute_time();
return true;
}
PX4_ERR("%s not available", config.name);
return false;
}
void WorkItem::Deinit()
void
WorkItem::Deinit()
{
// remove any currently queued work
if (_wq != nullptr) {
@@ -80,8 +82,47 @@ void WorkItem::Deinit()
px4::WorkQueue *wq_temp = _wq;
_wq = nullptr;
// remove any queued work
wq_temp->Remove(this);
wq_temp->Detach(this);
}
}
float
WorkItem::elapsed_time() const
{
return hrt_elapsed_time(&_start_time) / 1e6f;
}
float
WorkItem::average_rate() const
{
const float rate = _run_count / elapsed_time();
if (PX4_ISFINITE(rate)) {
return rate;
}
return 0.0f;
}
float
WorkItem::average_interval() const
{
const float interval = 1000000.0f / average_rate();
if (PX4_ISFINITE(interval)) {
return interval;
}
return 0.0f;
}
void
WorkItem::print_run_status() const
{
PX4_INFO_RAW("%-24s %8.1f Hz %12.1f us\n", _item_name, (double)average_rate(), (double)average_interval());
}
} // namespace px4

77
platforms/common/px4_work_queue/WorkQueue.cpp
View File

@@ -72,10 +72,44 @@ WorkQueue::~WorkQueue()
#endif /* __PX4_NUTTX */
}
void WorkQueue::Add(WorkItem *item)
bool
WorkQueue::Attach(WorkItem *item)
{
// TODO: prevent additions when shutting down
work_lock();
if (!should_exit()) {
_work_items.add(item);
work_unlock();
return true;
}
work_unlock();
return false;
}
void
WorkQueue::Detach(WorkItem *item)
{
work_lock();
_work_items.remove(item);
if (_work_items.size() == 0) {
// shutdown, no active WorkItems
PX4_DEBUG("stopping: %s, last active WorkItem closing", _config.name);
request_stop();
// Wake up the worker thread
px4_sem_post(&_process_lock);
}
work_unlock();
}
void
WorkQueue::Add(WorkItem *item)
{
work_lock();
_q.push(item);
work_unlock();
@@ -84,14 +118,16 @@ void WorkQueue::Add(WorkItem *item)
px4_sem_post(&_process_lock);
}
void WorkQueue::Remove(WorkItem *item)
void
WorkQueue::Remove(WorkItem *item)
{
work_lock();
_q.remove(item);
work_unlock();
}
void WorkQueue::Clear()
void
WorkQueue::Clear()
{
work_lock();
@@ -102,7 +138,8 @@ void WorkQueue::Clear()
work_unlock();
}
void WorkQueue::Run()
void
WorkQueue::Run()
{
while (!should_exit()) {
px4_sem_wait(&_process_lock);
@@ -114,17 +151,43 @@ void WorkQueue::Run()
WorkItem *work = _q.pop();
work_unlock(); // unlock work queue to run (item may requeue itself)
work->RunPreamble();
work->Run();
work_lock(); // re-lock
}
work_unlock();
}
PX4_DEBUG("%s: exiting", _config.name);
}
void WorkQueue::print_status()
void
WorkQueue::print_status(bool last)
{
PX4_INFO("WorkQueue: %s running", get_name());
const size_t num_items = _work_items.size();
PX4_INFO_RAW("%-16s\n", get_name());
size_t i = 0;
for (WorkItem *item : _work_items) {
i++;
if (last) {
PX4_INFO_RAW(" ");
} else {
PX4_INFO_RAW("| ");
}
if (i < num_items) {
PX4_INFO_RAW("|__ %zu) ", i);
} else {
PX4_INFO_RAW("\\__ %zu) ", i);
}
item->print_run_status();
}
}
} // namespace px4

96
platforms/common/px4_work_queue/WorkQueueManager.cpp
View File

@@ -59,10 +59,11 @@ static BlockingList<WorkQueue *> *_wq_manager_wqs_list{nullptr};
// queue of WorkQueues to be created (as threads in the wq manager task)
static BlockingQueue<const wq_config_t *, 1> *_wq_manager_create_queue{nullptr};
static px4::atomic_bool _wq_manager_should_exit{false};
static px4::atomic_bool _wq_manager_should_exit{true};
static WorkQueue *FindWorkQueueByName(const char *name)
static WorkQueue *
FindWorkQueueByName(const char *name)
{
if (_wq_manager_wqs_list == nullptr) {
PX4_ERR("not running");
@@ -81,7 +82,8 @@ static WorkQueue *FindWorkQueueByName(const char *name)
return nullptr;
}
WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq)
WorkQueue *
WorkQueueFindOrCreate(const wq_config_t &new_wq)
{
if (_wq_manager_create_queue == nullptr) {
PX4_ERR("not running");
@@ -116,7 +118,8 @@ WorkQueue *WorkQueueFindOrCreate(const wq_config_t &new_wq)
return wq;
}
const wq_config_t &device_bus_to_wq(uint32_t device_id_int)
const wq_config_t &
device_bus_to_wq(uint32_t device_id_int)
{
union device::Device::DeviceId device_id;
device_id.devid = device_id_int;
@@ -155,7 +158,8 @@ const wq_config_t &device_bus_to_wq(uint32_t device_id_int)
return wq_configurations::hp_default;
};
static void *WorkQueueRunner(void *context)
static void *
WorkQueueRunner(void *context)
{
wq_config_t *config = static_cast<wq_config_t *>(context);
WorkQueue wq(*config);
@@ -171,7 +175,8 @@ static void *WorkQueueRunner(void *context)
return nullptr;
}
static int WorkQueueManagerRun(int, char **)
static int
WorkQueueManagerRun(int, char **)
{
_wq_manager_wqs_list = new BlockingList<WorkQueue *>();
_wq_manager_create_queue = new BlockingQueue<const wq_config_t *, 1>();
@@ -232,7 +237,7 @@ static int WorkQueueManagerRun(int, char **)
int ret_create = pthread_create(&thread, &attr, WorkQueueRunner, (void *)wq);
if (ret_create == 0) {
PX4_INFO("creating: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize);
PX4_DEBUG("starting: %s, priority: %d, stack: %zu bytes", wq->name, param.sched_priority, stacksize);
} else {
PX4_ERR("failed to create thread for %s (%i): %s", wq->name, ret_create, strerror(ret_create));
@@ -250,26 +255,49 @@ static int WorkQueueManagerRun(int, char **)
return 0;
}
int WorkQueueManagerStart()
int
WorkQueueManagerStart()
{
if (_wq_manager_should_exit.load() && (_wq_manager_create_queue == nullptr)) {
_wq_manager_should_exit.store(false);
int task_id = px4_task_spawn_cmd("wq:manager",
SCHED_DEFAULT,
PX4_WQ_HP_BASE,
1200,
1280,
(px4_main_t)&WorkQueueManagerRun,
nullptr);
if (task_id < 0) {
_wq_manager_should_exit.store(true);
PX4_ERR("task start failed (%i)", task_id);
return -errno;
}
return 0;
} else {
PX4_WARN("already running");
return PX4_ERROR;
}
return PX4_OK;
}
int WorkQueueManagerStop()
int
WorkQueueManagerStop()
{
if (!_wq_manager_should_exit.load()) {
// error can't shutdown until all WorkItems are removed/stopped
if ((_wq_manager_wqs_list != nullptr) && (_wq_manager_wqs_list->size() > 0)) {
PX4_ERR("can't shutdown with active WQs");
WorkQueueManagerStatus();
return PX4_ERROR;
}
// first ask all WQs to stop
if (_wq_manager_wqs_list != nullptr) {
{
auto lg = _wq_manager_wqs_list->getLockGuard();
// ask all work queues (threads) to stop
@@ -277,6 +305,12 @@ int WorkQueueManagerStop()
for (WorkQueue *wq : *_wq_manager_wqs_list) {
wq->request_stop();
}
}
// wait until they're all stopped (empty list)
while (_wq_manager_wqs_list->size() > 0) {
px4_usleep(1000);
}
delete _wq_manager_wqs_list;
}
@@ -287,11 +321,49 @@ int WorkQueueManagerStop()
// push nullptr to wake the wq manager task
_wq_manager_create_queue->push(nullptr);
px4_usleep(1000);
px4_usleep(10000);
delete _wq_manager_create_queue;
}
} else {
PX4_WARN("not running");
return PX4_ERROR;
}
return PX4_OK;
}
int
WorkQueueManagerStatus()
{
if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) {
const size_t num_wqs = _wq_manager_wqs_list->size();
PX4_INFO_RAW("\nWork Queue: %-1zu threads RATE INTERVAL\n", num_wqs);
auto lg = _wq_manager_wqs_list->getLockGuard();
size_t i = 0;
for (WorkQueue *wq : *_wq_manager_wqs_list) {
i++;
const bool last_wq = !(i < num_wqs);
if (!last_wq) {
PX4_INFO_RAW("|__ %zu) ", i);
} else {
PX4_INFO_RAW("\\__ %zu) ", i);
}
wq->print_status(last_wq);
}
} else {
PX4_INFO("not running");
}
return PX4_OK;
}

2
platforms/common/px4_work_queue/test/wqueue_scheduled_test.h
View File

@@ -42,7 +42,7 @@ using namespace px4;
class WQueueScheduledTest : public px4::ScheduledWorkItem
{
public:
WQueueScheduledTest() : px4::ScheduledWorkItem(px4::wq_configurations::test2) {}
WQueueScheduledTest() : px4::ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test2) {}
~WQueueScheduledTest() = default;
int main();

2
platforms/common/px4_work_queue/test/wqueue_test.h
View File

@@ -42,7 +42,7 @@ using namespace px4;
class WQueueTest : public px4::WorkItem
{
public:
WQueueTest() : px4::WorkItem(px4::wq_configurations::test1) {}
WQueueTest() : px4::WorkItem("WQueueTest", px4::wq_configurations::test1) {}
~WQueueTest() = default;
int main();

2
posix-configs/rpi/px4_test.config
View File

@@ -75,6 +75,8 @@ perf
sleep 1
perf latency
sleep 1
work_queue status
sleep 1
uorb top -1
sleep 1

2
src/drivers/adc/adc.cpp
View File

@@ -104,7 +104,7 @@ private:
ADC::ADC(uint32_t base_address, uint32_t channels) :
CDev(ADC0_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_sample_perf(perf_alloc(PC_ELAPSED, "adc_samples")),
_base_address(base_address)
{

2
src/drivers/barometer/bmp280/bmp280.cpp
View File

@@ -112,7 +112,7 @@ extern "C" __EXPORT int bmp280_main(int argc, char *argv[]);
BMP280::BMP280(bmp280::IBMP280 *interface, const char *path) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_running(false),
_report_interval(0),

2
src/drivers/barometer/lps22hb/LPS22HB.cpp
View File

@@ -44,7 +44,7 @@
LPS22HB::LPS22HB(device::Device *interface, const char *path) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, "lps22hb_read")),
_comms_errors(perf_alloc(PC_COUNT, "lps22hb_comms_errors"))

2
src/drivers/barometer/lps25h/lps25h.cpp
View File

@@ -261,7 +261,7 @@ extern "C" __EXPORT int lps25h_main(int argc, char *argv[]);
LPS25H::LPS25H(device::Device *interface, const char *path) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_sample_perf(perf_alloc(PC_ELAPSED, "lps25h_read")),
_comms_errors(perf_alloc(PC_COUNT, "lps25h_comms_errors"))

2
src/drivers/barometer/mpl3115a2/mpl3115a2.cpp
View File

@@ -148,7 +148,7 @@ extern "C" __EXPORT int mpl3115a2_main(int argc, char *argv[]);
MPL3115A2::MPL3115A2(device::Device *interface, const char *path) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_measure_interval(0),
_reports(nullptr),

2
src/drivers/barometer/ms5611/ms5611.cpp
View File

@@ -183,7 +183,7 @@ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]);
MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char *path,
enum MS56XX_DEVICE_TYPES device_type) :
CDev(path),
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_prom(prom_buf.s),
_reports(nullptr),

2
src/drivers/batt_smbus/batt_smbus.cpp
View File

@@ -46,7 +46,7 @@
extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
BATT_SMBUS::BATT_SMBUS(SMBus *interface, const char *path) :
ScheduledWorkItem(px4::device_bus_to_wq(interface->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id())),
_interface(interface),
_cycle(perf_alloc(PC_ELAPSED, "batt_smbus_cycle")),
_batt_topic(nullptr),

2
src/drivers/camera_capture/camera_capture.cpp
View File

@@ -49,7 +49,7 @@ CameraCapture *g_camera_capture{nullptr};
}
CameraCapture::CameraCapture() :
ScheduledWorkItem(px4::wq_configurations::lp_default)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
memset(&_work_publisher, 0, sizeof(_work_publisher));

2
src/drivers/camera_trigger/camera_trigger.cpp
View File

@@ -233,7 +233,7 @@ CameraTrigger *g_camera_trigger;
}
CameraTrigger::CameraTrigger() :
ScheduledWorkItem(px4::wq_configurations::lp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_engagecall {},
_disengagecall {},
_engage_turn_on_off_call {},

2
src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
View File

@@ -84,7 +84,7 @@ static constexpr unsigned char crc_lsb_vector[] = {
};
CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_px4_rangefinder(0 /* TODO: device ids */, ORB_PRIO_DEFAULT, rotation)
{
// Store the port name.

2
src/drivers/distance_sensor/leddar_one/leddar_one.cpp
View File

@@ -174,7 +174,7 @@ private:
LeddarOne::LeddarOne(const char *device_path, const char *serial_port, uint8_t device_orientation):
CDev(device_path),
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_px4_rangefinder(0 /* device id not yet used */, ORB_PRIO_DEFAULT, device_orientation)
{
_serial_port = strdup(serial_port);

2
src/drivers/distance_sensor/ll40ls/LidarLite.cpp
View File

@@ -52,7 +52,6 @@ LidarLite::LidarLite(const uint8_t rotation) :
LidarLite::~LidarLite()
{
perf_free(_sample_perf);
perf_free(_sample_interval_perf);
perf_free(_comms_errors);
perf_free(_sensor_resets);
perf_free(_sensor_zero_resets);
@@ -62,7 +61,6 @@ void
LidarLite::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_sample_interval_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_sensor_resets);
perf_print_counter(_sensor_zero_resets);

1
src/drivers/distance_sensor/ll40ls/LidarLite.h
View File

@@ -90,7 +90,6 @@ protected:
PX4Rangefinder _px4_rangefinder;
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, "ll40ls: comms errors")};
perf_counter_t _sample_interval_perf{perf_alloc(PC_ELAPSED, "ll40ls: interval")};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, "ll40ls: read")};
perf_counter_t _sensor_resets{perf_alloc(PC_COUNT, "ll40ls: resets")};
perf_counter_t _sensor_zero_resets{perf_alloc(PC_COUNT, "ll40ls: zero resets")};

2
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
View File

@@ -44,7 +44,7 @@
LidarLiteI2C::LidarLiteI2C(const int bus, const uint8_t rotation, const int address) :
LidarLite(rotation),
I2C("LL40LS", nullptr, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
{
// up the retries since the device misses the first measure attempts
_retries = 3;

2
src/drivers/distance_sensor/ll40ls/LidarLitePWM.cpp
View File

@@ -46,7 +46,7 @@
LidarLitePWM::LidarLitePWM(const uint8_t rotation) :
LidarLite(rotation),
ScheduledWorkItem(px4::wq_configurations::hp_default)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
{
}

2
src/drivers/distance_sensor/mappydot/MappyDot.cpp
View File

@@ -225,7 +225,7 @@ private:
MappyDot::MappyDot(const int bus) :
I2C("MappyDot", MAPPYDOT_DEVICE_PATH, bus, MAPPYDOT_BASE_ADDR, MAPPYDOT_BUS_CLOCK),
ModuleParams(nullptr),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
{}
MappyDot::~MappyDot()

2
src/drivers/distance_sensor/mb12xx/mb12xx.cpp
View File

@@ -181,7 +181,7 @@ private:
MB12XX::MB12XX(const int bus) :
I2C("MB12xx", MB12XX_DEVICE_PATH, bus, MB12XX_BASE_ADDR, MB12XX_BUS_SPEED),
ModuleParams(nullptr),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id()))
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id()))
{
}

2
src/drivers/distance_sensor/sf0x/sf0x.cpp
View File

@@ -157,7 +157,7 @@ extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
SF0X::SF0X(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_rotation(rotation),
_min_distance(0.30f),
_max_distance(40.0f),

2
src/drivers/distance_sensor/sf1xx/sf1xx.cpp
View File

@@ -167,7 +167,7 @@ extern "C" __EXPORT int sf1xx_main(int argc, char *argv[]);
SF1XX::SF1XX(uint8_t rotation, int bus, int address) :
I2C("SF1XX", SF1XX_DEVICE_PATH, bus, address, 400000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_rotation(rotation)
{
}

2
src/drivers/distance_sensor/srf02/srf02.cpp
View File

@@ -172,7 +172,7 @@ extern "C" __EXPORT int srf02_main(int argc, char *argv[]);
SRF02::SRF02(uint8_t rotation, int bus, int address) :
I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_rotation(rotation)
{
}

2
src/drivers/distance_sensor/teraranger/teraranger.cpp
View File

@@ -192,7 +192,7 @@ static uint8_t crc8(uint8_t *p, uint8_t len)
TERARANGER::TERARANGER(const int bus, const uint8_t orientation, const int address) :
I2C("TERARANGER", TERARANGER_DEVICE_PATH, bus, address, 100000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_orientation(orientation)
{
// up the retries since the device misses the first measure attempts

2
src/drivers/distance_sensor/tfmini/TFMINI.cpp
View File

@@ -34,7 +34,7 @@
#include "TFMINI.hpp"
TFMINI::TFMINI(const char *port, uint8_t rotation) :
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_px4_rangefinder(0 /* TODO: device id */, ORB_PRIO_DEFAULT, rotation)
{
// store port name

2
src/drivers/distance_sensor/ulanding/ulanding.cpp
View File

@@ -172,7 +172,7 @@ private:
Radar::Radar(const char *port, uint8_t rotation) :
CDev(RANGE_FINDER0_DEVICE_PATH),
ScheduledWorkItem(px4::wq_configurations::hp_default),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
_rotation(rotation)
{
/* store port name */

2
src/drivers/distance_sensor/vl53lxx/vl53lxx.cpp
View File

@@ -167,7 +167,7 @@ private:
VL53LXX::VL53LXX(uint8_t rotation, int bus, int address) :
I2C("VL53LXX", VL53LXX_DEVICE_PATH, bus, address, 400000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_rotation(rotation)
{
// Allow 3 retries as the device typically misses the first measure attempts.

2
src/drivers/heater/heater.cpp
View File

@@ -51,7 +51,7 @@
Heater::Heater() :
ModuleParams(nullptr),
ScheduledWorkItem(px4::wq_configurations::lp_default)
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
{
px4_arch_configgpio(GPIO_HEATER_OUTPUT);
px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, 0);

6
src/drivers/imu/adis16448/ADIS16448.cpp
View File

@@ -39,7 +39,7 @@
ADIS16448::ADIS16448(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16448", nullptr, bus, device, SPIDEV_MODE3, 1000000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_px4_baro(get_device_id(), ORB_PRIO_MAX),
_px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
@@ -64,7 +64,6 @@ ADIS16448::~ADIS16448()
// Delete the perf counter.
perf_free(_perf_read);
perf_free(_perf_read_interval);
perf_free(_perf_transfer);
perf_free(_perf_bad_transfer);
perf_free(_perf_crc_bad);
@@ -314,7 +313,6 @@ void
ADIS16448::print_info()
{
perf_print_counter(_perf_read);
perf_print_counter(_perf_read_interval);
perf_print_counter(_perf_transfer);
perf_print_counter(_perf_bad_transfer);
perf_print_counter(_perf_crc_bad);
@@ -444,8 +442,6 @@ convert12BitToINT16(uint16_t word)
int
ADIS16448::measure()
{
perf_count(_perf_read_interval);
// Start measuring.
perf_begin(_perf_read);

1
src/drivers/imu/adis16448/ADIS16448.h
View File

@@ -211,7 +211,6 @@ private:
static constexpr float _sample_rate{ADIS16448_ACCEL_GYRO_UPDATE_RATE};
perf_counter_t _perf_read{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: read"))};
perf_counter_t _perf_read_interval{perf_alloc(PC_INTERVAL, "ADIS16448: read interval")};
perf_counter_t _perf_transfer{perf_counter_t(perf_alloc(PC_ELAPSED, "ADIS16448: transfer"))};
perf_counter_t _perf_bad_transfer{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: bad transfers"))};
perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, "ADIS16448: CRC16 bad"))};

6
src/drivers/imu/adis16477/ADIS16477.cpp
View File

@@ -55,10 +55,9 @@ using namespace time_literals;
ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16477", nullptr, bus, device, SPIDEV_MODE3, 1000000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
_sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16477: read interval")),
_sample_perf(perf_alloc(PC_ELAPSED, "adis16477: read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16477: bad transfers"))
{
@@ -82,7 +81,6 @@ ADIS16477::~ADIS16477()
stop();
// delete the perf counters
perf_free(_sample_interval_perf);
perf_free(_sample_perf);
perf_free(_bad_transfers);
}
@@ -321,7 +319,6 @@ ADIS16477::Run()
int
ADIS16477::measure()
{
perf_count(_sample_interval_perf);
perf_begin(_sample_perf);
// Fetch the full set of measurements from the ADIS16477 in one pass (burst read).
@@ -391,7 +388,6 @@ ADIS16477::measure()
void
ADIS16477::print_info()
{
perf_print_counter(_sample_interval_perf);
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);

1
src/drivers/imu/adis16477/ADIS16477.hpp
View File

@@ -65,7 +65,6 @@ private:
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
perf_counter_t _sample_interval_perf;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;

6
src/drivers/imu/adis16497/ADIS16497.cpp
View File

@@ -68,10 +68,9 @@ using namespace time_literals;
ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16497", nullptr, bus, device, SPIDEV_MODE3, 5000000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
_sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16497: read interval")),
_sample_perf(perf_alloc(PC_ELAPSED, "adis16497: read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers"))
{
@@ -95,7 +94,6 @@ ADIS16497::~ADIS16497()
stop();
// delete the perf counters
perf_free(_sample_interval_perf);
perf_free(_sample_perf);
perf_free(_bad_transfers);
}
@@ -384,7 +382,6 @@ ADIS16497::Run()
int
ADIS16497::measure()
{
perf_count(_sample_interval_perf);
perf_begin(_sample_perf);
// Fetch the full set of measurements from the ADIS16497 in one pass (burst read).
@@ -468,7 +465,6 @@ ADIS16497::measure()
void
ADIS16497::print_info()
{
perf_print_counter(_sample_interval_perf);
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);

1
src/drivers/imu/adis16497/ADIS16497.hpp
View File

@@ -101,7 +101,6 @@ private:
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
perf_counter_t _sample_interval_perf;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;

2
src/drivers/imu/bma180/bma180.cpp
View File

@@ -223,7 +223,7 @@ private:
BMA180::BMA180(int bus, uint32_t device) :
SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(this->get_device_id())),
_call_interval(0),
_reports(nullptr),
_accel_range_scale(0.0f),

7
src/drivers/imu/bmi055/BMI055_accel.cpp
View File

@@ -46,10 +46,9 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER
BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_accel_read")),
_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_accel_measure_interval")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_accel_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_accel_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi055_accel_duplicates")),
@@ -65,7 +64,6 @@ BMI055_accel::~BMI055_accel()
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_measure_interval);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
@@ -286,8 +284,6 @@ BMI055_accel::check_registers(void)
void
BMI055_accel::measure()
{
perf_count(_measure_interval);
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
@@ -409,7 +405,6 @@ void
BMI055_accel::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_measure_interval);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);

1
src/drivers/imu/bmi055/BMI055_accel.hpp
View File

@@ -169,7 +169,6 @@ private:
PX4Accelerometer _px4_accel;
perf_counter_t _sample_perf;
perf_counter_t _measure_interval;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _duplicates;

7
src/drivers/imu/bmi055/BMI055_gyro.cpp
View File

@@ -49,10 +49,9 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS]
BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) :
BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
_measure_interval(perf_alloc(PC_INTERVAL, "bmi055_gyro_measure_interval")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))
{
@@ -66,7 +65,6 @@ BMI055_gyro::~BMI055_gyro()
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_measure_interval);
perf_free(_bad_transfers);
perf_free(_bad_registers);
}
@@ -322,8 +320,6 @@ BMI055_gyro::check_registers(void)
void
BMI055_gyro::measure()
{
perf_count(_measure_interval);
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
@@ -421,7 +417,6 @@ BMI055_gyro::print_info()
PX4_INFO("Gyro");
perf_print_counter(_sample_perf);
perf_print_counter(_measure_interval);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);

1
src/drivers/imu/bmi055/BMI055_gyro.hpp
View File

@@ -161,7 +161,6 @@ private:
PX4Gyroscope _px4_gyro;
perf_counter_t _sample_perf;
perf_counter_t _measure_interval;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;

7
src/drivers/imu/bmi088/BMI088_accel.cpp
View File

@@ -54,10 +54,9 @@ const uint8_t BMI088_accel::_checked_registers[BMI088_ACCEL_NUM_CHECKED_REGISTER
BMI088_accel::BMI088_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) :
BMI088("BMI088_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI088_BUS_SPEED, rotation),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi088_accel_read")),
_measure_interval(perf_alloc(PC_INTERVAL, "bmi088_accel_measure_interval")),
_bad_transfers(perf_alloc(PC_COUNT, "bmi088_accel_bad_transfers")),
_bad_registers(perf_alloc(PC_COUNT, "bmi088_accel_bad_registers")),
_duplicates(perf_alloc(PC_COUNT, "bmi088_accel_duplicates")),
@@ -73,7 +72,6 @@ BMI088_accel::~BMI088_accel()
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_measure_interval);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_duplicates);
@@ -398,8 +396,6 @@ BMI088_accel::check_registers(void)
void
BMI088_accel::measure()
{
perf_count(_measure_interval);
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
return;
@@ -539,7 +535,6 @@ BMI088_accel::print_info()
PX4_INFO("Accel");
perf_print_counter(_sample_perf);
perf_print_counter(_measure_interval);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_duplicates);

Some files were not shown because too many files have changed in this diff Show More